CMakeLists.txt

@@ -9,29 +9,7 @@ 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 <source>/output
 set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/output" CACHE PATH "" FORCE)
@@ -40,6 +18,9 @@ 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)
 
@@ -50,9 +31,6 @@ 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}")

README.md

@@ -138,120 +138,3 @@ $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 接口开放。

cmake/FindExternalLib.cmake

@@ -55,17 +55,7 @@ function(setup_external_libs ARCH)
 
         # ---- library dirs ----
         if(EXISTS "${FULL_PATH}/lib")
-            file(GLOB _BUNDLED_LIBSTDCXX_FILES
+            list(APPEND LIBRARY_DIRS "${FULL_PATH}/lib")
-                    "${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.*
@@ -139,7 +129,6 @@ 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 <prefix>/lib ----
     if(INSTALL_SO_FILES)

cmvr-es/CMakeLists.txt

@@ -6,14 +6,11 @@ add_subdirectory(hardware)
 add_subdirectory(algorithms)
 add_subdirectory(simulate)
 add_subdirectory(devices)
-add_subdirectory(manager/control_authority)
 add_subdirectory(manager/device_manager)
 add_subdirectory(manager/media_source_hub)
 add_subdirectory(service/quic_edge)
-add_subdirectory(service/arm_teleop_client)
 add_subdirectory(task)
 add_subdirectory(task/quic_edge_task)
-add_subdirectory(task/ume_teleop_task)
 add_subdirectory(manager/task_manager)
 add_subdirectory(service)
 add_subdirectory(runtime)

cmvr-es/algorithms/controllers/CMakeLists.txt

@@ -1,6 +1,5 @@
 
 add_subdirectory(arm_control)
-add_subdirectory(ume_legacy)
 
 #find_package(VISP REQUIRED)
 

cmvr-es/algorithms/controllers/ume_legacy/CMakeLists.txt

@@ -1,78 +0,0 @@
-add_library(ume_legacy_controller SHARED
-        src/ume_legacy_controller.cpp
-        src/pinocchio_ume_legacy_model_adapter.cpp
-)
-
-target_include_directories(ume_legacy_controller
-        PUBLIC
-        ${CMAKE_CURRENT_SOURCE_DIR}/include
-)
-
-target_link_libraries(ume_legacy_controller
-        PRIVATE
-        pinocchio_default
-        pinocchio_parsers
-)
-
-add_library(
-        cmvr_es::algorithms::ume_legacy
-        ALIAS ume_legacy_controller
-)
-
-install(TARGETS ume_legacy_controller LIBRARY DESTINATION lib)
-
-if(BUILD_TESTING)
-    add_executable(ume_legacy_controller_golden_test
-            tests/ume_legacy_controller_golden_test.cpp
-    )
-    add_executable(pinocchio_ume_legacy_model_adapter_test
-            tests/pinocchio_ume_legacy_model_adapter_test.cpp
-    )
-
-    target_link_libraries(ume_legacy_controller_golden_test
-            PRIVATE
-            cmvr_es::algorithms::ume_legacy
-            gtest
-            gtest_main
-            pthread
-    )
-    target_link_libraries(pinocchio_ume_legacy_model_adapter_test
-            PRIVATE
-            cmvr_es::algorithms::ume_legacy
-            gtest
-            gtest_main
-            pthread
-    )
-
-    foreach(_ume_legacy_test_target
-            ume_legacy_controller_golden_test
-            pinocchio_ume_legacy_model_adapter_test)
-        target_compile_definitions(${_ume_legacy_test_target}
-                PRIVATE
-                CMVR_UME_FIXED_MODEL_PATH="${CMAKE_SOURCE_DIR}/model/ume/v6_bimanual/robot.xml"
-                CMVR_UME_FLOATING_MODEL_PATH="${CMAKE_SOURCE_DIR}/model/ume/v6_imu/robot.xml"
-        )
-        add_test(
-                NAME ${_ume_legacy_test_target}
-                COMMAND ${_ume_legacy_test_target}
-        )
-    endforeach()
-
-    set(_ume_legacy_test_environment
-            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
-    )
-
-    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
-        list(APPEND _ume_legacy_test_environment
-                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
-    endif()
-
-    foreach(_ume_legacy_test_target
-            ume_legacy_controller_golden_test
-            pinocchio_ume_legacy_model_adapter_test)
-        set_tests_properties(${_ume_legacy_test_target} PROPERTIES
-                TIMEOUT 10
-                ENVIRONMENT "${_ume_legacy_test_environment}"
-        )
-    endforeach()
-endif()

cmvr-es/algorithms/controllers/ume_legacy/include/pinocchio_ume_legacy_model_adapter.h

@@ -1,72 +0,0 @@
-#ifndef CMVR_ES_PINOCCHIO_UME_LEGACY_MODEL_ADAPTER_H
-#define CMVR_ES_PINOCCHIO_UME_LEGACY_MODEL_ADAPTER_H
-
-#include <cstddef>
-#include <memory>
-#include <string>
-
-#include "ume_legacy_model_adapter.h"
-
-namespace cmvr::ume_legacy {
-
-struct UmeLegacyModelContract {
-    std::size_t fixed_nq{0};
-    std::size_t fixed_nv{0};
-    std::size_t floating_nq{0};
-    std::size_t floating_nv{0};
-    Transform4x4RowMajor base_from_imu{};
-};
-
-// Concrete adapter for the two original UME MJCF models.
-//
-// Construction parses both models and throws std::runtime_error if their
-// dimensions, joint ordering, joint coordinate indices, or required frame
-// topology differ from the frozen structural legacy contract.
-//
-// The floating model evaluates the original rnea(q, measured_arm_velocity, 0)
-// path: base twist and all accelerations are zero. Consequently its result
-// preserves the legacy velocity-dependent terms as well as gravity.
-//
-// Pinocchio Data objects are mutable workspaces. One adapter instance must be
-// used by one controller thread at a time. All Eigen workspaces are allocated
-// at construction and reused on the control path.
-class PinocchioUmeLegacyModelAdapter final
-    : public UmeLegacyModelAdapter {
-public:
-    PinocchioUmeLegacyModelAdapter(
-        std::string fixed_model_path,
-        std::string floating_model_path);
-    ~PinocchioUmeLegacyModelAdapter() override;
-
-    PinocchioUmeLegacyModelAdapter(
-        const PinocchioUmeLegacyModelAdapter&) = delete;
-    PinocchioUmeLegacyModelAdapter& operator=(
-        const PinocchioUmeLegacyModelAdapter&) = delete;
-    PinocchioUmeLegacyModelAdapter(
-        PinocchioUmeLegacyModelAdapter&&) noexcept;
-    PinocchioUmeLegacyModelAdapter& operator=(
-        PinocchioUmeLegacyModelAdapter&&) noexcept;
-
-    bool computeGravityCompensation(
-        const BimanualModelState& state,
-        JointVector& right_gravity_nm,
-        JointVector& left_gravity_nm) const override;
-
-    bool projectHapticFeedback(
-        const BimanualModelState& state,
-        ArmSide side,
-        const RawHapticFeedback& feedback,
-        ProjectedHapticEffort& projected) const override;
-
-    const UmeLegacyModelContract& contract() const noexcept;
-    const std::string& fixedModelPath() const noexcept;
-    const std::string& floatingModelPath() const noexcept;
-
-private:
-    class Impl;
-    std::unique_ptr<Impl> impl_;
-};
-
-} // namespace cmvr::ume_legacy
-
-#endif // CMVR_ES_PINOCCHIO_UME_LEGACY_MODEL_ADAPTER_H

cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_controller.h

@@ -1,34 +0,0 @@
-#ifndef CMVR_ES_UME_LEGACY_CONTROLLER_H
-#define CMVR_ES_UME_LEGACY_CONTROLLER_H
-
-#include "ume_legacy_types.h"
-
-namespace cmvr::ume_legacy {
-
-// Constants from UME commit e087df5cd3b281418722e155d9975695f163698e:
-//   ume/robot/ume/v6_imu/ume_leader/controller.py
-//   ume/robot/openarm1/teleop_leader_tuning.py
-LegacyUmeTuning originalTuning() noexcept;
-
-JointVector frictionCompensation(
-    const JointVector& velocity_rad_s,
-    const LegacyUmeTuning& tuning) noexcept;
-
-JointVector stictionCompensation(
-    const JointVector& velocity_rad_s,
-    const LegacyUmeTuning& tuning) noexcept;
-
-// error_norm is non-negative in the legacy path because it is produced by
-// np.linalg.norm. std::abs is retained here to match the subsequent Python
-// expression exactly for direct unit-level use.
-double feedbackScale(
-    double error_norm,
-    const LegacyUmeTuning& tuning) noexcept;
-
-SideControlOutput computeSideCommand(
-    const SideControlInput& input,
-    const LegacyUmeTuning& tuning = originalTuning()) noexcept;
-
-} // namespace cmvr::ume_legacy
-
-#endif // CMVR_ES_UME_LEGACY_CONTROLLER_H

cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_model_adapter.h

@@ -1,32 +0,0 @@
-#ifndef CMVR_ES_UME_LEGACY_MODEL_ADAPTER_H
-#define CMVR_ES_UME_LEGACY_MODEL_ADAPTER_H
-
-#include "ume_legacy_types.h"
-
-namespace cmvr::ume_legacy {
-
-// Boundary for the two model operations used by the original IMU controller:
-//   1. floating-base RNEA gravity compensation;
-//   2. fixed-base J_rot^T projection of shoulder/wrist moments.
-//
-// Concrete implementations must load and validate their model contract so the
-// pure controller cannot silently substitute guessed kinematics or dynamics.
-class UmeLegacyModelAdapter {
-public:
-    virtual ~UmeLegacyModelAdapter() = default;
-
-    virtual bool computeGravityCompensation(
-        const BimanualModelState& state,
-        JointVector& right_gravity_nm,
-        JointVector& left_gravity_nm) const = 0;
-
-    virtual bool projectHapticFeedback(
-        const BimanualModelState& state,
-        ArmSide side,
-        const RawHapticFeedback& feedback,
-        ProjectedHapticEffort& projected) const = 0;
-};
-
-} // namespace cmvr::ume_legacy
-
-#endif // CMVR_ES_UME_LEGACY_MODEL_ADAPTER_H

cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_types.h

@@ -1,110 +0,0 @@
-#ifndef CMVR_ES_UME_LEGACY_TYPES_H
-#define CMVR_ES_UME_LEGACY_TYPES_H
-
-#include <array>
-#include <cstddef>
-
-namespace cmvr::ume_legacy {
-
-inline constexpr std::size_t kArmDof = 8;
-inline constexpr std::size_t kTransformElementCount = 16;
-
-using JointVector = std::array<double, kArmDof>;
-using Vector3 = std::array<double, 3>;
-using Transform4x4RowMajor =
-    std::array<double, kTransformElementCount>;
-
-enum class ArmSide {
-    Right,
-    Left
-};
-
-// Shoulder and wrist entries have already been projected by J_rot^T. The
-// elbow and gripper entries are the scalar follower efforts received by the
-// original UME controller. Keeping this type separate prevents a 3-D moment
-// from being mislabeled as a 6-D Cartesian wrench.
-struct ProjectedHapticEffort {
-    Vector3 shoulder_joint_torque{};
-    double elbow_effort{0.0};
-    Vector3 wrist_joint_torque{};
-    double gripper_effort{0.0};
-};
-
-struct TrackingError {
-    Vector3 shoulder_rotation{};
-    double elbow{0.0};
-    Vector3 wrist_rotation{};
-    double gripper{0.0};
-};
-
-struct FeedbackScales {
-    double shoulder{0.0};
-    double elbow{0.0};
-    double wrist{0.0};
-    double gripper{0.0};
-};
-
-struct LegacyUmeTuning {
-    JointVector friction_coefficient{};
-    JointVector friction_max_compensation{};
-    JointVector stiction_threshold_min_rad_s{};
-    JointVector stiction_threshold_max_rad_s{};
-    JointVector stiction_compensation{};
-
-    double feedback_error_tolerance_rad{0.0};
-    double feedback_tanh_sharpness{0.0};
-    double feedback_scale{0.0};
-    double feedback_limit_dm4340_nm{0.0};
-    double feedback_limit_dm4310_nm{0.0};
-};
-
-struct SideControlInput {
-    ArmSide side{ArmSide::Right};
-    JointVector joint_velocity_rad_s{};
-    JointVector gravity_compensation_nm{};
-    ProjectedHapticEffort projected_haptic{};
-    TrackingError tracking_error{};
-};
-
-struct SideControlOutput {
-    JointVector friction_compensation_nm{};
-    JointVector stiction_compensation_nm{};
-    JointVector feedforward_without_haptic_nm{};
-
-    // This is the interaction effort after the legacy left/right scalar sign
-    // conventions, but before scaling and clipping.
-    JointVector signed_interaction_nm{};
-    FeedbackScales feedback_scales{};
-
-    // The legacy algorithm clips only this feedback contribution. It does not
-    // apply a final clamp to gravity, friction, stiction, or command_torque.
-    JointVector limited_feedback_nm{};
-    JointVector command_torque_nm{};
-};
-
-// Pure model inputs/outputs shared by the legacy controller and its
-// Pinocchio/MJCF model adapter.
-struct BimanualModelState {
-    // Joint arrays follow the frozen RJ1..RJ8 / LJ1..LJ8 MJCF order.
-    JointVector right_position_rad{};
-    JointVector right_velocity_rad_s{};
-    JointVector left_position_rad{};
-    JointVector left_velocity_rad_s{};
-
-    // Homogeneous rigid transform from the IMU frame to the gravity/world
-    // frame. The adapter rejects non-finite and non-rigid matrices.
-    Transform4x4RowMajor world_from_imu{};
-};
-
-struct RawHapticFeedback {
-    // Moments use the LOCAL_WORLD_ALIGNED frame expected by the original
-    // Pinocchio J_rot^T mapping.
-    Vector3 shoulder_moment{};
-    double elbow_effort{0.0};
-    Vector3 wrist_moment{};
-    double gripper_effort{0.0};
-};
-
-} // namespace cmvr::ume_legacy
-
-#endif // CMVR_ES_UME_LEGACY_TYPES_H

cmvr-es/algorithms/controllers/ume_legacy/src/pinocchio_ume_legacy_model_adapter.cpp

@@ -1,585 +0,0 @@
-#include "pinocchio_ume_legacy_model_adapter.h"
-
-#include <array>
-#include <cmath>
-#include <sstream>
-#include <stdexcept>
-#include <utility>
-
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-
-#include <pinocchio/algorithm/frames.hpp>
-#include <pinocchio/algorithm/jacobian.hpp>
-#include <pinocchio/algorithm/joint-configuration.hpp>
-#include <pinocchio/algorithm/rnea.hpp>
-#include <pinocchio/multibody/data.hpp>
-#include <pinocchio/multibody/model.hpp>
-#include <pinocchio/parsers/mjcf.hpp>
-
-namespace cmvr::ume_legacy {
-namespace {
-
-using ExpectedArmJointNames = std::array<std::string, 16>;
-
-const ExpectedArmJointNames& expectedArmJointNames()
-{
-    static const ExpectedArmJointNames names{
-        "RJ1", "RJ2", "RJ3", "RJ4",
-        "RJ5", "RJ6", "RJ7", "RJ8",
-        "LJ1", "LJ2", "LJ3", "LJ4",
-        "LJ5", "LJ6", "LJ7", "LJ8"};
-    return names;
-}
-
-std::runtime_error contractError(
-    const std::string& model_kind,
-    const std::string& detail)
-{
-    return std::runtime_error(
-        "UME " + model_kind + " MJCF contract violation: " + detail);
-}
-
-void requireDimensions(
-    const pinocchio::Model& model,
-    const std::string& model_kind,
-    int nq,
-    int nv,
-    pinocchio::JointIndex njoints)
-{
-    if (model.nq != nq ||
-        model.nv != nv ||
-        model.njoints != njoints) {
-        std::ostringstream detail;
-        detail << "expected nq/nv/njoints "
-               << nq << "/" << nv << "/" << njoints
-               << ", got " << model.nq << "/" << model.nv
-               << "/" << model.njoints;
-        throw contractError(model_kind, detail.str());
-    }
-}
-
-void requireJoint(
-    const pinocchio::Model& model,
-    const std::string& model_kind,
-    pinocchio::JointIndex joint_index,
-    const std::string& expected_name,
-    int expected_idx_q,
-    int expected_nq,
-    int expected_idx_v,
-    int expected_nv)
-{
-    if (joint_index >= model.njoints) {
-        throw contractError(
-            model_kind,
-            "missing joint " + expected_name);
-    }
-
-    if (model.names[joint_index] != expected_name ||
-        model.idx_qs[joint_index] != expected_idx_q ||
-        model.nqs[joint_index] != expected_nq ||
-        model.idx_vs[joint_index] != expected_idx_v ||
-        model.nvs[joint_index] != expected_nv) {
-        std::ostringstream detail;
-        detail << "joint[" << joint_index << "] expected "
-               << expected_name << " q(" << expected_idx_q
-               << "," << expected_nq << ") v(" << expected_idx_v
-               << "," << expected_nv << "), got "
-               << model.names[joint_index] << " q("
-               << model.idx_qs[joint_index] << ","
-               << model.nqs[joint_index] << ") v("
-               << model.idx_vs[joint_index] << ","
-               << model.nvs[joint_index] << ")";
-        throw contractError(model_kind, detail.str());
-    }
-}
-
-pinocchio::FrameIndex requireUniqueFrame(
-    const pinocchio::Model& model,
-    const std::string& model_kind,
-    const std::string& frame_name,
-    const std::string& expected_parent_joint_name)
-{
-    pinocchio::FrameIndex found = model.nframes;
-    std::size_t count = 0;
-    for (pinocchio::FrameIndex index = 0;
-         index < model.nframes;
-         ++index) {
-        if (model.frames[index].name == frame_name) {
-            found = index;
-            ++count;
-        }
-    }
-    if (count != 1) {
-        std::ostringstream detail;
-        detail << "expected exactly one frame " << frame_name
-               << ", got " << count;
-        throw contractError(model_kind, detail.str());
-    }
-
-    const auto parent_joint = model.frames[found].parentJoint;
-    if (parent_joint >= model.njoints ||
-        model.names[parent_joint] != expected_parent_joint_name) {
-        std::ostringstream detail;
-        detail << "frame " << frame_name
-               << " expected parent joint "
-               << expected_parent_joint_name;
-        if (parent_joint < model.njoints) {
-            detail << ", got " << model.names[parent_joint];
-        } else {
-            detail << ", got invalid index " << parent_joint;
-        }
-        throw contractError(model_kind, detail.str());
-    }
-    return found;
-}
-
-void validateFixedModel(
-    const pinocchio::Model& model,
-    std::array<pinocchio::FrameIndex, 4>& frame_ids)
-{
-    requireDimensions(model, "fixed", 16, 16, 17);
-    const auto& names = expectedArmJointNames();
-    for (std::size_t index = 0; index < names.size(); ++index) {
-        requireJoint(
-            model,
-            "fixed",
-            static_cast<pinocchio::JointIndex>(index + 1),
-            names[index],
-            static_cast<int>(index),
-            1,
-            static_cast<int>(index),
-            1);
-    }
-
-    frame_ids[0] =
-        requireUniqueFrame(model, "fixed", "R_shoulder", "RJ3");
-    frame_ids[1] =
-        requireUniqueFrame(model, "fixed", "R_wrist", "RJ7");
-    frame_ids[2] =
-        requireUniqueFrame(model, "fixed", "L_shoulder", "LJ3");
-    frame_ids[3] =
-        requireUniqueFrame(model, "fixed", "L_wrist", "LJ7");
-}
-
-pinocchio::FrameIndex validateFloatingModel(
-    const pinocchio::Model& model)
-{
-    requireDimensions(model, "floating", 23, 22, 18);
-    requireJoint(
-        model,
-        "floating",
-        1,
-        "dm_j4340_2ec_freejoint",
-        0,
-        7,
-        0,
-        6);
-
-    const auto& names = expectedArmJointNames();
-    for (std::size_t index = 0; index < names.size(); ++index) {
-        requireJoint(
-            model,
-            "floating",
-            static_cast<pinocchio::JointIndex>(index + 2),
-            names[index],
-            static_cast<int>(index + 7),
-            1,
-            static_cast<int>(index + 6),
-            1);
-    }
-
-    requireUniqueFrame(
-        model, "floating", "R_shoulder", "RJ3");
-    requireUniqueFrame(
-        model, "floating", "R_wrist", "RJ7");
-    requireUniqueFrame(
-        model, "floating", "L_shoulder", "LJ3");
-    requireUniqueFrame(
-        model, "floating", "L_wrist", "LJ7");
-    return requireUniqueFrame(
-        model,
-        "floating",
-        "imu",
-        "dm_j4340_2ec_freejoint");
-}
-
-bool finite(const JointVector& values) noexcept
-{
-    for (const double value : values) {
-        if (!std::isfinite(value)) {
-            return false;
-        }
-    }
-    return true;
-}
-
-bool finite(const Vector3& values) noexcept
-{
-    for (const double value : values) {
-        if (!std::isfinite(value)) {
-            return false;
-        }
-    }
-    return true;
-}
-
-bool toIsometry(
-    const Transform4x4RowMajor& source,
-    Eigen::Isometry3d& destination) noexcept
-{
-    Eigen::Matrix4d matrix;
-    for (Eigen::Index row = 0; row < 4; ++row) {
-        for (Eigen::Index column = 0; column < 4; ++column) {
-            matrix(row, column) =
-                source[static_cast<std::size_t>(row * 4 + column)];
-        }
-    }
-    if (!matrix.allFinite()) {
-        return false;
-    }
-
-    constexpr double kTransformTolerance = 1e-6;
-    if (std::abs(matrix(3, 0)) > kTransformTolerance ||
-        std::abs(matrix(3, 1)) > kTransformTolerance ||
-        std::abs(matrix(3, 2)) > kTransformTolerance ||
-        std::abs(matrix(3, 3) - 1.0) > kTransformTolerance) {
-        return false;
-    }
-
-    const Eigen::Matrix3d rotation =
-        matrix.template block<3, 3>(0, 0);
-    if (!(rotation.transpose() * rotation)
-             .isApprox(Eigen::Matrix3d::Identity(),
-                       kTransformTolerance) ||
-        std::abs(rotation.determinant() - 1.0) >
-            kTransformTolerance) {
-        return false;
-    }
-
-    destination = Eigen::Isometry3d::Identity();
-    destination.linear() = rotation;
-    destination.translation() =
-        matrix.template block<3, 1>(0, 3);
-    return true;
-}
-
-Transform4x4RowMajor toRowMajor(
-    const Eigen::Matrix4d& matrix) noexcept
-{
-    Transform4x4RowMajor result{};
-    for (Eigen::Index row = 0; row < 4; ++row) {
-        for (Eigen::Index column = 0; column < 4; ++column) {
-            result[static_cast<std::size_t>(row * 4 + column)] =
-                matrix(row, column);
-        }
-    }
-    return result;
-}
-
-Eigen::Vector3d toEigen(const Vector3& value) noexcept
-{
-    return {value[0], value[1], value[2]};
-}
-
-Vector3 fromEigen(const Eigen::Vector3d& value) noexcept
-{
-    return {value.x(), value.y(), value.z()};
-}
-
-} // namespace
-
-class PinocchioUmeLegacyModelAdapter::Impl {
-public:
-    Impl(std::string fixed_path, std::string floating_path)
-        : fixed_model_path(std::move(fixed_path)),
-          floating_model_path(std::move(floating_path))
-    {
-        try {
-            pinocchio::mjcf::buildModel(
-                fixed_model_path, fixed_model, false);
-        } catch (const std::exception& error) {
-            throw std::runtime_error(
-                "Failed to load fixed UME MJCF '" +
-                fixed_model_path + "': " + error.what());
-        }
-        try {
-            pinocchio::mjcf::buildModel(
-                floating_model_path, floating_model, false);
-        } catch (const std::exception& error) {
-            throw std::runtime_error(
-                "Failed to load floating UME MJCF '" +
-                floating_model_path + "': " + error.what());
-        }
-
-        validateFixedModel(fixed_model, fixed_frame_ids);
-        const auto imu_frame_id =
-            validateFloatingModel(floating_model);
-
-        fixed_data =
-            std::make_unique<pinocchio::Data>(fixed_model);
-        floating_data =
-            std::make_unique<pinocchio::Data>(floating_model);
-        fixed_q =
-            Eigen::VectorXd::Zero(fixed_model.nq);
-        floating_q =
-            Eigen::VectorXd::Zero(floating_model.nq);
-        floating_velocity =
-            Eigen::VectorXd::Zero(floating_model.nv);
-        floating_acceleration =
-            Eigen::VectorXd::Zero(floating_model.nv);
-        shoulder_jacobian =
-            Eigen::Matrix<double, 6, Eigen::Dynamic>::Zero(
-                6, fixed_model.nv);
-        wrist_jacobian =
-            Eigen::Matrix<double, 6, Eigen::Dynamic>::Zero(
-                6, fixed_model.nv);
-
-        Eigen::VectorXd neutral =
-            pinocchio::neutral(floating_model);
-        pinocchio::framesForwardKinematics(
-            floating_model, *floating_data, neutral);
-        base_from_imu =
-            floating_data->oMf[imu_frame_id];
-
-        const Eigen::Matrix4d base_from_imu_matrix =
-            base_from_imu.toHomogeneousMatrix();
-        if (!base_from_imu_matrix.allFinite()) {
-            throw contractError(
-                "floating", "non-finite base_from_imu transform");
-        }
-
-        contract_info.fixed_nq =
-            static_cast<std::size_t>(fixed_model.nq);
-        contract_info.fixed_nv =
-            static_cast<std::size_t>(fixed_model.nv);
-        contract_info.floating_nq =
-            static_cast<std::size_t>(floating_model.nq);
-        contract_info.floating_nv =
-            static_cast<std::size_t>(floating_model.nv);
-        contract_info.base_from_imu =
-            toRowMajor(base_from_imu_matrix);
-    }
-
-    std::string fixed_model_path;
-    std::string floating_model_path;
-    pinocchio::Model fixed_model;
-    pinocchio::Model floating_model;
-    std::unique_ptr<pinocchio::Data> fixed_data;
-    std::unique_ptr<pinocchio::Data> floating_data;
-    std::array<pinocchio::FrameIndex, 4> fixed_frame_ids{};
-    pinocchio::SE3 base_from_imu{pinocchio::SE3::Identity()};
-    UmeLegacyModelContract contract_info;
-
-    // Reused by the single controller thread. This keeps the 2 kHz legacy
-    // model path free of avoidable Eigen heap allocation after construction.
-    Eigen::VectorXd fixed_q;
-    Eigen::VectorXd floating_q;
-    Eigen::VectorXd floating_velocity;
-    Eigen::VectorXd floating_acceleration;
-    Eigen::Matrix<double, 6, Eigen::Dynamic> shoulder_jacobian;
-    Eigen::Matrix<double, 6, Eigen::Dynamic> wrist_jacobian;
-};
-
-PinocchioUmeLegacyModelAdapter::PinocchioUmeLegacyModelAdapter(
-    std::string fixed_model_path,
-    std::string floating_model_path)
-    : impl_(std::make_unique<Impl>(
-          std::move(fixed_model_path),
-          std::move(floating_model_path)))
-{
-}
-
-PinocchioUmeLegacyModelAdapter::
-    ~PinocchioUmeLegacyModelAdapter() = default;
-
-PinocchioUmeLegacyModelAdapter::PinocchioUmeLegacyModelAdapter(
-    PinocchioUmeLegacyModelAdapter&&) noexcept = default;
-
-PinocchioUmeLegacyModelAdapter&
-PinocchioUmeLegacyModelAdapter::operator=(
-    PinocchioUmeLegacyModelAdapter&&) noexcept = default;
-
-bool PinocchioUmeLegacyModelAdapter::computeGravityCompensation(
-    const BimanualModelState& state,
-    JointVector& right_gravity_nm,
-    JointVector& left_gravity_nm) const
-{
-    right_gravity_nm = {};
-    left_gravity_nm = {};
-
-    if (!impl_ ||
-        !finite(state.right_position_rad) ||
-        !finite(state.right_velocity_rad_s) ||
-        !finite(state.left_position_rad) ||
-        !finite(state.left_velocity_rad_s)) {
-        return false;
-    }
-
-    Eigen::Isometry3d world_from_imu;
-    if (!toIsometry(state.world_from_imu, world_from_imu)) {
-        return false;
-    }
-
-    Eigen::Isometry3d base_from_imu =
-        Eigen::Isometry3d::Identity();
-    base_from_imu.linear() =
-        impl_->base_from_imu.rotation();
-    base_from_imu.translation() =
-        impl_->base_from_imu.translation();
-    const Eigen::Isometry3d world_from_base =
-        world_from_imu * base_from_imu.inverse();
-
-    Eigen::Quaterniond world_q_base(
-        world_from_base.rotation());
-    if (!world_q_base.coeffs().allFinite() ||
-        world_q_base.norm() <= 0.0) {
-        return false;
-    }
-    world_q_base.normalize();
-
-    auto& q = impl_->floating_q;
-    auto& velocity = impl_->floating_velocity;
-    auto& acceleration = impl_->floating_acceleration;
-    q.setZero();
-    velocity.setZero();
-    acceleration.setZero();
-
-    q.segment<3>(0) = world_from_base.translation();
-    q.segment<4>(3) = world_q_base.coeffs();
-    for (std::size_t index = 0; index < kArmDof; ++index) {
-        q[static_cast<Eigen::Index>(7 + index)] =
-            state.right_position_rad[index];
-        q[static_cast<Eigen::Index>(15 + index)] =
-            state.left_position_rad[index];
-        velocity[static_cast<Eigen::Index>(6 + index)] =
-            state.right_velocity_rad_s[index];
-        velocity[static_cast<Eigen::Index>(14 + index)] =
-            state.left_velocity_rad_s[index];
-    }
-
-    const auto& torque = pinocchio::rnea(
-        impl_->floating_model,
-        *impl_->floating_data,
-        q,
-        velocity,
-        acceleration);
-    if (!torque.allFinite() || torque.size() != 22) {
-        return false;
-    }
-
-    for (std::size_t index = 0; index < kArmDof; ++index) {
-        right_gravity_nm[index] =
-            torque[static_cast<Eigen::Index>(6 + index)];
-        left_gravity_nm[index] =
-            torque[static_cast<Eigen::Index>(14 + index)];
-    }
-    return true;
-}
-
-bool PinocchioUmeLegacyModelAdapter::projectHapticFeedback(
-    const BimanualModelState& state,
-    ArmSide side,
-    const RawHapticFeedback& feedback,
-    ProjectedHapticEffort& projected) const
-{
-    projected = {};
-    if (!impl_ ||
-        (side != ArmSide::Right &&
-         side != ArmSide::Left) ||
-        !finite(state.right_position_rad) ||
-        !finite(state.left_position_rad) ||
-        !finite(feedback.shoulder_moment) ||
-        !finite(feedback.wrist_moment) ||
-        !std::isfinite(feedback.elbow_effort) ||
-        !std::isfinite(feedback.gripper_effort)) {
-        return false;
-    }
-
-    auto& q = impl_->fixed_q;
-    q.setZero();
-    for (std::size_t index = 0; index < kArmDof; ++index) {
-        q[static_cast<Eigen::Index>(index)] =
-            state.right_position_rad[index];
-        q[static_cast<Eigen::Index>(8 + index)] =
-            state.left_position_rad[index];
-    }
-
-    pinocchio::framesForwardKinematics(
-        impl_->fixed_model, *impl_->fixed_data, q);
-
-    const std::size_t frame_offset =
-        side == ArmSide::Right ? 0 : 2;
-    const Eigen::Index shoulder_column =
-        side == ArmSide::Right ? 0 : 8;
-    const Eigen::Index wrist_column =
-        side == ArmSide::Right ? 4 : 12;
-
-    auto& shoulder_jacobian = impl_->shoulder_jacobian;
-    shoulder_jacobian.setZero();
-    pinocchio::computeFrameJacobian(
-        impl_->fixed_model,
-        *impl_->fixed_data,
-        q,
-        impl_->fixed_frame_ids[frame_offset],
-        pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
-        shoulder_jacobian);
-
-    auto& wrist_jacobian = impl_->wrist_jacobian;
-    wrist_jacobian.setZero();
-    pinocchio::computeFrameJacobian(
-        impl_->fixed_model,
-        *impl_->fixed_data,
-        q,
-        impl_->fixed_frame_ids[frame_offset + 1],
-        pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
-        wrist_jacobian);
-
-    if (!shoulder_jacobian.allFinite() ||
-        !wrist_jacobian.allFinite()) {
-        return false;
-    }
-
-    const Eigen::Vector3d shoulder_torque =
-        shoulder_jacobian
-            .block<3, 3>(3, shoulder_column)
-            .transpose() *
-        toEigen(feedback.shoulder_moment);
-    const Eigen::Vector3d wrist_torque =
-        wrist_jacobian
-            .block<3, 3>(3, wrist_column)
-            .transpose() *
-        toEigen(feedback.wrist_moment);
-    if (!shoulder_torque.allFinite() ||
-        !wrist_torque.allFinite()) {
-        return false;
-    }
-
-    projected.shoulder_joint_torque =
-        fromEigen(shoulder_torque);
-    projected.elbow_effort = feedback.elbow_effort;
-    projected.wrist_joint_torque =
-        fromEigen(wrist_torque);
-    projected.gripper_effort = feedback.gripper_effort;
-    return true;
-}
-
-const UmeLegacyModelContract&
-PinocchioUmeLegacyModelAdapter::contract() const noexcept
-{
-    return impl_->contract_info;
-}
-
-const std::string&
-PinocchioUmeLegacyModelAdapter::fixedModelPath() const noexcept
-{
-    return impl_->fixed_model_path;
-}
-
-const std::string&
-PinocchioUmeLegacyModelAdapter::floatingModelPath() const noexcept
-{
-    return impl_->floating_model_path;
-}
-
-} // namespace cmvr::ume_legacy

cmvr-es/algorithms/controllers/ume_legacy/src/ume_legacy_controller.cpp

@@ -1,193 +0,0 @@
-#include "ume_legacy_controller.h"
-
-#include <cmath>
-
-namespace cmvr::ume_legacy {
-namespace {
-
-constexpr double kPi =
-    3.141592653589793238462643383279502884;
-
-double legacyClip(double value, double minimum, double maximum) noexcept
-{
-    // Explicit comparisons preserve NaN propagation: both comparisons are
-    // false and value is returned, matching np.clip for a NaN input.
-    if (value < minimum) {
-        return minimum;
-    }
-    if (value > maximum) {
-        return maximum;
-    }
-    return value;
-}
-
-double norm(const Vector3& value) noexcept
-{
-    return std::sqrt(value[0] * value[0] +
-                     value[1] * value[1] +
-                     value[2] * value[2]);
-}
-
-JointVector flattenInteraction(
-    ArmSide side,
-    const ProjectedHapticEffort& projected) noexcept
-{
-    JointVector interaction{
-        projected.shoulder_joint_torque[0],
-        projected.shoulder_joint_torque[1],
-        projected.shoulder_joint_torque[2],
-        projected.elbow_effort,
-        projected.wrist_joint_torque[0],
-        projected.wrist_joint_torque[1],
-        projected.wrist_joint_torque[2],
-        projected.gripper_effort};
-
-    // Exact scalar sign conventions from the legacy controller:
-    //   right elbow +, right gripper -
-    //   left  elbow -, left  gripper +
-    if (side == ArmSide::Right) {
-        interaction[7] = -interaction[7];
-    } else {
-        interaction[3] = -interaction[3];
-    }
-    return interaction;
-}
-
-} // namespace
-
-LegacyUmeTuning originalTuning() noexcept
-{
-    LegacyUmeTuning tuning;
-    tuning.friction_coefficient =
-        {1.6, 1.6, 1.6, 1.6, 0.032, 0.032, 0.032, 0.032};
-    tuning.friction_max_compensation =
-        {0.4, 0.4, 0.4, 0.4, 0.1, 0.1, 0.1, 0.1};
-
-    const double one_degree = kPi / 180.0;
-    const double ten_degrees = 10.0 * one_degree;
-    tuning.stiction_threshold_min_rad_s =
-        {one_degree, one_degree, one_degree, one_degree,
-         one_degree, one_degree, one_degree, one_degree};
-    tuning.stiction_threshold_max_rad_s =
-        {ten_degrees, ten_degrees, ten_degrees, ten_degrees,
-         ten_degrees, ten_degrees, ten_degrees, ten_degrees};
-    tuning.stiction_compensation =
-        {0.5, 0.5, 0.5, 0.5, 0.0, 0.0, 0.0, 0.0};
-
-    tuning.feedback_error_tolerance_rad = one_degree;
-    tuning.feedback_tanh_sharpness = 10.0;
-    tuning.feedback_scale = 0.5;
-    tuning.feedback_limit_dm4340_nm = 4.0;
-    tuning.feedback_limit_dm4310_nm = 1.0;
-    return tuning;
-}
-
-JointVector frictionCompensation(
-    const JointVector& velocity_rad_s,
-    const LegacyUmeTuning& tuning) noexcept
-{
-    JointVector result{};
-    for (std::size_t index = 0; index < kArmDof; ++index) {
-        const double maximum =
-            tuning.friction_max_compensation[index];
-        result[index] = legacyClip(
-            tuning.friction_coefficient[index] *
-                velocity_rad_s[index],
-            -maximum,
-            maximum);
-    }
-    return result;
-}
-
-JointVector stictionCompensation(
-    const JointVector& velocity_rad_s,
-    const LegacyUmeTuning& tuning) noexcept
-{
-    JointVector result{};
-    for (std::size_t index = 0; index < kArmDof; ++index) {
-        const double velocity = velocity_rad_s[index];
-        const double speed = std::abs(velocity);
-
-        // Both inequalities are intentionally strict, matching:
-        // min < abs(qvel) < max.
-        if (tuning.stiction_threshold_min_rad_s[index] < speed &&
-            speed < tuning.stiction_threshold_max_rad_s[index]) {
-            if (velocity > 0.0) {
-                result[index] =
-                    tuning.stiction_compensation[index];
-            } else if (velocity < 0.0) {
-                result[index] =
-                    -tuning.stiction_compensation[index];
-            }
-        }
-    }
-    return result;
-}
-
-double feedbackScale(
-    double error_norm,
-    const LegacyUmeTuning& tuning) noexcept
-{
-    return tuning.feedback_scale *
-           (std::tanh(
-                tuning.feedback_tanh_sharpness *
-                (std::abs(error_norm) -
-                 tuning.feedback_error_tolerance_rad)) +
-            1.0) /
-           2.0;
-}
-
-SideControlOutput computeSideCommand(
-    const SideControlInput& input,
-    const LegacyUmeTuning& tuning) noexcept
-{
-    SideControlOutput output;
-    output.friction_compensation_nm =
-        frictionCompensation(input.joint_velocity_rad_s, tuning);
-    output.stiction_compensation_nm =
-        stictionCompensation(input.joint_velocity_rad_s, tuning);
-    output.signed_interaction_nm =
-        flattenInteraction(input.side, input.projected_haptic);
-
-    output.feedback_scales.shoulder =
-        feedbackScale(norm(input.tracking_error.shoulder_rotation),
-                      tuning);
-    output.feedback_scales.elbow =
-        feedbackScale(std::abs(input.tracking_error.elbow), tuning);
-    output.feedback_scales.wrist =
-        feedbackScale(norm(input.tracking_error.wrist_rotation),
-                      tuning);
-    output.feedback_scales.gripper =
-        feedbackScale(std::abs(input.tracking_error.gripper), tuning);
-
-    for (std::size_t index = 0; index < kArmDof; ++index) {
-        output.feedforward_without_haptic_nm[index] =
-            input.gravity_compensation_nm[index] +
-            output.friction_compensation_nm[index] +
-            output.stiction_compensation_nm[index];
-
-        double scale = output.feedback_scales.gripper;
-        double limit = tuning.feedback_limit_dm4310_nm;
-        if (index < 3) {
-            scale = output.feedback_scales.shoulder;
-            limit = tuning.feedback_limit_dm4340_nm;
-        } else if (index == 3) {
-            scale = output.feedback_scales.elbow;
-            limit = tuning.feedback_limit_dm4340_nm;
-        } else if (index < 7) {
-            scale = output.feedback_scales.wrist;
-        }
-
-        output.limited_feedback_nm[index] = legacyClip(
-            scale * output.signed_interaction_nm[index],
-            -limit,
-            limit);
-        output.command_torque_nm[index] =
-            output.feedforward_without_haptic_nm[index] -
-            output.limited_feedback_nm[index];
-    }
-
-    return output;
-}
-
-} // namespace cmvr::ume_legacy

cmvr-es/algorithms/controllers/ume_legacy/tests/pinocchio_ume_legacy_model_adapter_test.cpp

@@ -1,392 +0,0 @@
-#include "pinocchio_ume_legacy_model_adapter.h"
-
-#include <array>
-#include <cmath>
-#include <cstddef>
-#include <limits>
-#include <stdexcept>
-#include <string>
-
-#include <gtest/gtest.h>
-
-#ifndef CMVR_UME_FIXED_MODEL_PATH
-#error "CMVR_UME_FIXED_MODEL_PATH must identify the deployed fixed UME MJCF"
-#endif
-
-#ifndef CMVR_UME_FLOATING_MODEL_PATH
-#error "CMVR_UME_FLOATING_MODEL_PATH must identify the deployed floating UME MJCF"
-#endif
-
-namespace cmvr::ume_legacy {
-namespace {
-
-constexpr double kNumericalTolerance = 1e-10;
-
-PinocchioUmeLegacyModelAdapter makeAdapter()
-{
-    return PinocchioUmeLegacyModelAdapter(
-        CMVR_UME_FIXED_MODEL_PATH,
-        CMVR_UME_FLOATING_MODEL_PATH);
-}
-
-BimanualModelState makeGoldenState(
-    const PinocchioUmeLegacyModelAdapter& adapter)
-{
-    BimanualModelState state;
-    state.world_from_imu =
-        adapter.contract().base_from_imu;
-    state.right_position_rad =
-        {0.1, -0.2, 0.3, -0.4,
-         0.2, -0.1, 0.15, -0.05};
-    state.left_position_rad =
-        {-0.1, 0.2, -0.3, 0.4,
-         -0.2, 0.1, -0.15, 0.05};
-    return state;
-}
-
-template <std::size_t Size>
-void expectFinite(const std::array<double, Size>& values)
-{
-    for (std::size_t index = 0; index < Size; ++index) {
-        EXPECT_TRUE(std::isfinite(values[index]))
-            << "index " << index;
-    }
-}
-
-template <std::size_t Size>
-void expectNear(
-    const std::array<double, Size>& actual,
-    const std::array<double, Size>& expected,
-    double tolerance = kNumericalTolerance)
-{
-    for (std::size_t index = 0; index < Size; ++index) {
-        EXPECT_NEAR(actual[index], expected[index], tolerance)
-            << "index " << index;
-    }
-}
-
-Transform4x4RowMajor multiplyTransforms(
-    const Transform4x4RowMajor& left,
-    const Transform4x4RowMajor& right)
-{
-    Transform4x4RowMajor result{};
-    for (std::size_t row = 0; row < 4; ++row) {
-        for (std::size_t column = 0; column < 4; ++column) {
-            for (std::size_t inner = 0; inner < 4; ++inner) {
-                result[row * 4 + column] +=
-                    left[row * 4 + inner] *
-                    right[inner * 4 + column];
-            }
-        }
-    }
-    return result;
-}
-
-Transform4x4RowMajor makeNoncommutingWorldFromBase()
-{
-    constexpr double roll = 0.2;
-    constexpr double pitch = -0.35;
-    constexpr double yaw = 0.47;
-    const double sr = std::sin(roll);
-    const double cr = std::cos(roll);
-    const double sp = std::sin(pitch);
-    const double cp = std::cos(pitch);
-    const double sy = std::sin(yaw);
-    const double cy = std::cos(yaw);
-    return {
-        cy * cp,
-        cy * sp * sr - sy * cr,
-        cy * sp * cr + sy * sr,
-        0.4,
-        sy * cp,
-        sy * sp * sr + cy * cr,
-        sy * sp * cr - cy * sr,
-        -0.1,
-        -sp,
-        cp * sr,
-        cp * cr,
-        0.8,
-        0.0, 0.0, 0.0, 1.0};
-}
-
-TEST(PinocchioUmeLegacyModelAdapterTest,
-     LoadsOriginalMjcfWithoutGeometryAssetsAndFreezesContract)
-{
-    const auto adapter = makeAdapter();
-    const auto& contract = adapter.contract();
-
-    EXPECT_EQ(contract.fixed_nq, 16U);
-    EXPECT_EQ(contract.fixed_nv, 16U);
-    EXPECT_EQ(contract.floating_nq, 23U);
-    EXPECT_EQ(contract.floating_nv, 22U);
-    EXPECT_EQ(adapter.fixedModelPath(), CMVR_UME_FIXED_MODEL_PATH);
-    EXPECT_EQ(
-        adapter.floatingModelPath(),
-        CMVR_UME_FLOATING_MODEL_PATH);
-
-    // This transform comes from the original floating model's imu site.
-    // Pinocchio buildModel parses it without loading STL geometry.
-    const Transform4x4RowMajor expected_base_from_imu{
-        0.0, 0.0, -1.0, -0.0298,
-        0.0, 1.0, 0.0, 0.0,
-        1.0, 0.0, 0.0, -0.229564,
-        0.0, 0.0, 0.0, 1.0};
-    expectNear(
-        contract.base_from_imu,
-        expected_base_from_imu,
-        1e-5);
-}
-
-TEST(PinocchioUmeLegacyModelAdapterTest,
-     FloatingBaseRneaProducesFiniteFrozenJointEfforts)
-{
-    const auto adapter = makeAdapter();
-    const auto state = makeGoldenState(adapter);
-    JointVector right{};
-    JointVector left{};
-
-    ASSERT_TRUE(
-        adapter.computeGravityCompensation(
-            state, right, left));
-    expectFinite(right);
-    expectFinite(left);
-    expectNear(
-        right,
-        {4.2579946000243867,
-         -3.101883528283977,
-         5.8349126697703291,
-         -3.2335040596068012,
-         0.54313804667559806,
-         -0.28419763993223052,
-         0.075420409617272505,
-         -0.0050792810993999194});
-    expectNear(
-        left,
-        {-4.2570142852347947,
-         3.1002195124462104,
-         -5.8319486672094438,
-         3.2334454894750602,
-         -0.54274278459746039,
-         0.28419800074629464,
-         -0.075420524366616282,
-         0.0050792800399334561});
-}
-
-TEST(PinocchioUmeLegacyModelAdapterTest,
-     ImuDerivedBaseOrientationReversesGravityUnderHalfTurn)
-{
-    const auto adapter = makeAdapter();
-    auto state = makeGoldenState(adapter);
-    JointVector upright_right{};
-    JointVector upright_left{};
-    ASSERT_TRUE(adapter.computeGravityCompensation(
-        state, upright_right, upright_left));
-
-    const Transform4x4RowMajor world_from_base_half_turn_x{
-        1.0, 0.0, 0.0, 0.0,
-        0.0, -1.0, 0.0, 0.0,
-        0.0, 0.0, -1.0, 0.0,
-        0.0, 0.0, 0.0, 1.0};
-    state.world_from_imu = multiplyTransforms(
-        world_from_base_half_turn_x,
-        adapter.contract().base_from_imu);
-
-    JointVector inverted_right{};
-    JointVector inverted_left{};
-    ASSERT_TRUE(adapter.computeGravityCompensation(
-        state, inverted_right, inverted_left));
-    for (std::size_t index = 0; index < kArmDof; ++index) {
-        EXPECT_NEAR(
-            inverted_right[index],
-            -upright_right[index],
-            kNumericalTolerance)
-            << "right joint index " << index;
-        EXPECT_NEAR(
-            inverted_left[index],
-            -upright_left[index],
-            kNumericalTolerance)
-            << "left joint index " << index;
-    }
-}
-
-TEST(PinocchioUmeLegacyModelAdapterTest,
-     NoncommutingImuPoseAndAsymmetricVelocitiesMatchFrozenRnea)
-{
-    const auto adapter = makeAdapter();
-    auto state = makeGoldenState(adapter);
-    state.world_from_imu = multiplyTransforms(
-        makeNoncommutingWorldFromBase(),
-        adapter.contract().base_from_imu);
-    state.right_velocity_rad_s =
-        {0.7, -0.4, 0.2, -0.1,
-         1.1, -0.8, 0.5, -0.3};
-    state.left_velocity_rad_s =
-        {-0.6, 0.9, -0.2, 0.4,
-         -1.0, 0.7, -0.5, 0.25};
-
-    JointVector right{};
-    JointVector left{};
-    ASSERT_TRUE(adapter.computeGravityCompensation(
-        state, right, left));
-    expectFinite(right);
-    expectFinite(left);
-    expectNear(
-        right,
-        {2.080988418159027,
-         -1.3277143244666021,
-         2.3194639604892364,
-         -2.0699331198781317,
-         0.40716561983947641,
-         -0.20631486209184946,
-         0.19335176308712126,
-         -0.01319194690287678});
-    expectNear(
-        left,
-        {-5.6976332776660232,
-         2.9915426497221254,
-         -2.5700109870406949,
-         2.1379083447512071,
-         -0.36441212045948712,
-         0.19226687457105307,
-         -0.085601264411386338,
-         0.0065129700338426369});
-}
-
-TEST(PinocchioUmeLegacyModelAdapterTest,
-     FixedModelRotationalProjectionMatchesFrozenValues)
-{
-    const auto adapter = makeAdapter();
-    const auto state = makeGoldenState(adapter);
-    RawHapticFeedback feedback;
-    feedback.shoulder_moment = {0.5, -0.2, 0.3};
-    feedback.elbow_effort = 1.2;
-    feedback.wrist_moment = {-0.4, 0.1, 0.6};
-    feedback.gripper_effort = -0.7;
-
-    ProjectedHapticEffort right{};
-    ASSERT_TRUE(adapter.projectHapticFeedback(
-        state, ArmSide::Right, feedback, right));
-    expectFinite(right.shoulder_joint_torque);
-    expectFinite(right.wrist_joint_torque);
-    expectNear(
-        right.shoulder_joint_torque,
-        {-0.5,
-         -0.12674237993402607,
-         -0.30915027509149773});
-    expectNear(
-        right.wrist_joint_torque,
-        {-0.065872923184419674,
-         -0.028130723042130143,
-         -0.72743566625468636});
-    EXPECT_DOUBLE_EQ(right.elbow_effort, feedback.elbow_effort);
-    EXPECT_DOUBLE_EQ(
-        right.gripper_effort,
-        feedback.gripper_effort);
-
-    ProjectedHapticEffort left{};
-    ASSERT_TRUE(adapter.projectHapticFeedback(
-        state, ArmSide::Left, feedback, left));
-    expectFinite(left.shoulder_joint_torque);
-    expectFinite(left.wrist_joint_torque);
-    expectNear(
-        left.shoulder_joint_torque,
-        {-0.5,
-         -0.36032671657345572,
-         0.083245914753940151});
-    expectNear(
-        left.wrist_joint_torque,
-        {-0.13497315246130309,
-         0.15764759010789803,
-         -0.70779204949804098});
-    EXPECT_DOUBLE_EQ(left.elbow_effort, feedback.elbow_effort);
-    EXPECT_DOUBLE_EQ(
-        left.gripper_effort,
-        feedback.gripper_effort);
-}
-
-TEST(PinocchioUmeLegacyModelAdapterTest,
-     RejectsNonRigidOrNonFiniteInputsAndZerosOutputs)
-{
-    const auto adapter = makeAdapter();
-    auto state = makeGoldenState(adapter);
-    JointVector right;
-    JointVector left;
-    right.fill(1.0);
-    left.fill(1.0);
-
-    state.world_from_imu = {};
-    EXPECT_FALSE(adapter.computeGravityCompensation(
-        state, right, left));
-    expectNear(right, JointVector{});
-    expectNear(left, JointVector{});
-
-    state = makeGoldenState(adapter);
-    state.right_position_rad[3] =
-        std::numeric_limits<double>::quiet_NaN();
-    right.fill(1.0);
-    left.fill(1.0);
-    EXPECT_FALSE(adapter.computeGravityCompensation(
-        state, right, left));
-    expectNear(right, JointVector{});
-    expectNear(left, JointVector{});
-
-    state = makeGoldenState(adapter);
-    RawHapticFeedback feedback;
-    feedback.shoulder_moment[1] =
-        std::numeric_limits<double>::infinity();
-    ProjectedHapticEffort projected;
-    projected.elbow_effort = 1.0;
-    EXPECT_FALSE(adapter.projectHapticFeedback(
-        state, ArmSide::Right, feedback, projected));
-    expectNear(
-        projected.shoulder_joint_torque,
-        Vector3{});
-    expectNear(projected.wrist_joint_torque, Vector3{});
-    EXPECT_DOUBLE_EQ(projected.elbow_effort, 0.0);
-    EXPECT_DOUBLE_EQ(projected.gripper_effort, 0.0);
-
-    feedback = {};
-    projected.elbow_effort = 1.0;
-    EXPECT_FALSE(adapter.projectHapticFeedback(
-        state,
-        static_cast<ArmSide>(99),
-        feedback,
-        projected));
-    expectNear(
-        projected.shoulder_joint_torque,
-        Vector3{});
-    expectNear(projected.wrist_joint_torque, Vector3{});
-    EXPECT_DOUBLE_EQ(projected.elbow_effort, 0.0);
-    EXPECT_DOUBLE_EQ(projected.gripper_effort, 0.0);
-}
-
-TEST(PinocchioUmeLegacyModelAdapterTest,
-     MissingModelFailsAtConstruction)
-{
-    EXPECT_THROW(
-        PinocchioUmeLegacyModelAdapter(
-            "/definitely/missing/ume_fixed.xml",
-            CMVR_UME_FLOATING_MODEL_PATH),
-        std::runtime_error);
-}
-
-TEST(PinocchioUmeLegacyModelAdapterTest,
-     RejectsModelRoleSwapEvenThoughBothMjcfFilesParse)
-{
-    try {
-        PinocchioUmeLegacyModelAdapter adapter(
-            CMVR_UME_FLOATING_MODEL_PATH,
-            CMVR_UME_FIXED_MODEL_PATH);
-        (void)adapter;
-        FAIL() << "swapped fixed/floating models were accepted";
-    } catch (const std::runtime_error& error) {
-        EXPECT_NE(
-            std::string(error.what()).find(
-                "UME fixed MJCF contract violation: "
-                "expected nq/nv/njoints 16/16/17"),
-            std::string::npos);
-    }
-}
-
-} // namespace
-} // namespace cmvr::ume_legacy

cmvr-es/algorithms/controllers/ume_legacy/tests/ume_legacy_controller_golden_test.cpp

@@ -1,216 +0,0 @@
-#include "ume_legacy_controller.h"
-#include "ume_legacy_model_adapter.h"
-
-#include <array>
-#include <cmath>
-#include <cstddef>
-#include <limits>
-
-#include <gtest/gtest.h>
-
-namespace cmvr::ume_legacy {
-namespace {
-
-constexpr double kTolerance = 1e-12;
-
-void expectJointVectorNear(
-    const JointVector& actual,
-    const JointVector& expected,
-    double tolerance = kTolerance)
-{
-    for (std::size_t index = 0; index < kArmDof; ++index) {
-        EXPECT_NEAR(actual[index], expected[index], tolerance)
-            << "joint index " << index;
-    }
-}
-
-TEST(UmeLegacyControllerGoldenTest,
-     OriginalTuningAndFrictionMatchPythonOracle)
-{
-    const auto tuning = originalTuning();
-    EXPECT_DOUBLE_EQ(tuning.friction_coefficient[0], 1.6);
-    EXPECT_DOUBLE_EQ(tuning.friction_coefficient[4], 0.032);
-    EXPECT_DOUBLE_EQ(tuning.feedback_limit_dm4340_nm, 4.0);
-    EXPECT_DOUBLE_EQ(tuning.feedback_limit_dm4310_nm, 1.0);
-
-    const JointVector velocity{
-        -1.0, -0.1, 0.0, 2.0 * std::acos(-1.0) / 180.0,
-        -10.0, -1.0, 1.0, 10.0};
-    const JointVector expected{
-        -0.4, -0.16000000000000003, 0.0,
-        0.055850536063818547,
-        -0.1, -0.032, 0.032, 0.1};
-
-    expectJointVectorNear(
-        frictionCompensation(velocity, tuning),
-        expected);
-}
-
-TEST(UmeLegacyControllerGoldenTest,
-     StictionUsesStrictLegacyThresholds)
-{
-    const auto tuning = originalTuning();
-    const double minimum =
-        tuning.stiction_threshold_min_rad_s[0];
-    const double maximum =
-        tuning.stiction_threshold_max_rad_s[0];
-    const JointVector at_threshold{
-        minimum,
-        -minimum,
-        maximum,
-        -maximum,
-        0.0, 0.0, 0.0, 0.0};
-    expectJointVectorNear(
-        stictionCompensation(at_threshold, tuning),
-        JointVector{});
-
-    const JointVector strictly_inside{
-        2.0 * minimum,
-        -2.0 * minimum,
-        std::nextafter(
-            minimum, std::numeric_limits<double>::infinity()),
-        std::nextafter(maximum, 0.0),
-        2.0 * minimum,
-        -2.0 * minimum,
-        std::nextafter(
-            minimum, std::numeric_limits<double>::infinity()),
-        std::nextafter(maximum, 0.0)};
-    expectJointVectorNear(
-        stictionCompensation(strictly_inside, tuning),
-        {0.5, -0.5, 0.5, 0.5,
-         0.0, 0.0, 0.0, 0.0});
-}
-
-TEST(UmeLegacyControllerGoldenTest,
-     FeedbackScaleMatchesLegacyNormAndTanhGoldenValues)
-{
-    const auto tuning = originalTuning();
-    EXPECT_NEAR(
-        feedbackScale(0.0, tuning),
-        0.20680448412090907,
-        kTolerance);
-    EXPECT_DOUBLE_EQ(
-        feedbackScale(tuning.feedback_error_tolerance_rad, tuning),
-        0.25);
-    EXPECT_NEAR(
-        feedbackScale(0.05, tuning),
-        0.32861047013244982,
-        kTolerance);
-    EXPECT_NEAR(
-        feedbackScale(-0.2, tuning),
-        0.48734517579834447,
-        kTolerance);
-}
-
-TEST(UmeLegacyControllerGoldenTest,
-     CompleteRightSideCommandMatchesPythonGoldenVector)
-{
-    SideControlInput input;
-    input.side = ArmSide::Right;
-    input.joint_velocity_rad_s = {
-        -1.0, -0.1, 0.0, 2.0 * std::acos(-1.0) / 180.0,
-        -10.0, -1.0, 1.0, 10.0};
-    input.gravity_compensation_nm =
-        {0.5, -0.5, 1.0, -1.0,
-         0.25, -0.25, 0.75, -0.75};
-    input.projected_haptic.shoulder_joint_torque =
-        {1.2, -3.0, 10.0};
-    input.projected_haptic.elbow_effort = 2.0;
-    input.projected_haptic.wrist_joint_torque =
-        {0.5, -2.0, 5.0};
-    input.projected_haptic.gripper_effort = 3.0;
-    input.tracking_error.shoulder_rotation = {0.0, 0.0, 0.0};
-    input.tracking_error.elbow =
-        originalTuning().feedback_error_tolerance_rad;
-    input.tracking_error.wrist_rotation = {0.03, 0.04, 0.0};
-    input.tracking_error.gripper = -0.2;
-
-    const auto output = computeSideCommand(input);
-
-    expectJointVectorNear(
-        output.friction_compensation_nm,
-        {-0.4, -0.16000000000000003, 0.0,
-         0.055850536063818547,
-         -0.1, -0.032, 0.032, 0.1});
-    expectJointVectorNear(
-        output.stiction_compensation_nm,
-        {0.0, -0.5, 0.0, 0.5,
-         0.0, 0.0, 0.0, 0.0});
-    expectJointVectorNear(
-        output.feedforward_without_haptic_nm,
-        {0.099999999999999978, -1.1600000000000001,
-         1.0, -0.44414946393618149,
-         0.14999999999999999, -0.28200000000000003,
-         0.78200000000000003, -0.65000000000000002});
-    expectJointVectorNear(
-        output.signed_interaction_nm,
-        {1.2, -3.0, 10.0, 2.0,
-         0.5, -2.0, 5.0, -3.0});
-    expectJointVectorNear(
-        output.limited_feedback_nm,
-        {0.24816538094509089, -0.62041345236272716,
-         2.0680448412090908, 0.5,
-         0.16430523506622491, -0.65722094026489963,
-         1.0, -1.0});
-    expectJointVectorNear(
-        output.command_torque_nm,
-        {-0.14816538094509091, -0.53958654763727298,
-         -1.0680448412090908, -0.94414946393618149,
-         -0.014305235066224914, 0.37522094026489961,
-         -0.21799999999999997, 0.34999999999999998});
-}
-
-TEST(UmeLegacyControllerGoldenTest,
-     LeftAndRightScalarSignsAndGroupLimitsArePreserved)
-{
-    SideControlInput input;
-    input.projected_haptic.shoulder_joint_torque =
-        {100.0, -100.0, 100.0};
-    input.projected_haptic.elbow_effort = 100.0;
-    input.projected_haptic.wrist_joint_torque =
-        {100.0, -100.0, 100.0};
-    input.projected_haptic.gripper_effort = 100.0;
-    input.tracking_error.shoulder_rotation = {10.0, 0.0, 0.0};
-    input.tracking_error.elbow = 10.0;
-    input.tracking_error.wrist_rotation = {10.0, 0.0, 0.0};
-    input.tracking_error.gripper = 10.0;
-
-    input.side = ArmSide::Right;
-    const auto right = computeSideCommand(input);
-    expectJointVectorNear(
-        right.signed_interaction_nm,
-        {100.0, -100.0, 100.0, 100.0,
-         100.0, -100.0, 100.0, -100.0});
-    expectJointVectorNear(
-        right.command_torque_nm,
-        {-4.0, 4.0, -4.0, -4.0,
-         -1.0, 1.0, -1.0, 1.0});
-
-    input.side = ArmSide::Left;
-    const auto left = computeSideCommand(input);
-    expectJointVectorNear(
-        left.signed_interaction_nm,
-        {100.0, -100.0, 100.0, -100.0,
-         100.0, -100.0, 100.0, 100.0});
-    expectJointVectorNear(
-        left.command_torque_nm,
-        {-4.0, 4.0, -4.0, 4.0,
-         -1.0, 1.0, -1.0, -1.0});
-}
-
-TEST(UmeLegacyControllerGoldenTest,
-     FeedbackClipDoesNotClampOtherFeedforwardTerms)
-{
-    SideControlInput input;
-    input.gravity_compensation_nm =
-        {50.0, -50.0, 0.0, 0.0, 0.0, 0.0, 20.0, -20.0};
-    const auto output = computeSideCommand(input);
-
-    EXPECT_DOUBLE_EQ(output.command_torque_nm[0], 50.0);
-    EXPECT_DOUBLE_EQ(output.command_torque_nm[1], -50.0);
-    EXPECT_DOUBLE_EQ(output.command_torque_nm[6], 20.0);
-    EXPECT_DOUBLE_EQ(output.command_torque_nm[7], -20.0);
-}
-
-} // namespace
-} // namespace cmvr::ume_legacy

cmvr-es/common/types/arm/arm_types.h

@@ -103,11 +103,6 @@ struct JointGroupState {
     std::vector<double> position;
     std::vector<double> velocity;
     std::vector<double> effort;
-    std::uint64_t sequence{0};
-    std::int64_t sample_monotonic_ns{0};
-    bool position_valid{false};
-    bool velocity_valid{false};
-    bool effort_valid{false};
 
     bool validForModel(const RobotModel& model) const
     {
@@ -170,14 +165,6 @@ struct ServoOptions {
     double gain{300.0};
 };
 
-struct TorqueServoOptions {
-    // The UME legacy loop runs at 800 Hz by default.
-    double period{0.00125};
-    // A producer must continuously refresh the latest torque command. A stale
-    // command latches a fault and disables the actuator chain.
-    std::uint32_t command_watchdog_ms{20};
-};
-
 enum class RobotMode {
     Unknown = 0,
     Disconnected,
@@ -210,14 +197,6 @@ enum class ControlMode {
     Freedrive
 };
 
-enum class JointEffortSource {
-    Unspecified = 0,
-    MotorEstimate,
-    JointSensor,
-    ForceTorqueSensor,
-    Observer
-};
-
 struct ArmState {
     double timestamp{0.0};
     RobotMode robot_mode{RobotMode::Unknown};

cmvr-es/config/README.md

@@ -18,8 +18,6 @@ cmvr_es.pb.txt
 入口文件：
 
 - [`cmvr_es.pb.txt`](cmvr_es.pb.txt)
-- [`cmvr_es_ume.pb.txt`](cmvr_es_ume.pb.txt)：UME 主端样例
-- [`cmvr_es_robot.pb.txt`](cmvr_es_robot.pb.txt)：人形机械臂从端样例
 - [`manager/device_manager.pb.txt`](manager/device_manager.pb.txt)
 - [`manager/task_manager.pb.txt`](manager/task_manager.pb.txt)
 
@@ -31,10 +29,10 @@ cmvr_es.pb.txt
 <cmvr_es 可执行文件所在目录>/config/cmvr_es.pb.txt
 ```
 
-安装后的 `output/bin/cmvr_es` 因此会读取 `output/bin/config/cmvr_es.pb.txt`；直接运行 `build/cmvr_es` 则会查找 `build/config/cmvr_es.pb.txt`，不会自动跳到安装目录。传入显式根配置时使用 `--config`：
+安装后的 `output/bin/cmvr_es` 因此会读取 `output/bin/config/cmvr_es.pb.txt`；直接运行 `build/cmvr_es` 则会查找 `build/config/cmvr_es.pb.txt`，不会自动跳到安装目录。传入显式根配置时：
 
 ```bash
-./output/bin/cmvr_es --config /etc/cmvr-es/cmvr_es.pb.txt
+./output/bin/cmvr_es /etc/cmvr-es/cmvr_es.pb.txt
 ```
 
 设备、任务和证书等相对配置路径均以根配置文件所在目录解析。模型等资源通过 `ConfigHelper::resolveResourceFile()` 在配置根及父目录中查找；生产部署仍建议使用明确绝对路径。
@@ -111,48 +109,6 @@ output/bin/protoc \
 
 该命令只验证 Proto Text 解析，不验证文件、设备、证书、网络和跨字段语义。最终仍需运行组件测试和进程烟雾测试。
 
-## 双边遥操部署样例
-
-仓库提供两个相互独立的 CMVR-ES 配置入口：
-
-- UME 主端：[`cmvr_es_ume.pb.txt`](cmvr_es_ume.pb.txt)，只声明
-  `ume_left` 和 `ume_right`。两条机械臂在 DeviceManager 层默认关闭，
-  [`devices/arm/ume_arms.pb.txt`](devices/arm/ume_arms.pb.txt) 内部的
-  `hardware_enabled` 也默认关闭；两层开关必须经过标定与安全验收后分别启用。
-  出站 `ume_teleop` Task 默认关闭，样例不包含机器人地址或凭据。当前 Task
-  只实现会话/重连/心跳和 latest-only 指令邮箱，尚无生产算法调用
-  `submitSetpoint()`，返回 effort 也尚未接入本地触觉协调器。
-- 人形机械臂从端：[`cmvr_es_robot.pb.txt`](cmvr_es_robot.pb.txt)，通用 gRPC
-  server 可以启动，但 `ti5_motors` 和 `right_arm` 仍默认关闭。生产
-  `ArmTeleop` 已实现真实 `RobotArm` 适配，但服务配置的 `enable` 显式关闭且
-  样例哈希故意留空。`MotorRobotArm.enable_teleop_group_servo` 目前只是预留字段；
-  因为现有 `servoJ` 仍是逐关节顺序写，代码即使看到该字段为 true 也会拒绝能力。
-  必须先实现并验收原子或定时的组下发原语。因此启动 gRPC server 不等于允许遥操
-  执行，也不能绕过设备层硬件门。
-
-在两台边缘设备各自的源码或安装目录运行：
-
-```bash
-# UME 主端（源码配置）
-./output/bin/cmvr_es \
-  --config ./cmvr-es/config/cmvr_es_ume.pb.txt
-
-# 人形机械臂从端（源码配置）
-./output/bin/cmvr_es \
-  --config ./cmvr-es/config/cmvr_es_robot.pb.txt
-```
-
-如果使用安装后的配置副本，则相应命令为：
-
-```bash
-./output/bin/cmvr_es --config ./output/bin/config/cmvr_es_ume.pb.txt
-./output/bin/cmvr_es --config ./output/bin/config/cmvr_es_robot.pb.txt
-```
-
-上线前应把两套配置分别复制到两台机器的外部配置目录。主端需要填写从端地址、
-会话 manifest 和认证配置；从端需要换成现场机械臂设备配置，并在真实硬件测试后
-逐层开启。不要把生产 IP、token、私钥或设备标定值提交到仓库样例。
-
 ## 生产配置
 
 `cmake --install` 会重建 `output/bin/config/`。生产配置应复制到 `/etc/cmvr-es/` 等外部目录并显式传入。

cmvr-es/config/cmvr_es_robot.pb.txt

@@ -1,11 +0,0 @@
-# Follower robot-side CMVR-ES profile.
-#
-# The RobotArm ArmTeleop backend is implemented but explicitly disabled. The
-# physical arm and service backend remain closed. Current MotorRobotArm
-# sequential joint writes are rejected as a teleop group-servo capability until
-# an atomic/timed group primitive and its safety timing gates are accepted.
-cmvr_es {
-  logger_config_file: "logger/logger.pb.txt"
-  device_manager_config_file: "manager/device_manager_robot.pb.txt"
-  task_manager_config_file: "manager/task_manager_robot.pb.txt"
-}

cmvr-es/config/cmvr_es_ume.pb.txt

@@ -1,10 +0,0 @@
-# UME leader-side CMVR-ES profile.
-#
-# All relative paths below are resolved from this file's directory. This
-# checked-in profile contains no production endpoint, credentials or hardware
-# enablement.
-cmvr_es {
-  logger_config_file: "logger/logger.pb.txt"
-  device_manager_config_file: "manager/device_manager_ume.pb.txt"
-  task_manager_config_file: "manager/task_manager_ume.pb.txt"
-}

cmvr-es/config/devices/arm/arm.pb.txt

@@ -17,10 +17,6 @@ arm {
       buffer_size: 50
       default_vel: 1.0
       default_acc: 2.0
-      # Reserved only: current MotorRobotArm servoJ writes joints sequentially,
-      # so code rejects the teleop group-servo capability even if this is true.
-      # A reviewed atomic/timed group primitive is required before changing it.
-      enable_teleop_group_servo: false
     }
 
     kinematics {

cmvr-es/config/devices/arm/ume_arms.pb.txt

@@ -1,72 +0,0 @@
-# UME leader-arm device templates. They are deliberately disabled in
-# manager/device_manager.pb.txt and hardware_enabled remains false here.
-#
-# Before real hardware use, independently verify interface bitrate
-# (1 Mbit/s arbitration, 5 Mbit/s data, FD+BRS), motor/feedback IDs,
-# direction, zero offsets, mechanical joint limits and safe torque limits.
-# Each joint must also receive reviewed healthy_feedback_status and raw
-# temperature thresholds. They are deliberately absent below, so changing
-# hardware_enabled alone is insufficient to arm these placeholder profiles.
-arm {
-  robot_arms {
-    id: "ume_right"
-    ume {
-      can {
-        dev_id: "can4"
-        channel_id: 4
-        interface_name: "can4"
-        enable_fd: true
-        bitrate_switch: true
-        send_timeout_us: 100
-        receive_timeout_us: 100
-        receive_own_messages: false
-        enable_error_frames: true
-      }
-      control_frequency_hz: 800
-      cycle_deadline_us: 1000
-      feedback_watchdog_ms: 20
-      shutdown_timeout_ms: 50
-      hardware_enabled: false
-
-      joints { joint_name: "RJ1" command_id: 1 feedback_id: 17 reported_motor_id: 1 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
-      joints { joint_name: "RJ2" command_id: 2 feedback_id: 18 reported_motor_id: 2 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
-      joints { joint_name: "RJ3" command_id: 3 feedback_id: 19 reported_motor_id: 3 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
-      joints { joint_name: "RJ4" command_id: 4 feedback_id: 20 reported_motor_id: 4 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
-      joints { joint_name: "RJ5" command_id: 5 feedback_id: 21 reported_motor_id: 5 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
-      joints { joint_name: "RJ6" command_id: 6 feedback_id: 22 reported_motor_id: 6 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
-      joints { joint_name: "RJ7" command_id: 7 feedback_id: 23 reported_motor_id: 7 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
-      joints { joint_name: "RJ8" command_id: 8 feedback_id: 24 reported_motor_id: 8 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
-    }
-  }
-
-  robot_arms {
-    id: "ume_left"
-    ume {
-      can {
-        dev_id: "can5"
-        channel_id: 5
-        interface_name: "can5"
-        enable_fd: true
-        bitrate_switch: true
-        send_timeout_us: 100
-        receive_timeout_us: 100
-        receive_own_messages: false
-        enable_error_frames: true
-      }
-      control_frequency_hz: 800
-      cycle_deadline_us: 1000
-      feedback_watchdog_ms: 20
-      shutdown_timeout_ms: 50
-      hardware_enabled: false
-
-      joints { joint_name: "LJ1" command_id: 1 feedback_id: 17 reported_motor_id: 1 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
-      joints { joint_name: "LJ2" command_id: 2 feedback_id: 18 reported_motor_id: 2 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
-      joints { joint_name: "LJ3" command_id: 3 feedback_id: 19 reported_motor_id: 3 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
-      joints { joint_name: "LJ4" command_id: 4 feedback_id: 20 reported_motor_id: 4 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
-      joints { joint_name: "LJ5" command_id: 5 feedback_id: 21 reported_motor_id: 5 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
-      joints { joint_name: "LJ6" command_id: 6 feedback_id: 22 reported_motor_id: 6 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
-      joints { joint_name: "LJ7" command_id: 7 feedback_id: 23 reported_motor_id: 7 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
-      joints { joint_name: "LJ8" command_id: 8 feedback_id: 24 reported_motor_id: 8 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
-    }
-  }
-}

cmvr-es/config/devices/motor/plc_motors.pb.txt

@@ -1,56 +0,0 @@
-motor {
-  id: "plc_motors"
-
-  motor_groups {
-    id: "plc_axis_group"
-    bus_type: MOTOR_BUS_MODBUS_TCP
-    vendor: MOTOR_VENDOR_PLC_GENERIC
-    protocol: MOTOR_PROTOCOL_CMVR_PLC_V1
-
-    modbus_tcp {
-      host: "192.168.0.10"
-      port: 502
-      unit_id: 1
-
-      connect_timeout_ms: 500
-      io_timeout_ms: 100
-      heartbeat_period_ms: 100
-      communication_watchdog_ms: 1000
-      cyclic_watchdog_ms: 500
-      status_poll_period_ms: 20
-      reconnect_min_ms: 100
-      reconnect_max_ms: 2000
-      command_ack_timeout_ms: 500
-
-      protocol_major: 1
-      protocol_minor: 0
-
-      axes { motor_id: 1 axis_index: 0 }
-      axes { motor_id: 2 axis_index: 1 }
-    }
-
-    joint_limits {
-      enable: true
-      source: JOINT_LIMIT_SOURCE_CUSTOM
-      joints {
-        joint_name: "PLC_AXIS_1"
-        q_lb: -3.141592653589793
-        q_ub: 3.141592653589793
-        qd: 1.0
-        qdd: 2.0
-      }
-      joints {
-        joint_name: "PLC_AXIS_2"
-        q_lb: -1.5707963267948966
-        q_ub: 1.5707963267948966
-        qd: 0.5
-        qdd: 1.0
-      }
-    }
-
-    motors {
-      motors { id: 1 joint_name: "PLC_AXIS_1" }
-      motors { id: 2 joint_name: "PLC_AXIS_2" }
-    }
-  }
-}

cmvr-es/config/manager/device_manager.pb.txt

@@ -82,14 +82,6 @@ device_manager {
     enable: false
   }
 
-  devices {
-    id: "plc_motors"
-    type: DEVICE_TYPE_MOTOR_SYSTEM
-    config_file: "devices/motor/plc_motors.pb.txt"
-    # Configure the PLC endpoint and complete the safety checkout before enabling.
-    enable: false
-  }
-
   devices {
     id: "right_arm"
     type: DEVICE_TYPE_ROBOT_ARM
@@ -111,23 +103,6 @@ device_manager {
     enable: false
   }
 
-  devices {
-    id: "ume_right"
-    type: DEVICE_TYPE_ROBOT_ARM
-    config_file: "devices/arm/ume_arms.pb.txt"
-    # Two gates must be explicitly changed after the physical safety review:
-    # this entry and ume.hardware_enabled in the arm config.
-    enable: false
-  }
-
-  devices {
-    id: "ume_left"
-    type: DEVICE_TYPE_ROBOT_ARM
-    config_file: "devices/arm/ume_arms.pb.txt"
-    # Two gates must be explicitly changed after the physical safety review.
-    enable: false
-  }
-
   devices {
     id: "bio_head"
     type: DEVICE_TYPE_BIO_HEAD_ROBOT

cmvr-es/config/manager/device_manager_robot.pb.txt

@@ -1,24 +0,0 @@
-# Follower robot-side devices.
-device_manager {
-  name: "cmvr_es_robot"
-  version: "0.1"
-  description: "CMVR humanoid follower edge system"
-  init_all_motors_when_no_active_joints: false
-
-  devices {
-    id: "ti5_motors"
-    type: DEVICE_TYPE_MOTOR_SYSTEM
-    config_file: "devices/motor/ti5_motors.pb.txt"
-    # Physical motor communication remains fail-closed in this example.
-    enable: false
-  }
-
-  devices {
-    id: "right_arm"
-    type: DEVICE_TYPE_ROBOT_ARM
-    config_file: "devices/arm/arm.pb.txt"
-    # Do not enable until the motor system, URDF, limits, servoJ timing and
-    # independent emergency-stop path have passed the robot safety checkout.
-    enable: false
-  }
-}

cmvr-es/config/manager/device_manager_ume.pb.txt

@@ -1,24 +0,0 @@
-# UME leader-side devices only.
-device_manager {
-  name: "cmvr_es_ume"
-  version: "0.1"
-  description: "UME leader edge system"
-  init_all_motors_when_no_active_joints: false
-
-  devices {
-    id: "ume_right"
-    type: DEVICE_TYPE_ROBOT_ARM
-    config_file: "devices/arm/ume_arms.pb.txt"
-    # Hardware gate 1/2. Gate 2/2 is ume.hardware_enabled in the arm config.
-    # Keep both false until CAN mapping, limits and physical safety are verified.
-    enable: false
-  }
-
-  devices {
-    id: "ume_left"
-    type: DEVICE_TYPE_ROBOT_ARM
-    config_file: "devices/arm/ume_arms.pb.txt"
-    # Hardware gate 1/2. Gate 2/2 is ume.hardware_enabled in the arm config.
-    enable: false
-  }
-}

cmvr-es/config/manager/task_manager.pb.txt

@@ -30,13 +30,4 @@ task_manager {
     # Host-development default: no QUIC Gateway or physical media devices.
     enable: true
   }
-  tasks {
-    id: "ume_teleop"
-    type: TASK_TYPE_UME_TELEOP
-    run_mode: TASK_RUN_MODE_BLOCKING_SERVICE
-    config_file: "tasks/ume_teleop_task/ume_teleop_task.pb.txt"
-    # Fail-safe default: configure the remote robot endpoint, manifest and
-    # deployment security policy before enabling this outbound control task.
-    enable: false
-  }
 }

cmvr-es/config/manager/task_manager_robot.pb.txt

@@ -1,14 +0,0 @@
-# Follower robot-side tasks.
-task_manager {
-  tasks {
-    id: "grpc_server"
-    type: TASK_TYPE_GRPC_SERVER
-    run_mode: TASK_RUN_MODE_BLOCKING_SERVICE
-    config_file: "tasks/grpc_server_task/grpc_server_task.pb.txt"
-    # The generic gRPC server may be enabled for integration. This does not
-    # enable a physical arm: device entries and the implemented RobotArm
-    # ArmTeleop adapter are explicitly disabled. Current MotorRobotArm
-    # sequential joint dispatch also fails the group-servo capability gate.
-    enable: true
-  }
-}

cmvr-es/config/manager/task_manager_ume.pb.txt

@@ -1,12 +0,0 @@
-# UME leader-side tasks.
-task_manager {
-  tasks {
-    id: "ume_teleop"
-    type: TASK_TYPE_UME_TELEOP
-    run_mode: TASK_RUN_MODE_BLOCKING_SERVICE
-    config_file: "tasks/ume_teleop_task/ume_teleop_task.pb.txt"
-    # Fail-closed: configure the follower endpoint, expected manifest and
-    # transport security before enabling outbound teleoperation.
-    enable: false
-  }
-}

cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt

@@ -5,22 +5,4 @@ grpc_server {
   enable_reflection: true
   camera_stream_max_pending_frames: 2
   camera_stream_max_frame_age_ms: 250
-
-  # The RobotArm adapter is implemented, but remains explicitly closed until
-  # the device itself enables teleop group servo, real hashes are provisioned,
-  # and group-write timing and independent stop behavior pass hardware review.
-  arm_teleop_backend {
-    enable: false
-    device_id: "right_arm"
-    # Deliberately empty placeholders are invalid when enable=true.
-    model_sha256: ""
-    calibration_sha256: ""
-    base_frame: "PELVIS_S"
-    tool_frame: "R_FINGER_TIP_FIXED"
-    servo_period_s: 0.001
-    max_apply_duration_us: 800
-    require_powered: true
-    max_initial_position_step_rad: 0.02
-    max_position_step_rad: 0.003
-  }
 }

cmvr-es/config/tasks/ume_teleop_task/ume_teleop_task.pb.txt

@@ -1,35 +0,0 @@
-ume_teleop {
-  id: "ume_teleop"
-
-  # Deliberately left empty. The TaskManager entry is disabled by default, and
-  # init fails closed if it is enabled before a robot endpoint is configured.
-  server_address: ""
-
-  # M6 implements explicit insecure transport for isolated development only.
-  # Production deployment must add and configure channel credentials first.
-  allow_insecure: false
-
-  open_session {
-    protocol_major: 1
-    protocol_minor: 0
-    client_instance_id: "ume-controller"
-    requested_command_rate_hz: 250
-    requested_state_rate_hz: 250
-    watchdog_timeout_ms: 100
-    requested_lease_ms: 500
-
-    # Replace with the manifest exported by the CMVR-ES robot instance.
-    expected_robot {
-      robot_id: ""
-      position_unit: "rad"
-      velocity_unit: "rad/s"
-      effort_unit: "N*m"
-    }
-  }
-
-  reconnect {
-    initial_delay_ms: 100
-    maximum_delay_ms: 5000
-    multiplier: 2.0
-  }
-}

cmvr-es/devices/arm/CMakeLists.txt

@@ -1,7 +1,6 @@
 add_subdirectory(motor_robot_arm)
 add_subdirectory(aubo_arm)
 add_subdirectory(huayan_arm)
-add_subdirectory(ume_robot_arm)
 
 add_library(robot_arm INTERFACE)
 
@@ -12,7 +11,6 @@ target_link_libraries(robot_arm
         cmvr_es::device::motor_robot_arm
         cmvr_es::device::aubo_arm
         cmvr_es::device::huayan_arm
-        cmvr_es::device::ume_robot_arm
         cmvr_es::proto
 )
 

cmvr-es/devices/arm/aubo_arm/CMakeLists.txt

@@ -46,54 +46,7 @@ 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()

cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp

@@ -1,15 +1,12 @@
 #include "devices/arm/aubo_arm/aubo_arm.h"
 
 #include <algorithm>
-#include <cctype>
 #include <chrono>
-#include <cstring>
 #include <exception>
 #include <thread>
 #include <tuple>
 
 #include "common/base/logging/logger.h"
-#include "json/json.h"
 
 #include "aubo_sdk/rpc.h"
 
@@ -47,103 +44,6 @@ 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<char>(std::tolower(c));
-    });
-    return value;
-}
-
-bool parseJsonCommand(const std::string& request_json,
-                      Json::Value& root,
-                      std::string& error)
-{
-    Json::CharReaderBuilder builder;
-    std::unique_ptr<Json::CharReader> 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<arcs::aubo_sdk::RpcClient>& rpc_client,
                                            const std::string& context,
                                            Result& result)
@@ -279,142 +179,6 @@ 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<int>(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;
@@ -1024,7 +788,6 @@ Result AuboArm::stopServoMode()
 
 Result AuboArm::connect(const std::string& ip, const int port)
 {
-    std::lock_guard lock(mutex_);
     if (connected_.load()) {
         return Result::success();
     }
@@ -1092,7 +855,6 @@ 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()) {

cmvr-es/devices/arm/aubo_arm/aubo_arm.h

@@ -21,8 +21,6 @@ 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; }

cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_json_command_test.cpp

@@ -1,84 +0,0 @@
-#include "devices/arm/aubo_arm/aubo_arm.h"
-
-#include <iostream>
-#include <string>
-
-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;
-}

cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h

@@ -36,13 +36,6 @@ public:
     RobotMode getRobotMode() const override { return RobotMode::Idle; }
     SafetyMode getSafetyMode() const override;
     ControlMode getControlMode() const override { return ControlMode::Position; }
-    bool supportsTeleopGroupServo() const noexcept override
-    {
-        // commandCyclicPosition is currently dispatched one joint at a time.
-        // A config switch cannot turn that partial-write behavior into the
-        // atomic/timed group primitive required by network teleoperation.
-        return false;
-    }
 
     Result torqueOn() override;
     Result torqueOff() override;
@@ -132,8 +125,6 @@ private:
 
     mutable std::mutex mutex_;
     std::atomic<bool> busy_{false};
-    std::atomic<bool> powered_on_{false};
-    mutable std::atomic<std::uint64_t> joint_state_sequence_{0};
     double speed_scaling_{1.0};
     bool emergency_stopped_{false};
     ServoOptions servo_options_;

cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp

@@ -1,7 +1,6 @@
 #include "arm/motor_robot_arm/include/motor_robot_arm.h"
 
 #include <chrono>
-#include <cmath>
 #include <Eigen/Dense>
 #include <stdexcept>
 #include <thread>
@@ -29,23 +28,6 @@ struct BusyGuard {
     ~BusyGuard() { busy.store(false); }
 };
 
-const config::JointLimitsConfig* configuredJointLimits(
-    const config::ArmKinematicsConfig& kinematics)
-{
-    switch (kinematics.algorithm_case()) {
-        case config::ArmKinematicsConfig::kPinocchioDlsIkSolver:
-            return &kinematics.pinocchio_dls_ik_solver()
-                        .joint_limit_policy()
-                        .limits();
-        case config::ArmKinematicsConfig::kPinocchioQpIkSolver:
-            return &kinematics.pinocchio_qp_ik_solver()
-                        .joint_limit_policy()
-                        .limits();
-        default:
-            return nullptr;
-    }
-}
-
 } // namespace
 
 MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg)
@@ -84,39 +66,6 @@ MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg)
     model_.manufacturer = "cmvr";
     model_.dof = static_cast<std::size_t>(dof_);
     model_.joint_names = joint_names_;
-
-    const auto* configured_limits = configuredJointLimits(cfg_.kinematics());
-    if (configured_limits != nullptr && configured_limits->enable() &&
-        configured_limits->source() ==
-            config::JOINT_LIMIT_SOURCE_CUSTOM &&
-        configured_limits->joints_size() == dof_) {
-        bool valid_limits = true;
-        model_.joint_limits.reserve(static_cast<std::size_t>(dof_));
-        for (int index = 0; index < dof_; ++index) {
-            const auto& source = configured_limits->joints(index);
-            if (source.joint_name() != joint_names_[static_cast<std::size_t>(index)] ||
-                !std::isfinite(source.q_lb()) ||
-                !std::isfinite(source.q_ub()) ||
-                !std::isfinite(source.qd()) ||
-                source.q_lb() >= source.q_ub() ||
-                source.qd() <= 0.0) {
-                valid_limits = false;
-                break;
-            }
-            JointLimit limit;
-            limit.lower = source.q_lb();
-            limit.upper = source.q_ub();
-            limit.max_velocity = source.qd();
-            limit.max_acceleration = source.qdd();
-            model_.joint_limits.push_back(limit);
-        }
-        if (!valid_limits) {
-            model_.joint_limits.clear();
-            CMVR_LOG(ERROR)
-                << "[MotorRobotArm] invalid or misordered custom joint limits: "
-                << id_;
-        }
-    }
 }
 
 MotorRobotArm::~MotorRobotArm()
@@ -185,7 +134,7 @@ ArmState MotorRobotArm::getRobotState() const
 {
     ArmState state;
     state.connected = motor_manager_ != nullptr;
-    state.powered_on = powered_on_.load(std::memory_order_acquire);
+    state.powered_on = true;
     state.brake_released = !emergency_stopped_;
     state.moving = busy();
     state.emergency_stopped = emergency_stopped_;
@@ -205,35 +154,15 @@ JointGroupState MotorRobotArm::getJointState() const
     state.position.reserve(joint_names_.size());
     state.velocity.reserve(joint_names_.size());
     state.effort.reserve(joint_names_.size());
-    bool values_valid = true;
     for (const auto& joint_name : joint_names_) {
         auto motor = getMotor_(joint_name);
         if (!motor) {
-            values_valid = false;
             continue;
         }
-        const double position = motor->getQ();
+        state.position.push_back(motor->getQ());
-        const double velocity = motor->getQd();
+        state.velocity.push_back(motor->getQd());
-        values_valid =
-            values_valid && std::isfinite(position) && std::isfinite(velocity);
-        state.position.push_back(position);
-        state.velocity.push_back(velocity);
         state.effort.push_back(0.0);
     }
-    values_valid =
-        values_valid && state.position.size() == joint_names_.size() &&
-        state.velocity.size() == joint_names_.size();
-    state.sequence =
-        joint_state_sequence_.fetch_add(1, std::memory_order_relaxed) + 1;
-    state.sample_monotonic_ns =
-        std::chrono::duration_cast<std::chrono::nanoseconds>(
-            std::chrono::steady_clock::now().time_since_epoch())
-            .count();
-    state.position_valid = values_valid;
-    state.velocity_valid = values_valid;
-    // MotorRobotArm currently has no verified effort feedback path. The zero
-    // placeholders above must never be advertised as measured torque.
-    state.effort_valid = false;
     return state;
 }
 
@@ -273,15 +202,11 @@ Result MotorRobotArm::torqueOn()
         }
     }
     emergency_stopped_ = false;
-    powered_on_.store(true, std::memory_order_release);
     return Result::success();
 }
 
 Result MotorRobotArm::torqueOff()
 {
-    // Until every joint reports a successful disable, the aggregate powered
-    // state is unknown and therefore must not satisfy a require_powered gate.
-    powered_on_.store(false, std::memory_order_release);
     for (const auto& joint_name : joint_names_) {
         auto motor = getMotor_(joint_name);
         if (!motor) {

cmvr-es/devices/arm/robot_arm.h

@@ -28,15 +28,6 @@ public:
     virtual SafetyMode getSafetyMode() const = 0;
     virtual ControlMode getControlMode() const = 0;
 
-    // ArmTeleop requires an explicitly reviewed group-servo implementation.
-    // Existing and vendor arms remain unavailable until their implementations
-    // override this capability after timing and partial-write validation.
-    virtual bool supportsTeleopGroupServo() const noexcept { return false; }
-    virtual JointEffortSource jointEffortSource() const noexcept
-    {
-        return JointEffortSource::Unspecified;
-    }
-
     virtual Result torqueOn() = 0;
     virtual Result torqueOff() = 0;
     virtual Result calibrateZeroQ(const std::string& joint_name) = 0;
@@ -82,27 +73,6 @@ public:
                                FrameType frame = FrameType::Base) = 0;
     virtual Result stopServoMode() = 0;
 
-    // Torque streaming is optional. Backends which do not provide an atomic
-    // group torque port retain source compatibility and fail explicitly.
-    virtual Result startTorqueMode(const TorqueServoOptions&)
-    {
-        return Result::failure(
-            ArmErrorCode::UnsupportedCommand,
-            "torque servo mode is unsupported by this RobotArm");
-    }
-    virtual Result servoTorque(const JointTorqueCommand&)
-    {
-        return Result::failure(
-            ArmErrorCode::UnsupportedCommand,
-            "torque servo command is unsupported by this RobotArm");
-    }
-    virtual Result stopTorqueMode()
-    {
-        return Result::failure(
-            ArmErrorCode::UnsupportedCommand,
-            "torque servo mode is unsupported by this RobotArm");
-    }
-
     virtual Result connect(const std::string& ip, int port) = 0;
     virtual Result disconnect() = 0;
     virtual bool isConnected() const = 0;

cmvr-es/devices/arm/robot_arm_factory.h

@@ -9,7 +9,6 @@
 #include "devices/arm/aubo_arm/aubo_arm.h"
 #include "devices/arm/huayan_arm/huayan_arm.h"
 #include "devices/arm/motor_robot_arm/include/motor_robot_arm.h"
-#include "devices/arm/ume_robot_arm/include/ume_robot_arm.h"
 
 namespace cmvr::device {
 
@@ -37,9 +36,6 @@ public:
             return nullptr;
         }
 
-        case config::RobotArmConfig::kUme:
-            return std::make_shared<UmeRobotArm>(cfg);
-
         case config::RobotArmConfig::BACKEND_NOT_SET:
         default:
         {

cmvr-es/devices/arm/ume_robot_arm/CMakeLists.txt

@@ -1,92 +0,0 @@
-add_library(ume_robot_arm SHARED
-        src/damiao_mit_codec.cpp
-        src/damiao_can_fd_chain.cpp
-        src/ume_robot_arm.cpp
-)
-
-target_include_directories(ume_robot_arm PUBLIC
-        ${CMAKE_CURRENT_SOURCE_DIR}
-)
-
-target_link_libraries(ume_robot_arm
-        PUBLIC
-        cmvr_es::device::canbus
-        cmvr_es::ik_solver
-        cmvr_es::common
-        PRIVATE
-        cmvr_es::proto
-        cmvr_es::logging
-        pthread
-)
-
-add_library(cmvr_es::device::ume_robot_arm ALIAS ume_robot_arm)
-install(TARGETS ume_robot_arm LIBRARY DESTINATION lib)
-
-if(BUILD_TESTING)
-    add_executable(damiao_mit_codec_test
-            tests/damiao_mit_codec_test.cpp
-    )
-    target_link_libraries(damiao_mit_codec_test
-            PRIVATE
-            cmvr_es::device::ume_robot_arm
-            gtest
-            gtest_main
-            pthread
-    )
-    add_test(
-            NAME damiao_mit_codec_test
-            COMMAND damiao_mit_codec_test
-    )
-    set(_ume_robot_arm_test_environment
-            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
-    )
-    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
-        list(APPEND _ume_robot_arm_test_environment
-                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
-    endif()
-    set_tests_properties(damiao_mit_codec_test PROPERTIES
-            TIMEOUT 10
-            ENVIRONMENT "${_ume_robot_arm_test_environment}"
-    )
-
-    add_executable(damiao_can_fd_chain_test
-            tests/damiao_can_fd_chain_test.cpp
-    )
-    target_link_libraries(damiao_can_fd_chain_test
-            PRIVATE
-            cmvr_es::device::ume_robot_arm
-            gtest
-            gtest_main
-            pthread
-    )
-    add_test(
-            NAME damiao_can_fd_chain_test
-            COMMAND damiao_can_fd_chain_test
-    )
-    set_tests_properties(damiao_can_fd_chain_test PROPERTIES
-            TIMEOUT 10
-            ENVIRONMENT "${_ume_robot_arm_test_environment}"
-    )
-
-    add_executable(ume_robot_arm_test
-            tests/ume_robot_arm_test.cpp
-    )
-    target_link_libraries(ume_robot_arm_test
-            PRIVATE
-            cmvr_es::device::ume_robot_arm
-            gtest
-            gtest_main
-            pthread
-    )
-    target_compile_definitions(ume_robot_arm_test PRIVATE
-            CMVR_UME_ARM_CONFIG_PATH="${PROJECT_SOURCE_DIR}/cmvr-es/config/devices/arm/ume_arms.pb.txt"
-    )
-    add_test(
-            NAME ume_robot_arm_test
-            COMMAND ume_robot_arm_test
-    )
-    set_tests_properties(ume_robot_arm_test PROPERTIES
-            TIMEOUT 10
-            ENVIRONMENT "${_ume_robot_arm_test_environment}"
-    )
-endif()

cmvr-es/devices/arm/ume_robot_arm/include/damiao_can_fd_chain.h

@@ -1,145 +0,0 @@
-#ifndef CMVR_ES_DAMIAO_CAN_FD_CHAIN_H
-#define CMVR_ES_DAMIAO_CAN_FD_CHAIN_H
-
-#include <atomic>
-#include <chrono>
-#include <cstddef>
-#include <cstdint>
-#include <memory>
-#include <mutex>
-#include <string>
-#include <vector>
-
-#include "arm/ume_robot_arm/include/damiao_mit_codec.h"
-#include "common/types/arm/arm_types.h"
-
-namespace cmvr::device {
-
-class AbstractCanbus;
-
-struct DamiaoJointSpec {
-    std::string joint_name;
-    std::uint32_t command_id{0};
-    std::uint32_t feedback_id{0};
-    std::uint8_t reported_motor_id{0};
-    DamiaoMotorModel model{DamiaoMotorModel::Unknown};
-    int direction{1};
-    double zero_offset_rad{0.0};
-    double joint_lower_rad{0.0};
-    double joint_upper_rad{0.0};
-    double max_velocity_rad_s{0.0};
-    double max_torque_nm{0.0};
-    std::uint16_t healthy_status_mask{0};
-    std::uint8_t max_driver_temperature_raw{0};
-    std::uint8_t max_motor_temperature_raw{0};
-};
-
-struct DamiaoChainOptions {
-    bool is_fd{true};
-    bool bitrate_switch{true};
-    bool hardware_enabled{false};
-};
-
-struct DamiaoChainStatistics {
-    std::uint64_t exchanges{0};
-    std::uint64_t deadline_misses{0};
-    std::uint64_t unknown_feedback{0};
-    std::uint64_t duplicate_feedback{0};
-    std::uint64_t rejected_commands{0};
-    std::uint64_t protocol_saturations{0};
-};
-
-enum class DamiaoChainState : std::uint8_t {
-    Closed = 0,
-    Initialized,
-    Passive,
-    Armed,
-    Active,
-    FaultLatched,
-    Stopped
-};
-
-class DamiaoCanFdChain {
-public:
-    DamiaoCanFdChain(std::shared_ptr<AbstractCanbus> bus,
-                     std::vector<DamiaoJointSpec> joints,
-                     DamiaoChainOptions options);
-    ~DamiaoCanFdChain();
-
-    DamiaoCanFdChain(const DamiaoCanFdChain&) = delete;
-    DamiaoCanFdChain& operator=(const DamiaoCanFdChain&) = delete;
-
-    Result init();
-    Result openPassive();
-    Result clearFault(std::chrono::steady_clock::time_point deadline);
-    Result arm(std::chrono::steady_clock::time_point deadline);
-    Result setZero(std::size_t joint_index,
-                   std::chrono::steady_clock::time_point deadline);
-    Result exchange(const DamiaoMitCommand* joint_commands,
-                    std::size_t command_count,
-                    DamiaoJointFeedback* joint_feedback,
-                    std::size_t feedback_count,
-                    std::chrono::steady_clock::time_point deadline);
-    Result disable() noexcept;
-    Result latchFault(const std::string& reason) noexcept;
-    void stop() noexcept;
-
-    DamiaoChainState state() const noexcept { return state_.load(); }
-    std::size_t size() const noexcept { return joints_.size(); }
-    bool hardwareEnabled() const noexcept { return options_.hardware_enabled; }
-    DamiaoChainStatistics statistics() const;
-    std::string lastError() const;
-    const std::vector<DamiaoJointSpec>& joints() const noexcept { return joints_; }
-
-private:
-    Result validateConfig_() const;
-    Result sendModeAll_(
-        DamiaoMode mode,
-        std::chrono::steady_clock::time_point deadline,
-        bool expect_feedback);
-    Result sendModeOne_(
-        std::size_t joint_index,
-        DamiaoMode mode,
-        std::chrono::steady_clock::time_point deadline);
-    Result receiveCycle_(
-        DamiaoJointFeedback* feedback,
-        std::size_t feedback_count,
-        std::chrono::steady_clock::time_point deadline);
-    bool sendFrames_(
-        const std::vector<CanFrame>& frames,
-        std::chrono::steady_clock::time_point deadline) noexcept;
-    bool sendFramesBestEffort_(
-        const std::vector<CanFrame>& frames) noexcept;
-    bool feedbackTransportAndHealthValid_(
-        const CanFrame& frame,
-        const DamiaoJointSpec& joint,
-        const DamiaoJointFeedback& feedback) const noexcept;
-    bool latchFaultAndDisable_(const std::string& reason) noexcept;
-    bool bestEffortZeroAndDisable_() noexcept;
-    void setError_(const std::string& error) noexcept;
-    std::size_t jointIndexForFeedbackId_(std::uint32_t id) const noexcept;
-    DamiaoMitCommand toMotorCommand_(
-        const DamiaoJointSpec& spec,
-        const DamiaoMitCommand& command,
-        bool& safety_saturated) const noexcept;
-    void toJointFeedback_(const DamiaoJointSpec& spec,
-                          DamiaoJointFeedback& feedback) const noexcept;
-
-    std::shared_ptr<AbstractCanbus> bus_;
-    std::vector<DamiaoJointSpec> joints_;
-    DamiaoChainOptions options_;
-    std::vector<CanFrame> tx_frames_;
-    std::vector<CanFrame> rx_frames_;
-    std::vector<DamiaoJointFeedback> feedback_scratch_;
-    std::vector<bool> feedback_seen_;
-
-    mutable std::mutex io_mutex_;
-    mutable std::mutex status_mutex_;
-    std::atomic<DamiaoChainState> state_{DamiaoChainState::Closed};
-    DamiaoChainStatistics statistics_;
-    std::string last_error_;
-};
-
-} // namespace cmvr::device
-
-#endif // CMVR_ES_DAMIAO_CAN_FD_CHAIN_H

cmvr-es/devices/arm/ume_robot_arm/include/damiao_mit_codec.h

@@ -1,135 +0,0 @@
-#ifndef CMVR_ES_DAMIAO_MIT_CODEC_H
-#define CMVR_ES_DAMIAO_MIT_CODEC_H
-
-#include <cstdint>
-
-#include "canbus/abstract_canbus.h"
-
-namespace cmvr::device {
-
-enum class DamiaoMotorModel : std::uint8_t {
-    Unknown = 0,
-    DM4310,
-    DM4310_48V,
-    DM4340,
-    DM4340_48V,
-    DM6006,
-    DM8006,
-    DM8009,
-    DM10010L,
-    DM10010,
-    DMH3510,
-    DMH6215,
-    DMG6220
-};
-
-struct DamiaoMotorLimits {
-    double q_max_rad{0.0};
-    double dq_max_rad_s{0.0};
-    double tau_max_nm{0.0};
-
-    bool valid() const noexcept;
-};
-
-struct DamiaoMitCommand {
-    double q_rad{0.0};
-    double dq_rad_s{0.0};
-    double kp{0.0};
-    double kd{0.0};
-    double tau_ff_nm{0.0};
-};
-
-enum DamiaoSaturation : std::uint8_t {
-    DAMIAO_SATURATION_NONE = 0,
-    DAMIAO_SATURATION_Q = 1U << 0U,
-    DAMIAO_SATURATION_DQ = 1U << 1U,
-    DAMIAO_SATURATION_KP = 1U << 2U,
-    DAMIAO_SATURATION_KD = 1U << 3U,
-    DAMIAO_SATURATION_TAU = 1U << 4U
-};
-
-enum class DamiaoCodecError : std::uint8_t {
-    None = 0,
-    UnknownModel,
-    InvalidLimits,
-    NonFiniteInput,
-    InvalidCanId,
-    InvalidFrame,
-    UnexpectedFeedbackId
-};
-
-struct DamiaoEncodeResult {
-    DamiaoCodecError error{DamiaoCodecError::None};
-    std::uint8_t saturation_mask{DAMIAO_SATURATION_NONE};
-
-    explicit operator bool() const noexcept
-    {
-        return error == DamiaoCodecError::None;
-    }
-};
-
-struct DamiaoJointFeedback {
-    std::uint8_t reported_motor_id{0};
-    std::uint8_t status{0};
-    std::uint8_t driver_temperature_raw{0};
-    std::uint8_t motor_temperature_raw{0};
-    double q_rad{0.0};
-    double dq_rad_s{0.0};
-    double tau_nm{0.0};
-    std::int64_t rx_monotonic_ns{0};
-    bool valid{false};
-};
-
-enum class DamiaoMode : std::uint8_t {
-    ClearFault,
-    Enable,
-    Disable,
-    SetZero
-};
-
-class DamiaoMitCodec {
-public:
-    static constexpr double kKpMax = 500.0;
-    static constexpr double kKdMax = 5.0;
-
-    static DamiaoMotorLimits limitsFor(DamiaoMotorModel model) noexcept;
-
-    static DamiaoEncodeResult encodeMit(
-        std::uint32_t command_id,
-        DamiaoMotorModel model,
-        const DamiaoMitCommand& command,
-        bool is_fd,
-        bool bitrate_switch,
-        CanFrame& frame) noexcept;
-
-    static DamiaoCodecError decodeFeedback(
-        const CanFrame& frame,
-        std::uint32_t expected_feedback_id,
-        DamiaoMotorModel model,
-        DamiaoJointFeedback& feedback) noexcept;
-
-    static DamiaoCodecError encodeMode(
-        std::uint32_t command_id,
-        DamiaoMode mode,
-        bool is_fd,
-        bool bitrate_switch,
-        CanFrame& frame) noexcept;
-
-    // Public for protocol golden-vector tests. The unusual +1 decode behavior
-    // intentionally matches the legacy UME Python implementation.
-    static std::uint16_t floatToUint(
-        double value,
-        double minimum,
-        double maximum,
-        unsigned bits,
-        bool& saturated) noexcept;
-    static double uintToFloat(
-        std::uint16_t value,
-        double minimum,
-        double maximum,
-        unsigned bits) noexcept;
-};
-
-} // namespace cmvr::device
-
-#endif // CMVR_ES_DAMIAO_MIT_CODEC_H

cmvr-es/devices/arm/ume_robot_arm/include/ume_robot_arm.h

@@ -1,181 +0,0 @@
-#ifndef CMVR_ES_UME_ROBOT_ARM_H
-#define CMVR_ES_UME_ROBOT_ARM_H
-
-#include <array>
-#include <atomic>
-#include <cstddef>
-#include <cstdint>
-#include <memory>
-#include <mutex>
-#include <string>
-#include <thread>
-#include <vector>
-
-#include "arm/robot_arm.h"
-#include "arm/ume_robot_arm/include/damiao_can_fd_chain.h"
-#include "cmvr/config/arm_config/arm_config.pb.h"
-
-namespace cmvr::device {
-
-class AbstractCanbus;
-
-struct UmeArmSample {
-    static constexpr std::size_t kDof = 8;
-
-    std::uint64_t sequence{0};
-    std::int64_t sample_monotonic_ns{0};
-    std::array<double, kDof> q{};
-    std::array<double, kDof> dq{};
-    std::array<double, kDof> tau_measured{};
-    std::array<std::int64_t, kDof> motor_rx_time_ns{};
-    std::uint8_t valid_mask{0};
-};
-
-// One UmeRobotArm represents one physical eight-axis leader arm and one
-// SocketCAN-FD interface. The class owns its local high-frequency actuator
-// loop; networking and follower kinematics remain outside this device.
-class UmeRobotArm final : public RobotArm {
-public:
-    explicit UmeRobotArm(const config::RobotArmConfig& cfg);
-    UmeRobotArm(const config::RobotArmConfig& cfg,
-                std::shared_ptr<AbstractCanbus> canbus);
-    ~UmeRobotArm() override;
-
-    std::string typeName() const override { return "UmeRobotArm"; }
-    bool init() override;
-    bool start() override;
-    bool stop() override;
-    DeviceHealthSnapshot healthSnapshot() override;
-
-    RobotModel getRobotModel() const override { return model_; }
-    std::size_t getDof() const override { return UmeArmSample::kDof; }
-    ArmState getRobotState() const override;
-    JointGroupState getJointState() const override;
-    Result readSample(UmeArmSample& sample) const;
-    CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
-    RobotMode getRobotMode() const override;
-    SafetyMode getSafetyMode() const override;
-    ControlMode getControlMode() const override;
-
-    Result torqueOn() override;
-    Result torqueOff() override;
-    Result calibrateZeroQ(const std::string& joint_name) override;
-
-    Result emergencyStop() override;
-    Result protectiveStop() override;
-    Result setSpeedScaling(double scaling) override;
-    double getSpeedScaling() const override { return 1.0; }
-    bool isProtectiveStopped() const override
-    {
-        return protective_stopped_.load();
-    }
-    bool isEmergencyStopped() const override
-    {
-        return emergency_stopped_.load();
-    }
-    bool isFault() const override { return fault_latched_.load(); }
-
-    Result moveJ(const JointPositionCommand& target,
-                 const MotionOptions& options) override;
-    Result speedJ(const JointVelocityCommand& velocity,
-                  double acceleration,
-                  double duration) override;
-    Result stopJ(double acceleration) override;
-    Result moveL(const CartesianPose& target,
-                 const MotionOptions& options,
-                 FrameType frame = FrameType::Base) override;
-    Result speedL(const CartesianVelocity& velocity,
-                  double acceleration,
-                  double duration,
-                  FrameType frame = FrameType::Base) override;
-    Result stopL(std::optional<double> acceleration = std::nullopt) override;
-    Result stopMotion() override;
-
-    Result startServoMode(const ServoOptions& options) override;
-    Result servoJ(const JointPositionCommand& target) override;
-    Result servoL(const CartesianPose& target,
-                  FrameType frame = FrameType::Base) override;
-    Result servoSpeedJ(const JointVelocityCommand& velocity) override;
-    Result servoSpeedL(const CartesianVelocity& velocity,
-                       FrameType frame = FrameType::Base) override;
-    Result stopServoMode() override;
-
-    Result startTorqueMode(const TorqueServoOptions& options) override;
-    Result servoTorque(const JointTorqueCommand& target) override;
-    Result stopTorqueMode() override;
-
-    Result connect(const std::string& ip, int port) override;
-    Result disconnect() override;
-    bool isConnected() const override { return initialized_.load(); }
-    Result powerOn() override { return torqueOn(); }
-    Result powerOff() override { return torqueOff(); }
-    Result brakeRelease() override;
-    Result shutdown() override;
-    Result clearFault() override;
-    Result unlockProtectiveStop() override;
-    Result loadProgram(const std::string& program_name) override;
-    Result playProgram() override;
-    Result pauseProgram() override;
-    Result stopProgram() override;
-
-    std::vector<double> ik(const std::string& base_link,
-                           const std::string& ee_link,
-                           const CartesianPose& pose) override;
-    std::shared_ptr<cmvr::IKSolver> kinematicsSolver() const override
-    {
-        return ik_solver_;
-    }
-    CartesianPose fk(const std::string& base_link,
-                     const std::string& ee_link) override;
-    CartesianPose fk(bool is_tcp = true) override;
-    CartesianVelocity getSpeedLCommandTwistBase() const override { return {}; }
-    bool busy() const override { return powered_on_.load(); }
-
-private:
-    void normalizeConfig_();
-    bool buildModelAndChain_();
-    void controlLoop_() noexcept;
-    void recordFault_(const std::string& message) noexcept;
-    Result requirePassive_(const std::string& operation) const;
-    static Result unsupported_(const std::string& operation);
-    static std::int64_t monotonicNowNs_() noexcept;
-
-    config::RobotArmConfig cfg_;
-    config::UmeRobotArmBackendConfig ume_cfg_;
-    std::shared_ptr<AbstractCanbus> canbus_;
-    std::unique_ptr<DamiaoCanFdChain> chain_;
-    std::vector<DamiaoJointSpec> joint_specs_;
-    RobotModel model_;
-    std::shared_ptr<cmvr::IKSolver> ik_solver_;
-
-    mutable std::mutex lifecycle_mutex_;
-    mutable std::mutex command_mutex_;
-    mutable std::mutex sample_mutex_;
-    mutable std::mutex status_mutex_;
-    mutable std::mutex kinematics_mutex_;
-    std::thread control_thread_;
-
-    std::array<double, UmeArmSample::kDof> latest_torque_command_{};
-    UmeArmSample latest_sample_;
-    std::string last_error_;
-
-    std::atomic<bool> initialized_{false};
-    std::atomic<bool> running_{false};
-    std::atomic<bool> torque_mode_{false};
-    std::atomic<bool> powered_on_{false};
-    std::atomic<bool> fault_latched_{false};
-    std::atomic<bool> protective_stopped_{false};
-    std::atomic<bool> emergency_stopped_{false};
-    std::atomic<bool> command_ready_{false};
-    std::atomic<std::uint64_t> command_sequence_{0};
-    std::atomic<std::int64_t> command_time_ns_{0};
-    std::atomic<std::int64_t> loop_period_ns_{1250000};
-    std::atomic<std::uint32_t> command_watchdog_ms_{20};
-    std::uint32_t cycle_deadline_us_{900};
-    std::uint32_t feedback_watchdog_ms_{20};
-    std::uint32_t shutdown_timeout_ms_{50};
-};
-
-} // namespace cmvr::device
-
-#endif // CMVR_ES_UME_ROBOT_ARM_H

cmvr-es/devices/arm/ume_robot_arm/src/damiao_can_fd_chain.cpp

@@ -1,705 +0,0 @@
-#include "arm/ume_robot_arm/include/damiao_can_fd_chain.h"
-
-#include <algorithm>
-#include <cmath>
-#include <limits>
-#include <unordered_set>
-
-#include "canbus/abstract_canbus.h"
-
-namespace cmvr::device {
-namespace {
-
-Result invalidArgument(const std::string& message)
-{
-    return Result::failure(ArmErrorCode::InvalidArgument, message);
-}
-
-Result commandFailed(const std::string& message)
-{
-    return Result::failure(ArmErrorCode::CommandFailed, message);
-}
-
-Result notReady(const std::string& message)
-{
-    return Result::failure(ArmErrorCode::RobotNotReady, message);
-}
-
-} // namespace
-
-DamiaoCanFdChain::DamiaoCanFdChain(
-    std::shared_ptr<AbstractCanbus> bus,
-    std::vector<DamiaoJointSpec> joints,
-    DamiaoChainOptions options)
-    : bus_(std::move(bus)),
-      joints_(std::move(joints)),
-      options_(options),
-      feedback_scratch_(joints_.size()),
-      feedback_seen_(joints_.size(), false)
-{
-    tx_frames_.reserve(joints_.size());
-    rx_frames_.reserve(1);
-}
-
-DamiaoCanFdChain::~DamiaoCanFdChain()
-{
-    stop();
-}
-
-Result DamiaoCanFdChain::validateConfig_() const
-{
-    if (!bus_) {
-        return invalidArgument("Damiao CAN bus is null");
-    }
-    if (joints_.empty()) {
-        return invalidArgument("Damiao joint list is empty");
-    }
-
-    std::unordered_set<std::string> names;
-    std::unordered_set<std::uint32_t> command_ids;
-    std::unordered_set<std::uint32_t> feedback_ids;
-    std::unordered_set<std::uint32_t> reported_ids;
-    for (const auto& joint : joints_) {
-        if (joint.joint_name.empty() ||
-            !names.insert(joint.joint_name).second) {
-            return invalidArgument("Damiao joint names must be non-empty and unique");
-        }
-        if (joint.command_id == 0 || joint.command_id > 0x7FFU ||
-            !command_ids.insert(joint.command_id).second) {
-            return invalidArgument("Damiao command IDs must be unique standard CAN IDs");
-        }
-        if (joint.feedback_id == 0 || joint.feedback_id > 0x7FFU ||
-            !feedback_ids.insert(joint.feedback_id).second) {
-            return invalidArgument("Damiao feedback IDs must be unique standard CAN IDs");
-        }
-        if (joint.reported_motor_id > 0x0FU ||
-            !reported_ids.insert(joint.reported_motor_id).second) {
-            return invalidArgument("Damiao reported motor IDs must be unique 4-bit values");
-        }
-        if (!DamiaoMitCodec::limitsFor(joint.model).valid()) {
-            return invalidArgument("Damiao motor model is unknown");
-        }
-        if (joint.direction != 1 && joint.direction != -1) {
-            return invalidArgument("Damiao joint direction must be +1 or -1");
-        }
-        if (!std::isfinite(joint.zero_offset_rad) ||
-            !std::isfinite(joint.joint_lower_rad) ||
-            !std::isfinite(joint.joint_upper_rad) ||
-            joint.joint_upper_rad <= joint.joint_lower_rad ||
-            !std::isfinite(joint.max_velocity_rad_s) ||
-            joint.max_velocity_rad_s <= 0.0 ||
-            !std::isfinite(joint.max_torque_nm) ||
-            joint.max_torque_nm <= 0.0) {
-            return invalidArgument("Damiao mechanical limits are invalid");
-        }
-        if (options_.hardware_enabled &&
-            (joint.healthy_status_mask == 0U ||
-             joint.max_driver_temperature_raw == 0U ||
-             joint.max_motor_temperature_raw == 0U)) {
-            return invalidArgument(
-                "Damiao hardware enable requires a reviewed feedback-status "
-                "whitelist and nonzero raw temperature thresholds");
-        }
-    }
-    return Result::success();
-}
-
-Result DamiaoCanFdChain::init()
-{
-    std::lock_guard lock(io_mutex_);
-    const auto config_result = validateConfig_();
-    if (!config_result.ok()) {
-        setError_(config_result.message);
-        state_.store(DamiaoChainState::FaultLatched);
-        return config_result;
-    }
-    if (state_.load() != DamiaoChainState::Closed &&
-        state_.load() != DamiaoChainState::Stopped) {
-        return Result::success();
-    }
-    if (!bus_->init()) {
-        setError_("failed to initialize Damiao CAN bus");
-        state_.store(DamiaoChainState::FaultLatched);
-        return notReady(lastError());
-    }
-    state_.store(DamiaoChainState::Initialized);
-    return Result::success();
-}
-
-Result DamiaoCanFdChain::openPassive()
-{
-    std::lock_guard lock(io_mutex_);
-    if (state_.load() != DamiaoChainState::Initialized) {
-        return notReady("Damiao chain is not initialized");
-    }
-    if (!bus_->start()) {
-        setError_("failed to start Damiao CAN bus");
-        state_.store(DamiaoChainState::FaultLatched);
-        return notReady(lastError());
-    }
-    // Deliberately no clear-fault or enable command here.
-    state_.store(DamiaoChainState::Passive);
-    return Result::success();
-}
-
-Result DamiaoCanFdChain::clearFault(
-    const std::chrono::steady_clock::time_point deadline)
-{
-    std::lock_guard lock(io_mutex_);
-    if (!options_.hardware_enabled) {
-        return Result::failure(
-            ArmErrorCode::CommandRejected,
-            "Damiao hardware commands are disabled by configuration");
-    }
-    const auto current = state_.load();
-    if (current != DamiaoChainState::Passive &&
-        current != DamiaoChainState::FaultLatched) {
-        return notReady("clearFault requires a disabled Damiao chain");
-    }
-    const auto result = sendModeAll_(
-        DamiaoMode::ClearFault, deadline, true);
-    if (!result.ok()) {
-        state_.store(DamiaoChainState::FaultLatched);
-        return result;
-    }
-    // Clearing a fault never arms the motors.
-    state_.store(DamiaoChainState::Passive);
-    return Result::success();
-}
-
-Result DamiaoCanFdChain::arm(
-    const std::chrono::steady_clock::time_point deadline)
-{
-    std::lock_guard lock(io_mutex_);
-    if (!options_.hardware_enabled) {
-        return Result::failure(
-            ArmErrorCode::CommandRejected,
-            "Damiao hardware commands are disabled by configuration");
-    }
-    if (state_.load() != DamiaoChainState::Passive) {
-        return notReady("Damiao chain must be passive before arm");
-    }
-    const auto result = sendModeAll_(DamiaoMode::Enable, deadline, true);
-    if (!result.ok()) {
-        latchFaultAndDisable_(result.message);
-        return Result::failure(result.code, lastError());
-    }
-    state_.store(DamiaoChainState::Armed);
-    return Result::success();
-}
-
-Result DamiaoCanFdChain::setZero(
-    const std::size_t joint_index,
-    const std::chrono::steady_clock::time_point deadline)
-{
-    std::lock_guard lock(io_mutex_);
-    if (!options_.hardware_enabled) {
-        return Result::failure(
-            ArmErrorCode::CommandRejected,
-            "Damiao hardware commands are disabled by configuration");
-    }
-    if (state_.load() != DamiaoChainState::Passive) {
-        return notReady("setZero requires a passive Damiao chain");
-    }
-    return sendModeOne_(joint_index, DamiaoMode::SetZero, deadline);
-}
-
-DamiaoMitCommand DamiaoCanFdChain::toMotorCommand_(
-    const DamiaoJointSpec& spec,
-    const DamiaoMitCommand& command,
-    bool& safety_saturated) const noexcept
-{
-    DamiaoMitCommand motor = command;
-    safety_saturated = false;
-
-    const double limited_q =
-        std::clamp(command.q_rad, spec.joint_lower_rad, spec.joint_upper_rad);
-    const double limited_dq =
-        std::clamp(command.dq_rad_s,
-                   -spec.max_velocity_rad_s, spec.max_velocity_rad_s);
-    const double limited_tau =
-        std::clamp(command.tau_ff_nm,
-                   -spec.max_torque_nm, spec.max_torque_nm);
-    safety_saturated =
-        limited_q != command.q_rad ||
-        limited_dq != command.dq_rad_s ||
-        limited_tau != command.tau_ff_nm;
-
-    motor.q_rad =
-        spec.direction * (limited_q - spec.zero_offset_rad);
-    motor.dq_rad_s = spec.direction * limited_dq;
-    motor.tau_ff_nm = spec.direction * limited_tau;
-    return motor;
-}
-
-void DamiaoCanFdChain::toJointFeedback_(
-    const DamiaoJointSpec& spec,
-    DamiaoJointFeedback& feedback) const noexcept
-{
-    feedback.q_rad =
-        spec.direction * feedback.q_rad + spec.zero_offset_rad;
-    feedback.dq_rad_s = spec.direction * feedback.dq_rad_s;
-    feedback.tau_nm = spec.direction * feedback.tau_nm;
-}
-
-Result DamiaoCanFdChain::exchange(
-    const DamiaoMitCommand* joint_commands,
-    const std::size_t command_count,
-    DamiaoJointFeedback* joint_feedback,
-    const std::size_t feedback_count,
-    const std::chrono::steady_clock::time_point deadline)
-{
-    std::lock_guard lock(io_mutex_);
-    if (!joint_commands || !joint_feedback ||
-        command_count != joints_.size() ||
-        feedback_count != joints_.size()) {
-        {
-            std::lock_guard status_lock(status_mutex_);
-            ++statistics_.rejected_commands;
-        }
-        return invalidArgument("Damiao exchange dimensions do not match configured joints");
-    }
-    const auto current = state_.load();
-    if (current != DamiaoChainState::Armed &&
-        current != DamiaoChainState::Active) {
-        return notReady("Damiao chain is not armed");
-    }
-    if (std::chrono::steady_clock::now() >= deadline) {
-        {
-            std::lock_guard status_lock(status_mutex_);
-            ++statistics_.deadline_misses;
-        }
-        latchFaultAndDisable_(
-            "Damiao exchange deadline expired before send");
-        return Result::failure(ArmErrorCode::Timeout, lastError());
-    }
-
-    tx_frames_.clear();
-    std::uint64_t saturation_count = 0;
-    for (std::size_t i = 0; i < joints_.size(); ++i) {
-        bool safety_saturated = false;
-        const auto motor_command =
-            toMotorCommand_(joints_[i], joint_commands[i], safety_saturated);
-        CanFrame frame;
-        const auto encoded = DamiaoMitCodec::encodeMit(
-            joints_[i].command_id, joints_[i].model, motor_command,
-            options_.is_fd, options_.bitrate_switch, frame);
-        if (!encoded) {
-            {
-                std::lock_guard status_lock(status_mutex_);
-                ++statistics_.rejected_commands;
-            }
-            return invalidArgument("Damiao command failed protocol validation");
-        }
-        if (safety_saturated ||
-            encoded.saturation_mask != DAMIAO_SATURATION_NONE) {
-            ++saturation_count;
-        }
-        tx_frames_.push_back(frame);
-    }
-    if (!bus_->discardPendingFrames()) {
-        latchFaultAndDisable_(
-            "failed to drain stale Damiao feedback before command");
-        return commandFailed(lastError());
-    }
-    if (std::chrono::steady_clock::now() >= deadline) {
-        {
-            std::lock_guard status_lock(status_mutex_);
-            ++statistics_.deadline_misses;
-        }
-        latchFaultAndDisable_(
-            "Damiao exchange deadline expired before command commit");
-        return Result::failure(ArmErrorCode::Timeout, lastError());
-    }
-    if (!sendFrames_(tx_frames_, deadline)) {
-        latchFaultAndDisable_(
-            "failed to send Damiao MIT command batch before deadline");
-        return commandFailed(lastError());
-    }
-    if (std::chrono::steady_clock::now() >= deadline) {
-        {
-            std::lock_guard status_lock(status_mutex_);
-            ++statistics_.deadline_misses;
-        }
-        latchFaultAndDisable_(
-            "Damiao MIT command batch exceeded its deadline");
-        return Result::failure(ArmErrorCode::Timeout, lastError());
-    }
-
-    const auto receive_result =
-        receiveCycle_(joint_feedback, feedback_count, deadline);
-    {
-        std::lock_guard status_lock(status_mutex_);
-        ++statistics_.exchanges;
-        statistics_.protocol_saturations += saturation_count;
-    }
-    if (!receive_result.ok()) {
-        latchFaultAndDisable_(receive_result.message);
-        return Result::failure(receive_result.code, lastError());
-    }
-    state_.store(DamiaoChainState::Active);
-    return Result::success();
-}
-
-Result DamiaoCanFdChain::sendModeAll_(
-    const DamiaoMode mode,
-    const std::chrono::steady_clock::time_point deadline,
-    const bool expect_feedback)
-{
-    tx_frames_.clear();
-    for (const auto& joint : joints_) {
-        CanFrame frame;
-        const auto error = DamiaoMitCodec::encodeMode(
-            joint.command_id, mode, options_.is_fd,
-            options_.bitrate_switch, frame);
-        if (error != DamiaoCodecError::None) {
-            return invalidArgument("failed to encode Damiao lifecycle command");
-        }
-        tx_frames_.push_back(frame);
-    }
-    if (expect_feedback && !bus_->discardPendingFrames()) {
-        return commandFailed(
-            "failed to drain stale Damiao lifecycle feedback");
-    }
-    if (std::chrono::steady_clock::now() >= deadline) {
-        return Result::failure(
-            ArmErrorCode::Timeout,
-            "Damiao lifecycle deadline expired before command commit");
-    }
-    if (!sendFrames_(tx_frames_, deadline)) {
-        return commandFailed(
-            "failed to send Damiao lifecycle command before deadline");
-    }
-    if (std::chrono::steady_clock::now() >= deadline) {
-        return Result::failure(
-            ArmErrorCode::Timeout,
-            "Damiao lifecycle command exceeded its deadline");
-    }
-    if (!expect_feedback) {
-        return Result::success();
-    }
-    return receiveCycle_(
-        feedback_scratch_.data(), feedback_scratch_.size(), deadline);
-}
-
-Result DamiaoCanFdChain::sendModeOne_(
-    const std::size_t joint_index,
-    const DamiaoMode mode,
-    const std::chrono::steady_clock::time_point deadline)
-{
-    if (joint_index >= joints_.size()) {
-        return invalidArgument("Damiao joint index is out of range");
-    }
-    CanFrame frame;
-    const auto error = DamiaoMitCodec::encodeMode(
-        joints_[joint_index].command_id, mode, options_.is_fd,
-        options_.bitrate_switch, frame);
-    if (error != DamiaoCodecError::None) {
-        return invalidArgument("failed to encode Damiao lifecycle command");
-    }
-    tx_frames_.assign(1, frame);
-    if (!bus_->discardPendingFrames()) {
-        return commandFailed(
-            "failed to drain stale Damiao lifecycle feedback");
-    }
-    if (std::chrono::steady_clock::now() >= deadline) {
-        return Result::failure(
-            ArmErrorCode::Timeout,
-            "Damiao lifecycle deadline expired before command commit");
-    }
-    if (!sendFrames_(tx_frames_, deadline)) {
-        return commandFailed(
-            "failed to send Damiao lifecycle command before deadline");
-    }
-    if (std::chrono::steady_clock::now() >= deadline) {
-        return Result::failure(
-            ArmErrorCode::Timeout,
-            "Damiao lifecycle command exceeded its deadline");
-    }
-
-    std::fill(feedback_seen_.begin(), feedback_seen_.end(), false);
-    while (std::chrono::steady_clock::now() < deadline) {
-        rx_frames_.clear();
-        int32_t count = 1;
-        if (bus_->receive(&rx_frames_, &count) != msgs::ErrorCode::OK) {
-            continue;
-        }
-        for (const auto& received : rx_frames_) {
-            if (received.id != joints_[joint_index].feedback_id) {
-                continue;
-            }
-            DamiaoJointFeedback feedback;
-            if (DamiaoMitCodec::decodeFeedback(
-                    received, joints_[joint_index].feedback_id,
-                    joints_[joint_index].model, feedback) !=
-                    DamiaoCodecError::None ||
-                feedback.reported_motor_id !=
-                    joints_[joint_index].reported_motor_id ||
-                !feedbackTransportAndHealthValid_(
-                    received, joints_[joint_index], feedback)) {
-                return commandFailed("invalid Damiao lifecycle feedback");
-            }
-            return Result::success();
-        }
-    }
-    return Result::failure(
-        ArmErrorCode::Timeout, "Damiao lifecycle feedback timed out");
-}
-
-Result DamiaoCanFdChain::receiveCycle_(
-    DamiaoJointFeedback* feedback,
-    const std::size_t feedback_count,
-    const std::chrono::steady_clock::time_point deadline)
-{
-    if (!feedback || feedback_count != joints_.size()) {
-        return invalidArgument("Damiao feedback dimensions do not match");
-    }
-
-    std::fill(feedback_seen_.begin(), feedback_seen_.end(), false);
-    std::size_t received_count = 0;
-    while (received_count < joints_.size() &&
-           std::chrono::steady_clock::now() < deadline) {
-        rx_frames_.clear();
-        int32_t count = 1;
-        if (bus_->receive(&rx_frames_, &count) != msgs::ErrorCode::OK) {
-            continue;
-        }
-        for (const auto& frame : rx_frames_) {
-            const auto index = jointIndexForFeedbackId_(frame.id);
-            if (index == joints_.size()) {
-                std::lock_guard status_lock(status_mutex_);
-                ++statistics_.unknown_feedback;
-                continue;
-            }
-            if (feedback_seen_[index]) {
-                std::lock_guard status_lock(status_mutex_);
-                ++statistics_.duplicate_feedback;
-                continue;
-            }
-            DamiaoJointFeedback decoded;
-            if (DamiaoMitCodec::decodeFeedback(
-                    frame, joints_[index].feedback_id,
-                    joints_[index].model, decoded) !=
-                    DamiaoCodecError::None ||
-                decoded.reported_motor_id !=
-                    joints_[index].reported_motor_id ||
-                !feedbackTransportAndHealthValid_(
-                    frame, joints_[index], decoded)) {
-                return commandFailed("Damiao feedback failed validation");
-            }
-            toJointFeedback_(joints_[index], decoded);
-            feedback[index] = decoded;
-            feedback_seen_[index] = true;
-            ++received_count;
-        }
-    }
-    if (received_count != joints_.size()) {
-        std::lock_guard status_lock(status_mutex_);
-        ++statistics_.deadline_misses;
-        return Result::failure(
-            ArmErrorCode::Timeout,
-            "Damiao feedback cycle missed its deadline");
-    }
-    return Result::success();
-}
-
-bool DamiaoCanFdChain::feedbackTransportAndHealthValid_(
-    const CanFrame& frame,
-    const DamiaoJointSpec& joint,
-    const DamiaoJointFeedback& feedback) const noexcept
-{
-    if (frame.is_fd != options_.is_fd) {
-        return false;
-    }
-    if (options_.is_fd && options_.bitrate_switch &&
-        !frame.bitrate_switch) {
-        return false;
-    }
-    if (joint.healthy_status_mask == 0U) {
-        // An empty whitelist is tolerated only while the actuator hardware
-        // gate is closed, so passive software/configuration checks can run.
-        return !options_.hardware_enabled;
-    }
-    if (feedback.status > 0x0FU ||
-        (joint.healthy_status_mask &
-         static_cast<std::uint16_t>(1U << feedback.status)) == 0U) {
-        return false;
-    }
-    return feedback.driver_temperature_raw <=
-               joint.max_driver_temperature_raw &&
-           feedback.motor_temperature_raw <=
-               joint.max_motor_temperature_raw;
-}
-
-bool DamiaoCanFdChain::sendFrames_(
-    const std::vector<CanFrame>& frames,
-    const std::chrono::steady_clock::time_point deadline) noexcept
-{
-    if (frames.empty() ||
-        frames.size() > static_cast<std::size_t>(
-                            std::numeric_limits<int32_t>::max())) {
-        return false;
-    }
-    int32_t count = static_cast<int32_t>(frames.size());
-    return bus_->sendUntil(frames, &count, deadline) ==
-               msgs::ErrorCode::OK &&
-           count == static_cast<int32_t>(frames.size());
-}
-
-bool DamiaoCanFdChain::sendFramesBestEffort_(
-    const std::vector<CanFrame>& frames) noexcept
-{
-    if (frames.empty() ||
-        frames.size() > static_cast<std::size_t>(
-                            std::numeric_limits<int32_t>::max())) {
-        return false;
-    }
-    int32_t count = static_cast<int32_t>(frames.size());
-    return bus_->send(frames, &count) == msgs::ErrorCode::OK &&
-           count == static_cast<int32_t>(frames.size());
-}
-
-Result DamiaoCanFdChain::disable() noexcept
-{
-    std::lock_guard lock(io_mutex_);
-    if (state_.load() == DamiaoChainState::Closed ||
-        state_.load() == DamiaoChainState::Initialized ||
-        state_.load() == DamiaoChainState::Stopped) {
-        return Result::success();
-    }
-    const bool disabled = bestEffortZeroAndDisable_();
-    if (!disabled) {
-        setError_(
-            "failed to send all Damiao zero/disable safety frames");
-        state_.store(DamiaoChainState::FaultLatched);
-        return commandFailed(lastError());
-    }
-    if (state_.load() != DamiaoChainState::FaultLatched) {
-        state_.store(DamiaoChainState::Passive);
-    }
-    return Result::success();
-}
-
-Result DamiaoCanFdChain::latchFault(const std::string& reason) noexcept
-{
-    std::lock_guard lock(io_mutex_);
-    if (!latchFaultAndDisable_(reason)) {
-        return commandFailed(lastError());
-    }
-    return Result::success();
-}
-
-bool DamiaoCanFdChain::latchFaultAndDisable_(
-    const std::string& reason) noexcept
-{
-    state_.store(DamiaoChainState::FaultLatched);
-    setError_(reason);
-    if (bestEffortZeroAndDisable_()) {
-        return true;
-    }
-    setError_(
-        reason +
-        "; failed to send all Damiao zero/disable safety frames");
-    return false;
-}
-
-bool DamiaoCanFdChain::bestEffortZeroAndDisable_() noexcept
-{
-    if (!bus_ || !options_.hardware_enabled) {
-        return true;
-    }
-
-    const auto current = state_.load();
-    if (current != DamiaoChainState::Passive &&
-        current != DamiaoChainState::Armed &&
-        current != DamiaoChainState::Active &&
-        current != DamiaoChainState::FaultLatched) {
-        return true;
-    }
-
-    bool all_sent = true;
-    if (current == DamiaoChainState::Armed ||
-        current == DamiaoChainState::Active ||
-        current == DamiaoChainState::FaultLatched) {
-        tx_frames_.clear();
-        for (const auto& joint : joints_) {
-            DamiaoMitCommand zero;
-            CanFrame frame;
-            if (DamiaoMitCodec::encodeMit(
-                    joint.command_id, joint.model, zero,
-                    options_.is_fd, options_.bitrate_switch, frame)) {
-                tx_frames_.push_back(frame);
-            }
-        }
-        if (!tx_frames_.empty()) {
-            all_sent = sendFramesBestEffort_(tx_frames_) && all_sent;
-        }
-    }
-
-    tx_frames_.clear();
-    for (const auto& joint : joints_) {
-        CanFrame frame;
-        if (DamiaoMitCodec::encodeMode(
-                joint.command_id, DamiaoMode::Disable,
-                options_.is_fd, options_.bitrate_switch, frame) ==
-            DamiaoCodecError::None) {
-            tx_frames_.push_back(frame);
-        }
-    }
-    if (!tx_frames_.empty()) {
-        all_sent = sendFramesBestEffort_(tx_frames_) && all_sent;
-    }
-    return all_sent;
-}
-
-void DamiaoCanFdChain::stop() noexcept
-{
-    std::lock_guard lock(io_mutex_);
-    const auto current = state_.load();
-    if (current == DamiaoChainState::Closed ||
-        current == DamiaoChainState::Stopped) {
-        return;
-    }
-    if (!bestEffortZeroAndDisable_()) {
-        setError_(
-            "failed to send all Damiao shutdown safety frames");
-    }
-    if (bus_) {
-        bus_->stop();
-    }
-    state_.store(DamiaoChainState::Stopped);
-}
-
-std::size_t DamiaoCanFdChain::jointIndexForFeedbackId_(
-    const std::uint32_t id) const noexcept
-{
-    for (std::size_t i = 0; i < joints_.size(); ++i) {
-        if (joints_[i].feedback_id == id) {
-            return i;
-        }
-    }
-    return joints_.size();
-}
-
-void DamiaoCanFdChain::setError_(const std::string& error) noexcept
-{
-    try {
-        std::lock_guard lock(status_mutex_);
-        last_error_ = error;
-    } catch (...) {
-    }
-}
-
-DamiaoChainStatistics DamiaoCanFdChain::statistics() const
-{
-    std::lock_guard lock(status_mutex_);
-    return statistics_;
-}
-
-std::string DamiaoCanFdChain::lastError() const
-{
-    std::lock_guard lock(status_mutex_);
-    return last_error_;
-}
-
-} // namespace cmvr::device

cmvr-es/devices/arm/ume_robot_arm/src/damiao_mit_codec.cpp

@@ -1,254 +0,0 @@
-#include "arm/ume_robot_arm/include/damiao_mit_codec.h"
-
-#include <algorithm>
-#include <cmath>
-#include <cstring>
-
-namespace cmvr::device {
-namespace {
-
-constexpr unsigned kPositionBits = 16;
-constexpr unsigned kVelocityBits = 12;
-constexpr unsigned kGainBits = 12;
-constexpr unsigned kTorqueBits = 12;
-constexpr std::uint32_t kCanStandardMaxId = 0x7FFU;
-
-bool finiteCommand(const DamiaoMitCommand& command) noexcept
-{
-    return std::isfinite(command.q_rad) &&
-           std::isfinite(command.dq_rad_s) &&
-           std::isfinite(command.kp) &&
-           std::isfinite(command.kd) &&
-           std::isfinite(command.tau_ff_nm);
-}
-
-std::uint8_t modeByte(const DamiaoMode mode) noexcept
-{
-    switch (mode) {
-    case DamiaoMode::ClearFault:
-        return 0xFBU;
-    case DamiaoMode::Enable:
-        return 0xFCU;
-    case DamiaoMode::Disable:
-        return 0xFDU;
-    case DamiaoMode::SetZero:
-        return 0xFEU;
-    }
-    return 0;
-}
-
-} // namespace
-
-bool DamiaoMotorLimits::valid() const noexcept
-{
-    return std::isfinite(q_max_rad) && q_max_rad > 0.0 &&
-           std::isfinite(dq_max_rad_s) && dq_max_rad_s > 0.0 &&
-           std::isfinite(tau_max_nm) && tau_max_nm > 0.0;
-}
-
-DamiaoMotorLimits DamiaoMitCodec::limitsFor(
-    const DamiaoMotorModel model) noexcept
-{
-    switch (model) {
-    case DamiaoMotorModel::DM4310:
-        return {12.5, 30.0, 10.0};
-    case DamiaoMotorModel::DM4310_48V:
-        return {12.5, 50.0, 10.0};
-    case DamiaoMotorModel::DM4340:
-        return {12.5, 8.0, 28.0};
-    case DamiaoMotorModel::DM4340_48V:
-        return {12.5, 10.0, 28.0};
-    case DamiaoMotorModel::DM6006:
-        return {12.5, 45.0, 20.0};
-    case DamiaoMotorModel::DM8006:
-        return {12.5, 45.0, 40.0};
-    case DamiaoMotorModel::DM8009:
-        return {12.5, 45.0, 54.0};
-    case DamiaoMotorModel::DM10010L:
-        return {12.5, 25.0, 200.0};
-    case DamiaoMotorModel::DM10010:
-        return {12.5, 20.0, 200.0};
-    case DamiaoMotorModel::DMH3510:
-        return {12.5, 280.0, 1.0};
-    case DamiaoMotorModel::DMH6215:
-        return {12.5, 45.0, 10.0};
-    case DamiaoMotorModel::DMG6220:
-        return {12.5, 45.0, 10.0};
-    case DamiaoMotorModel::Unknown:
-    default:
-        return {};
-    }
-}
-
-std::uint16_t DamiaoMitCodec::floatToUint(
-    const double value,
-    const double minimum,
-    const double maximum,
-    const unsigned bits,
-    bool& saturated) noexcept
-{
-    saturated = value < minimum || value > maximum;
-    if (!std::isfinite(value) || !std::isfinite(minimum) ||
-        !std::isfinite(maximum) || maximum <= minimum ||
-        bits == 0 || bits > 16) {
-        saturated = true;
-        return 0;
-    }
-
-    const double clamped = std::clamp(value, minimum, maximum);
-    const std::uint32_t levels = (std::uint32_t{1} << bits) - 1U;
-    const double normalized = (clamped - minimum) / (maximum - minimum);
-    return static_cast<std::uint16_t>(normalized * levels);
-}
-
-double DamiaoMitCodec::uintToFloat(
-    const std::uint16_t value,
-    const double minimum,
-    const double maximum,
-    const unsigned bits) noexcept
-{
-    if (!std::isfinite(minimum) || !std::isfinite(maximum) ||
-        maximum <= minimum || bits == 0 || bits > 16) {
-        return 0.0;
-    }
-    const double span = maximum - minimum;
-    const double levels = static_cast<double>(std::uint32_t{1} << bits);
-    return (static_cast<double>(value) + 1.0) * span / levels + minimum;
-}
-
-DamiaoEncodeResult DamiaoMitCodec::encodeMit(
-    const std::uint32_t command_id,
-    const DamiaoMotorModel model,
-    const DamiaoMitCommand& command,
-    const bool is_fd,
-    const bool bitrate_switch,
-    CanFrame& frame) noexcept
-{
-    DamiaoEncodeResult result;
-    const auto limits = limitsFor(model);
-    if (!limits.valid()) {
-        result.error = DamiaoCodecError::UnknownModel;
-        return result;
-    }
-    if (!finiteCommand(command)) {
-        result.error = DamiaoCodecError::NonFiniteInput;
-        return result;
-    }
-    if (command_id > kCanStandardMaxId) {
-        result.error = DamiaoCodecError::InvalidCanId;
-        return result;
-    }
-
-    bool saturated = false;
-    const auto q = floatToUint(
-        command.q_rad, -limits.q_max_rad, limits.q_max_rad,
-        kPositionBits, saturated);
-    if (saturated) result.saturation_mask |= DAMIAO_SATURATION_Q;
-
-    const auto dq = floatToUint(
-        command.dq_rad_s, -limits.dq_max_rad_s, limits.dq_max_rad_s,
-        kVelocityBits, saturated);
-    if (saturated) result.saturation_mask |= DAMIAO_SATURATION_DQ;
-
-    const auto kp = floatToUint(
-        command.kp, 0.0, kKpMax, kGainBits, saturated);
-    if (saturated) result.saturation_mask |= DAMIAO_SATURATION_KP;
-
-    const auto kd = floatToUint(
-        command.kd, 0.0, kKdMax, kGainBits, saturated);
-    if (saturated) result.saturation_mask |= DAMIAO_SATURATION_KD;
-
-    const auto tau = floatToUint(
-        command.tau_ff_nm, -limits.tau_max_nm, limits.tau_max_nm,
-        kTorqueBits, saturated);
-    if (saturated) result.saturation_mask |= DAMIAO_SATURATION_TAU;
-
-    frame = {};
-    frame.id = command_id;
-    frame.len = 8;
-    frame.is_fd = is_fd;
-    frame.bitrate_switch = is_fd && bitrate_switch;
-    frame.data[0] = static_cast<std::uint8_t>((q >> 8U) & 0xFFU);
-    frame.data[1] = static_cast<std::uint8_t>(q & 0xFFU);
-    frame.data[2] = static_cast<std::uint8_t>((dq >> 4U) & 0xFFU);
-    frame.data[3] = static_cast<std::uint8_t>(
-        ((dq & 0xFU) << 4U) | ((kp >> 8U) & 0xFU));
-    frame.data[4] = static_cast<std::uint8_t>(kp & 0xFFU);
-    frame.data[5] = static_cast<std::uint8_t>((kd >> 4U) & 0xFFU);
-    frame.data[6] = static_cast<std::uint8_t>(
-        ((kd & 0xFU) << 4U) | ((tau >> 8U) & 0xFU));
-    frame.data[7] = static_cast<std::uint8_t>(tau & 0xFFU);
-    return result;
-}
-
-DamiaoCodecError DamiaoMitCodec::decodeFeedback(
-    const CanFrame& frame,
-    const std::uint32_t expected_feedback_id,
-    const DamiaoMotorModel model,
-    DamiaoJointFeedback& feedback) noexcept
-{
-    feedback = {};
-    const auto limits = limitsFor(model);
-    if (!limits.valid()) {
-        return DamiaoCodecError::UnknownModel;
-    }
-    if (frame.is_error_frame || frame.is_remote_frame ||
-        frame.is_extended_id || frame.error_state_indicator ||
-        frame.len != 8) {
-        return DamiaoCodecError::InvalidFrame;
-    }
-    if (frame.id != expected_feedback_id) {
-        return DamiaoCodecError::UnexpectedFeedbackId;
-    }
-
-    const std::uint16_t q =
-        static_cast<std::uint16_t>(
-            (static_cast<std::uint16_t>(frame.data[1]) << 8U) |
-            frame.data[2]);
-    const std::uint16_t dq =
-        static_cast<std::uint16_t>(
-            (static_cast<std::uint16_t>(frame.data[3]) << 4U) |
-            (frame.data[4] >> 4U));
-    const std::uint16_t tau =
-        static_cast<std::uint16_t>(
-            ((static_cast<std::uint16_t>(frame.data[4]) & 0xFU) << 8U) |
-            frame.data[5]);
-
-    feedback.reported_motor_id = frame.data[0] & 0x0FU;
-    feedback.status = frame.data[0] >> 4U;
-    feedback.driver_temperature_raw = frame.data[6];
-    feedback.motor_temperature_raw = frame.data[7];
-    feedback.q_rad =
-        uintToFloat(q, -limits.q_max_rad, limits.q_max_rad, kPositionBits);
-    feedback.dq_rad_s =
-        uintToFloat(dq, -limits.dq_max_rad_s, limits.dq_max_rad_s,
-                    kVelocityBits);
-    feedback.tau_nm =
-        uintToFloat(tau, -limits.tau_max_nm, limits.tau_max_nm,
-                    kTorqueBits);
-    feedback.rx_monotonic_ns = frame.rx_monotonic_ns;
-    feedback.valid = true;
-    return DamiaoCodecError::None;
-}
-
-DamiaoCodecError DamiaoMitCodec::encodeMode(
-    const std::uint32_t command_id,
-    const DamiaoMode mode,
-    const bool is_fd,
-    const bool bitrate_switch,
-    CanFrame& frame) noexcept
-{
-    if (command_id > kCanStandardMaxId) {
-        return DamiaoCodecError::InvalidCanId;
-    }
-    frame = {};
-    frame.id = command_id;
-    frame.len = 8;
-    frame.is_fd = is_fd;
-    frame.bitrate_switch = is_fd && bitrate_switch;
-    std::memset(frame.data, 0xFF, 7);
-    frame.data[7] = modeByte(mode);
-    return DamiaoCodecError::None;
-}
-
-} // namespace cmvr::device

cmvr-es/devices/arm/ume_robot_arm/tests/damiao_can_fd_chain_test.cpp

@@ -1,407 +0,0 @@
-#include "arm/ume_robot_arm/include/damiao_can_fd_chain.h"
-
-#include <chrono>
-#include <deque>
-#include <memory>
-#include <string>
-#include <thread>
-#include <utility>
-#include <vector>
-
-#include <gtest/gtest.h>
-
-#include "canbus/abstract_canbus.h"
-
-namespace cmvr::device {
-namespace {
-
-class FakeCanbus final : public AbstractCanbus {
-public:
-    std::string typeName() const override { return "FakeCanbus"; }
-    bool init() override
-    {
-        initialized = true;
-        return init_result;
-    }
-    bool start() override
-    {
-        started = start_result;
-        is_started_ = started;
-        return started;
-    }
-    bool stop() override
-    {
-        stopped = true;
-        started = false;
-        is_started_ = false;
-        return true;
-    }
-
-    msgs::ErrorCode send(const std::vector<CanFrame>& frames,
-                         int32_t* frame_num) override
-    {
-        if (!started || !frame_num ||
-            *frame_num != static_cast<int32_t>(frames.size())) {
-            return msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
-        }
-        if (!send_result) {
-            return msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
-        }
-        sent_batches.push_back(frames);
-        if (!scheduled_replies.empty()) {
-            for (const auto& reply : scheduled_replies.front()) {
-                replies.push_back(reply);
-            }
-            scheduled_replies.pop_front();
-        }
-        return msgs::ErrorCode::OK;
-    }
-
-    msgs::ErrorCode receive(std::vector<CanFrame>* frames,
-                            int32_t* frame_num) override
-    {
-        if (!started || !frames || !frame_num || replies.empty()) {
-            return msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
-        }
-        frames->clear();
-        frames->push_back(replies.front());
-        replies.pop_front();
-        *frame_num = 1;
-        return msgs::ErrorCode::OK;
-    }
-
-    bool discardPendingFrames() override
-    {
-        ++drain_calls;
-        if (drain_delay > std::chrono::microseconds::zero()) {
-            std::this_thread::sleep_for(drain_delay);
-        }
-        replies.clear();
-        return drain_result;
-    }
-
-    std::string getErrorString(int32_t) override { return {}; }
-
-    void enqueueReplies(std::vector<CanFrame> batch)
-    {
-        scheduled_replies.push_back(std::move(batch));
-    }
-
-    bool init_result{true};
-    bool start_result{true};
-    bool initialized{false};
-    bool started{false};
-    bool stopped{false};
-    bool drain_result{true};
-    bool send_result{true};
-    std::size_t drain_calls{0};
-    std::chrono::microseconds drain_delay{0};
-    std::vector<std::vector<CanFrame>> sent_batches;
-    std::deque<CanFrame> replies;
-    std::deque<std::vector<CanFrame>> scheduled_replies;
-};
-
-DamiaoJointSpec joint(std::string name,
-                      std::uint32_t command_id,
-                      std::uint32_t feedback_id,
-                      std::uint8_t reported_id,
-                      int direction = 1)
-{
-    DamiaoJointSpec spec;
-    spec.joint_name = std::move(name);
-    spec.command_id = command_id;
-    spec.feedback_id = feedback_id;
-    spec.reported_motor_id = reported_id;
-    spec.model = DamiaoMotorModel::DM4310;
-    spec.direction = direction;
-    spec.zero_offset_rad = direction == 1 ? 0.1 : -0.2;
-    spec.joint_lower_rad = -2.0;
-    spec.joint_upper_rad = 2.0;
-    spec.max_velocity_rad_s = 3.0;
-    spec.max_torque_nm = 2.0;
-    spec.healthy_status_mask = 1U << 0U;
-    spec.max_driver_temperature_raw = 80U;
-    spec.max_motor_temperature_raw = 90U;
-    return spec;
-}
-
-CanFrame feedback(std::uint32_t id,
-                  std::uint8_t reported_id,
-                  std::uint8_t status = 0U)
-{
-    CanFrame frame;
-    frame.id = id;
-    frame.len = 8;
-    frame.is_fd = true;
-    frame.bitrate_switch = true;
-    frame.rx_monotonic_ns = 100;
-    frame.data[0] =
-        static_cast<std::uint8_t>((status << 4U) | reported_id);
-    frame.data[1] = 0x80;
-    frame.data[2] = 0x00;
-    frame.data[3] = 0x80;
-    frame.data[4] = 0x08;
-    frame.data[5] = 0x00;
-    frame.data[6] = 30U;
-    frame.data[7] = 35U;
-    return frame;
-}
-
-std::chrono::steady_clock::time_point soon()
-{
-    return std::chrono::steady_clock::now() +
-           std::chrono::milliseconds(20);
-}
-
-std::size_t countLifecycleByte(
-    const std::vector<std::vector<CanFrame>>& batches,
-    const std::uint8_t value)
-{
-    std::size_t count = 0;
-    for (const auto& batch : batches) {
-        for (const auto& frame : batch) {
-            if (frame.len == 8 &&
-                frame.data[0] == 0xFF &&
-                frame.data[7] == value) {
-                ++count;
-            }
-        }
-    }
-    return count;
-}
-
-TEST(DamiaoCanFdChainTest, PassiveOpenNeverEnablesHardware)
-{
-    auto bus = std::make_shared<FakeCanbus>();
-    DamiaoCanFdChain chain(
-        bus, {joint("J1", 1, 0x11, 1)},
-        DamiaoChainOptions{true, true, false});
-
-    ASSERT_TRUE(chain.init().ok());
-    ASSERT_TRUE(chain.openPassive().ok());
-    EXPECT_EQ(chain.state(), DamiaoChainState::Passive);
-    EXPECT_TRUE(bus->sent_batches.empty());
-
-    const auto arm_result = chain.arm(soon());
-    EXPECT_FALSE(arm_result.ok());
-    EXPECT_EQ(arm_result.code, ArmErrorCode::CommandRejected);
-    EXPECT_TRUE(bus->sent_batches.empty());
-}
-
-TEST(DamiaoCanFdChainTest, ExplicitArmAndExchangeUseUniqueConfiguredFeedback)
-{
-    auto bus = std::make_shared<FakeCanbus>();
-    DamiaoCanFdChain chain(
-        bus,
-        {joint("J1", 1, 0x11, 1),
-         joint("J2", 2, 0x12, 2, -1)},
-        DamiaoChainOptions{true, true, true});
-    ASSERT_TRUE(chain.init().ok());
-    ASSERT_TRUE(chain.openPassive().ok());
-
-    // A stale invalid frame is already queued before this request. The drain
-    // must remove it; only replies generated by the subsequent send may be
-    // accepted.
-    bus->replies.push_back(feedback(0x11, 1, 2));
-    bus->enqueueReplies({
-        feedback(0x12, 2),
-        feedback(0x11, 1),
-    });
-    ASSERT_TRUE(chain.arm(soon()).ok());
-    EXPECT_EQ(chain.state(), DamiaoChainState::Armed);
-    EXPECT_EQ(countLifecycleByte(bus->sent_batches, 0xFC), 2U);
-
-    bus->enqueueReplies({
-        feedback(0x11, 1),
-        feedback(0x12, 2),
-    });
-    DamiaoMitCommand commands[2]{};
-    commands[0].tau_ff_nm = 1.0;
-    commands[1].tau_ff_nm = -1.0;
-    DamiaoJointFeedback states[2]{};
-    ASSERT_TRUE(chain.exchange(
-        commands, 2, states, 2, soon()).ok());
-    EXPECT_EQ(chain.state(), DamiaoChainState::Active);
-    EXPECT_TRUE(states[0].valid);
-    EXPECT_TRUE(states[1].valid);
-    // J2 has direction=-1 and offset=-0.2.
-    EXPECT_NEAR(states[1].q_rad, -0.2003814697265625, 1e-12);
-    EXPECT_NEAR(states[1].dq_rad_s, -0.0146484375, 1e-12);
-    EXPECT_NEAR(states[1].tau_nm, -0.0048828125, 1e-12);
-}
-
-TEST(DamiaoCanFdChainTest, MissedFeedbackLatchesFaultAndNeverReenables)
-{
-    auto bus = std::make_shared<FakeCanbus>();
-    DamiaoCanFdChain chain(
-        bus, {joint("J1", 1, 0x11, 1)},
-        DamiaoChainOptions{true, true, true});
-    ASSERT_TRUE(chain.init().ok());
-    ASSERT_TRUE(chain.openPassive().ok());
-    bus->enqueueReplies({feedback(0x11, 1)});
-    ASSERT_TRUE(chain.arm(soon()).ok());
-
-    DamiaoMitCommand command;
-    DamiaoJointFeedback state;
-    const auto result = chain.exchange(
-        &command, 1, &state, 1,
-        std::chrono::steady_clock::now() +
-            std::chrono::milliseconds(1));
-    EXPECT_FALSE(result.ok());
-    EXPECT_EQ(result.code, ArmErrorCode::Timeout);
-    EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched);
-    EXPECT_EQ(countLifecycleByte(bus->sent_batches, 0xFC), 1U);
-    EXPECT_GE(countLifecycleByte(bus->sent_batches, 0xFD), 1U);
-
-    // Clearing the fault is explicit and leaves the chain passive.
-    bus->enqueueReplies({feedback(0x11, 1)});
-    ASSERT_TRUE(chain.clearFault(soon()).ok());
-    EXPECT_EQ(chain.state(), DamiaoChainState::Passive);
-    EXPECT_EQ(countLifecycleByte(bus->sent_batches, 0xFC), 1U);
-}
-
-TEST(DamiaoCanFdChainTest, DuplicateFeedbackCannotSatisfyAGroupCycle)
-{
-    auto bus = std::make_shared<FakeCanbus>();
-    DamiaoCanFdChain chain(
-        bus,
-        {joint("J1", 1, 0x11, 1),
-         joint("J2", 2, 0x12, 2)},
-        DamiaoChainOptions{true, true, true});
-    ASSERT_TRUE(chain.init().ok());
-    ASSERT_TRUE(chain.openPassive().ok());
-    bus->enqueueReplies({
-        feedback(0x11, 1),
-        feedback(0x12, 2),
-    });
-    ASSERT_TRUE(chain.arm(soon()).ok());
-
-    bus->enqueueReplies({
-        feedback(0x11, 1),
-        feedback(0x11, 1),
-    });
-    DamiaoMitCommand commands[2]{};
-    DamiaoJointFeedback states[2]{};
-    EXPECT_FALSE(chain.exchange(
-        commands, 2, states, 2,
-        std::chrono::steady_clock::now() +
-            std::chrono::milliseconds(1)).ok());
-    EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched);
-    EXPECT_EQ(chain.statistics().duplicate_feedback, 1U);
-}
-
-TEST(DamiaoCanFdChainTest, RejectsUnreviewedStatusAndClassicFrame)
-{
-    auto bus = std::make_shared<FakeCanbus>();
-    DamiaoCanFdChain chain(
-        bus, {joint("J1", 1, 0x11, 1)},
-        DamiaoChainOptions{true, true, true});
-    ASSERT_TRUE(chain.init().ok());
-    ASSERT_TRUE(chain.openPassive().ok());
-    bus->enqueueReplies({feedback(0x11, 1)});
-    ASSERT_TRUE(chain.arm(soon()).ok());
-
-    bus->enqueueReplies({feedback(0x11, 1, 2)});
-    DamiaoMitCommand command;
-    DamiaoJointFeedback state;
-    EXPECT_FALSE(chain.exchange(
-        &command, 1, &state, 1, soon()).ok());
-    EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched);
-
-    auto second_bus = std::make_shared<FakeCanbus>();
-    DamiaoCanFdChain second(
-        second_bus, {joint("J1", 1, 0x11, 1)},
-        DamiaoChainOptions{true, true, true});
-    ASSERT_TRUE(second.init().ok());
-    ASSERT_TRUE(second.openPassive().ok());
-    auto classic = feedback(0x11, 1);
-    classic.is_fd = false;
-    classic.bitrate_switch = false;
-    second_bus->enqueueReplies({classic});
-    EXPECT_FALSE(second.arm(soon()).ok());
-    EXPECT_EQ(second.state(), DamiaoChainState::FaultLatched);
-
-    auto third_bus = std::make_shared<FakeCanbus>();
-    DamiaoCanFdChain third(
-        third_bus, {joint("J1", 1, 0x11, 1)},
-        DamiaoChainOptions{true, true, true});
-    ASSERT_TRUE(third.init().ok());
-    ASSERT_TRUE(third.openPassive().ok());
-    auto error_passive = feedback(0x11, 1);
-    error_passive.error_state_indicator = true;
-    third_bus->enqueueReplies({error_passive});
-    EXPECT_FALSE(third.arm(soon()).ok());
-    EXPECT_EQ(third.state(), DamiaoChainState::FaultLatched);
-}
-
-TEST(DamiaoCanFdChainTest, HardwareEnableRequiresReviewedHealthContract)
-{
-    auto bus = std::make_shared<FakeCanbus>();
-    auto unreviewed = joint("J1", 1, 0x11, 1);
-    unreviewed.healthy_status_mask = 0U;
-    DamiaoCanFdChain chain(
-        bus, {unreviewed},
-        DamiaoChainOptions{true, true, true});
-
-    const auto result = chain.init();
-    EXPECT_FALSE(result.ok());
-    EXPECT_EQ(result.code, ArmErrorCode::InvalidArgument);
-    EXPECT_FALSE(bus->initialized);
-}
-
-TEST(DamiaoCanFdChainTest, DisableReportsUnconfirmedSafetyFrames)
-{
-    auto bus = std::make_shared<FakeCanbus>();
-    DamiaoCanFdChain chain(
-        bus, {joint("J1", 1, 0x11, 1)},
-        DamiaoChainOptions{true, true, true});
-    ASSERT_TRUE(chain.init().ok());
-    ASSERT_TRUE(chain.openPassive().ok());
-    bus->enqueueReplies({feedback(0x11, 1)});
-    ASSERT_TRUE(chain.arm(soon()).ok());
-
-    bus->send_result = false;
-    const auto result = chain.disable();
-    EXPECT_FALSE(result.ok());
-    EXPECT_EQ(result.code, ArmErrorCode::CommandFailed);
-    EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched);
-}
-
-TEST(DamiaoCanFdChainTest, ExpiredDeadlineAfterDrainNeverCommitsEnable)
-{
-    auto bus = std::make_shared<FakeCanbus>();
-    DamiaoCanFdChain chain(
-        bus, {joint("J1", 1, 0x11, 1)},
-        DamiaoChainOptions{true, true, true});
-    ASSERT_TRUE(chain.init().ok());
-    ASSERT_TRUE(chain.openPassive().ok());
-
-    bus->drain_delay = std::chrono::milliseconds(3);
-    bus->enqueueReplies({feedback(0x11, 1)});
-    const auto result = chain.arm(
-        std::chrono::steady_clock::now() +
-        std::chrono::milliseconds(1));
-
-    EXPECT_FALSE(result.ok());
-    EXPECT_EQ(result.code, ArmErrorCode::Timeout);
-    EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched);
-    EXPECT_EQ(countLifecycleByte(bus->sent_batches, 0xFC), 0U);
-    EXPECT_GE(countLifecycleByte(bus->sent_batches, 0xFD), 1U);
-}
-
-TEST(DamiaoCanFdChainTest, ConfigurationRejectsAmbiguousMappings)
-{
-    auto bus = std::make_shared<FakeCanbus>();
-    DamiaoCanFdChain chain(
-        bus,
-        {joint("J1", 1, 0x11, 1),
-         joint("J1", 2, 0x12, 2)},
-        DamiaoChainOptions{true, true, false});
-    const auto result = chain.init();
-    EXPECT_FALSE(result.ok());
-    EXPECT_EQ(result.code, ArmErrorCode::InvalidArgument);
-    EXPECT_FALSE(bus->initialized);
-}
-
-} // namespace
-} // namespace cmvr::device

cmvr-es/devices/arm/ume_robot_arm/tests/damiao_mit_codec_test.cpp

@@ -1,150 +0,0 @@
-#include "arm/ume_robot_arm/include/damiao_mit_codec.h"
-
-#include <array>
-#include <cmath>
-#include <limits>
-
-#include <gtest/gtest.h>
-
-namespace cmvr::device {
-namespace {
-
-void expectPayload(const CanFrame& frame,
-                   const std::array<std::uint8_t, 8>& expected)
-{
-    ASSERT_EQ(frame.len, expected.size());
-    for (std::size_t i = 0; i < expected.size(); ++i) {
-        EXPECT_EQ(frame.data[i], expected[i]) << "byte " << i;
-    }
-}
-
-TEST(DamiaoMitCodecTest, MatchesLegacyPythonGoldenVectors)
-{
-    CanFrame frame;
-    DamiaoMitCommand zero;
-    auto result = DamiaoMitCodec::encodeMit(
-        1, DamiaoMotorModel::DM4310, zero, true, true, frame);
-    ASSERT_TRUE(result);
-    EXPECT_EQ(result.saturation_mask, DAMIAO_SATURATION_NONE);
-    EXPECT_TRUE(frame.is_fd);
-    EXPECT_TRUE(frame.bitrate_switch);
-    expectPayload(frame, {0x7F, 0xFF, 0x7F, 0xF0,
-                          0x00, 0x00, 0x07, 0xFF});
-
-    DamiaoMitCommand nontrivial;
-    nontrivial.kp = 100.0;
-    nontrivial.kd = 1.0;
-    nontrivial.q_rad = 1.25;
-    nontrivial.dq_rad_s = -2.5;
-    nontrivial.tau_ff_nm = 3.0;
-    result = DamiaoMitCodec::encodeMit(
-        1, DamiaoMotorModel::DM4310, nontrivial, false, false, frame);
-    ASSERT_TRUE(result);
-    expectPayload(frame, {0x8C, 0xCC, 0x75, 0x43,
-                          0x33, 0x33, 0x3A, 0x65});
-}
-
-TEST(DamiaoMitCodecTest, ReportsProtocolSaturationWithoutHidingIt)
-{
-    DamiaoMitCommand command;
-    command.q_rad = 100.0;
-    command.dq_rad_s = -100.0;
-    command.kp = 600.0;
-    command.kd = -1.0;
-    command.tau_ff_nm = 100.0;
-    CanFrame frame;
-    const auto result = DamiaoMitCodec::encodeMit(
-        2, DamiaoMotorModel::DM4310, command, false, false, frame);
-    ASSERT_TRUE(result);
-    EXPECT_EQ(
-        result.saturation_mask,
-        DAMIAO_SATURATION_Q | DAMIAO_SATURATION_DQ |
-            DAMIAO_SATURATION_KP | DAMIAO_SATURATION_KD |
-            DAMIAO_SATURATION_TAU);
-}
-
-TEST(DamiaoMitCodecTest, RejectsNonFiniteInput)
-{
-    DamiaoMitCommand command;
-    command.tau_ff_nm = std::numeric_limits<double>::quiet_NaN();
-    CanFrame frame;
-    const auto result = DamiaoMitCodec::encodeMit(
-        1, DamiaoMotorModel::DM4310, command, false, false, frame);
-    EXPECT_FALSE(result);
-    EXPECT_EQ(result.error, DamiaoCodecError::NonFiniteInput);
-}
-
-TEST(DamiaoMitCodecTest, EncodesLifecycleFramesWithoutEnablingImplicitly)
-{
-    CanFrame frame;
-    ASSERT_EQ(DamiaoMitCodec::encodeMode(
-                  3, DamiaoMode::Enable, true, true, frame),
-              DamiaoCodecError::None);
-    expectPayload(frame, {0xFF, 0xFF, 0xFF, 0xFF,
-                          0xFF, 0xFF, 0xFF, 0xFC});
-
-    ASSERT_EQ(DamiaoMitCodec::encodeMode(
-                  3, DamiaoMode::Disable, true, true, frame),
-              DamiaoCodecError::None);
-    EXPECT_EQ(frame.data[7], 0xFD);
-    ASSERT_EQ(DamiaoMitCodec::encodeMode(
-                  3, DamiaoMode::SetZero, true, true, frame),
-              DamiaoCodecError::None);
-    EXPECT_EQ(frame.data[7], 0xFE);
-    ASSERT_EQ(DamiaoMitCodec::encodeMode(
-                  3, DamiaoMode::ClearFault, true, true, frame),
-              DamiaoCodecError::None);
-    EXPECT_EQ(frame.data[7], 0xFB);
-}
-
-TEST(DamiaoMitCodecTest, DecodesLegacyFeedbackAndRequiresConfiguredId)
-{
-    CanFrame frame;
-    frame.id = 0x11;
-    frame.len = 8;
-    frame.is_fd = true;
-    frame.bitrate_switch = true;
-    frame.rx_monotonic_ns = 1234567;
-    frame.data[0] = 0xA1;
-    frame.data[1] = 0x80;
-    frame.data[2] = 0x00;
-    frame.data[3] = 0x80;
-    frame.data[4] = 0x08;
-    frame.data[5] = 0x00;
-    frame.data[6] = 40;
-    frame.data[7] = 41;
-
-    DamiaoJointFeedback feedback;
-    EXPECT_EQ(DamiaoMitCodec::decodeFeedback(
-                  frame, 0x12, DamiaoMotorModel::DM4310, feedback),
-              DamiaoCodecError::UnexpectedFeedbackId);
-    EXPECT_FALSE(feedback.valid);
-
-    ASSERT_EQ(DamiaoMitCodec::decodeFeedback(
-                  frame, 0x11, DamiaoMotorModel::DM4310, feedback),
-              DamiaoCodecError::None);
-    EXPECT_TRUE(feedback.valid);
-    EXPECT_EQ(feedback.reported_motor_id, 1);
-    EXPECT_EQ(feedback.status, 0x0A);
-    EXPECT_EQ(feedback.driver_temperature_raw, 40);
-    EXPECT_EQ(feedback.motor_temperature_raw, 41);
-    EXPECT_EQ(feedback.rx_monotonic_ns, 1234567);
-    EXPECT_NEAR(feedback.q_rad, 0.0003814697265625, 1e-12);
-    EXPECT_NEAR(feedback.dq_rad_s, 0.0146484375, 1e-12);
-    EXPECT_NEAR(feedback.tau_nm, 0.0048828125, 1e-12);
-}
-
-TEST(DamiaoMitCodecTest, ContainsAllLegacyMotorRanges)
-{
-    EXPECT_DOUBLE_EQ(
-        DamiaoMitCodec::limitsFor(DamiaoMotorModel::DM8009).tau_max_nm,
-        54.0);
-    EXPECT_DOUBLE_EQ(
-        DamiaoMitCodec::limitsFor(DamiaoMotorModel::DMH3510).dq_max_rad_s,
-        280.0);
-    EXPECT_FALSE(
-        DamiaoMitCodec::limitsFor(DamiaoMotorModel::Unknown).valid());
-}
-
-} // namespace
-} // namespace cmvr::device

cmvr-es/devices/arm/ume_robot_arm/tests/ume_robot_arm_test.cpp

@@ -1,338 +0,0 @@
-#include "arm/ume_robot_arm/include/ume_robot_arm.h"
-
-#include <chrono>
-#include <cstdint>
-#include <deque>
-#include <functional>
-#include <memory>
-#include <mutex>
-#include <string>
-#include <thread>
-#include <vector>
-
-#include <gtest/gtest.h>
-
-#include "canbus/abstract_canbus.h"
-#include "common/io/proto_file_io.h"
-
-#ifndef CMVR_UME_ARM_CONFIG_PATH
-#define CMVR_UME_ARM_CONFIG_PATH ""
-#endif
-
-namespace cmvr::device {
-namespace {
-
-class LoopbackDamiaoBus final : public AbstractCanbus {
-public:
-    std::string typeName() const override { return "LoopbackDamiaoBus"; }
-    bool init() override
-    {
-        std::lock_guard lock(mutex);
-        initialized = true;
-        return true;
-    }
-    bool start() override
-    {
-        std::lock_guard lock(mutex);
-        started = true;
-        is_started_ = true;
-        return true;
-    }
-    bool stop() override
-    {
-        std::lock_guard lock(mutex);
-        started = false;
-        is_started_ = false;
-        return true;
-    }
-
-    msgs::ErrorCode send(
-        const std::vector<CanFrame>& frames,
-        int32_t* frame_num) override
-    {
-        std::lock_guard lock(mutex);
-        if (!started || !frame_num ||
-            *frame_num != static_cast<int32_t>(frames.size())) {
-            return msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
-        }
-        if (!send_result) {
-            return msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
-        }
-        sent_batches.push_back(frames);
-        for (const auto& frame : frames) {
-            if (frame.id < 1U || frame.id > 8U) {
-                continue;
-            }
-            CanFrame reply;
-            reply.id = 0x10U + frame.id;
-            reply.len = 8U;
-            reply.is_fd = true;
-            reply.bitrate_switch = true;
-            reply.rx_monotonic_ns =
-                std::chrono::duration_cast<std::chrono::nanoseconds>(
-                    std::chrono::steady_clock::now().time_since_epoch())
-                    .count();
-            reply.data[0] = static_cast<std::uint8_t>(frame.id);
-            reply.data[1] = 0x80U;
-            reply.data[2] = 0x00U;
-            reply.data[3] = 0x80U;
-            reply.data[4] = 0x08U;
-            reply.data[5] = 0x00U;
-            replies.push_back(reply);
-        }
-        return msgs::ErrorCode::OK;
-    }
-
-    msgs::ErrorCode receive(
-        std::vector<CanFrame>* frames,
-        int32_t* frame_num) override
-    {
-        std::lock_guard lock(mutex);
-        if (!started || !frames || !frame_num || replies.empty()) {
-            return msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
-        }
-        frames->clear();
-        frames->push_back(replies.front());
-        replies.pop_front();
-        *frame_num = 1;
-        return msgs::ErrorCode::OK;
-    }
-
-    bool discardPendingFrames() override
-    {
-        std::lock_guard lock(mutex);
-        replies.clear();
-        return started;
-    }
-
-    std::string getErrorString(int32_t) override { return {}; }
-
-    void setSendResult(const bool result)
-    {
-        std::lock_guard lock(mutex);
-        send_result = result;
-    }
-
-    std::size_t lifecycleCount(const std::uint8_t byte) const
-    {
-        std::lock_guard lock(mutex);
-        std::size_t count = 0;
-        for (const auto& batch : sent_batches) {
-            for (const auto& frame : batch) {
-                if (frame.len == 8U &&
-                    frame.data[0] == 0xFFU &&
-                    frame.data[7] == byte) {
-                    ++count;
-                }
-            }
-        }
-        return count;
-    }
-
-    bool initialized{false};
-    bool started{false};
-    bool send_result{true};
-    std::deque<CanFrame> replies;
-    std::vector<std::vector<CanFrame>> sent_batches;
-    mutable std::mutex mutex;
-};
-
-config::RobotArmConfig configFor(const bool hardware_enabled)
-{
-    config::RobotArmConfig cfg;
-    cfg.set_id("ume_right");
-    auto* ume = cfg.mutable_ume();
-    ume->set_hardware_enabled(hardware_enabled);
-    ume->set_control_frequency_hz(800U);
-    ume->set_cycle_deadline_us(1000U);
-    ume->set_feedback_watchdog_ms(20U);
-    auto* can = ume->mutable_can();
-    can->set_interface_name("fake-can");
-    can->set_enable_fd(true);
-    can->set_bitrate_switch(true);
-    can->set_send_timeout_us(100U);
-    can->set_receive_timeout_us(100U);
-    can->set_receive_own_messages(false);
-    for (std::uint32_t i = 1; i <= 8U; ++i) {
-        auto* joint = ume->add_joints();
-        joint->set_joint_name("RJ" + std::to_string(i));
-        joint->set_command_id(i);
-        joint->set_feedback_id(0x10U + i);
-        joint->set_reported_motor_id(i);
-        joint->set_model(config::DAMIAO_MOTOR_MODEL_DM4310);
-        joint->set_direction(1);
-        joint->set_joint_lower_rad(-2.0);
-        joint->set_joint_upper_rad(2.0);
-        joint->set_max_velocity_rad_s(3.0);
-        joint->set_max_torque_nm(2.0);
-        joint->add_healthy_feedback_status(0U);
-        joint->set_max_driver_temperature_raw(80U);
-        joint->set_max_motor_temperature_raw(90U);
-    }
-    return cfg;
-}
-
-bool waitUntil(
-    const std::function<bool()>& predicate,
-    const std::chrono::milliseconds timeout)
-{
-    const auto deadline = std::chrono::steady_clock::now() + timeout;
-    while (std::chrono::steady_clock::now() < deadline) {
-        if (predicate()) {
-            return true;
-        }
-        std::this_thread::sleep_for(std::chrono::milliseconds(1));
-    }
-    return predicate();
-}
-
-TEST(UmeRobotArmTest, LifecycleIsPassiveUntilExplicitFreshTorqueCommand)
-{
-    auto bus = std::make_shared<LoopbackDamiaoBus>();
-    UmeRobotArm arm(configFor(true), bus);
-
-    ASSERT_TRUE(arm.init());
-    EXPECT_EQ(bus->lifecycleCount(0xFCU), 0U);
-    ASSERT_TRUE(arm.start());
-    EXPECT_EQ(bus->lifecycleCount(0xFCU), 0U);
-
-    TorqueServoOptions options;
-    options.period = 0.00125;
-    options.command_watchdog_ms = 100U;
-    ASSERT_TRUE(arm.startTorqueMode(options).ok());
-    JointTorqueCommand command;
-    command.torque.assign(8U, 0.0);
-    ASSERT_TRUE(arm.servoTorque(command).ok());
-    ASSERT_TRUE(arm.torqueOn().ok());
-
-    ASSERT_TRUE(waitUntil(
-        [&arm] { return arm.getJointState().sequence > 0U; },
-        std::chrono::milliseconds(30)));
-    const auto state = arm.getJointState();
-    EXPECT_TRUE(state.position_valid);
-    EXPECT_TRUE(state.velocity_valid);
-    EXPECT_TRUE(state.effort_valid);
-    EXPECT_EQ(state.position.size(), 8U);
-    EXPECT_EQ(bus->lifecycleCount(0xFCU), 8U);
-    EXPECT_EQ(arm.getControlMode(), ControlMode::Torque);
-
-    EXPECT_TRUE(arm.stop());
-    EXPECT_GE(bus->lifecycleCount(0xFDU), 8U);
-}
-
-TEST(UmeRobotArmTest, StaleCommandLatchesFaultAndNeverReenables)
-{
-    auto bus = std::make_shared<LoopbackDamiaoBus>();
-    UmeRobotArm arm(configFor(true), bus);
-    ASSERT_TRUE(arm.init());
-    ASSERT_TRUE(arm.start());
-
-    TorqueServoOptions options;
-    options.period = 0.001;
-    options.command_watchdog_ms = 2U;
-    ASSERT_TRUE(arm.startTorqueMode(options).ok());
-    JointTorqueCommand command;
-    command.torque.assign(8U, 0.0);
-    ASSERT_TRUE(arm.servoTorque(command).ok());
-    ASSERT_TRUE(arm.torqueOn().ok());
-    ASSERT_TRUE(waitUntil(
-        [&arm] { return arm.isFault(); },
-        std::chrono::milliseconds(50)));
-
-    EXPECT_FALSE(arm.busy());
-    EXPECT_EQ(bus->lifecycleCount(0xFCU), 8U);
-    EXPECT_GE(bus->lifecycleCount(0xFDU), 8U);
-    EXPECT_EQ(arm.healthSnapshot().state, DeviceHealthState::Fault);
-}
-
-TEST(UmeRobotArmTest, HardwareGateRejectsEnableWithoutWritingIt)
-{
-    auto bus = std::make_shared<LoopbackDamiaoBus>();
-    UmeRobotArm arm(configFor(false), bus);
-    ASSERT_TRUE(arm.init());
-    ASSERT_TRUE(arm.start());
-    ASSERT_TRUE(arm.startTorqueMode(TorqueServoOptions{}).ok());
-    JointTorqueCommand command;
-    command.torque.assign(8U, 0.0);
-    ASSERT_TRUE(arm.servoTorque(command).ok());
-
-    const auto result = arm.torqueOn();
-    EXPECT_FALSE(result.ok());
-    EXPECT_EQ(result.code, ArmErrorCode::CommandRejected);
-    EXPECT_EQ(bus->lifecycleCount(0xFCU), 0U);
-}
-
-TEST(UmeRobotArmTest, PositionServoIsExplicitlyUnsupported)
-{
-    auto bus = std::make_shared<LoopbackDamiaoBus>();
-    UmeRobotArm arm(configFor(false), bus);
-    JointPositionCommand command;
-    command.position.assign(8U, 0.0);
-    const auto result = arm.servoJ(command);
-    EXPECT_EQ(result.code, ArmErrorCode::UnsupportedCommand);
-}
-
-TEST(UmeRobotArmTest, RejectsCycleDeadlineLongerThanControlPeriod)
-{
-    auto cfg = configFor(false);
-    cfg.mutable_ume()->set_cycle_deadline_us(2000U);
-    auto bus = std::make_shared<LoopbackDamiaoBus>();
-    UmeRobotArm arm(cfg, bus);
-
-    EXPECT_FALSE(arm.init());
-    EXPECT_FALSE(bus->initialized);
-    EXPECT_EQ(
-        arm.healthSnapshot().state,
-        DeviceHealthState::Fault);
-}
-
-TEST(UmeRobotArmTest, RejectsReportedMotorIdBeforeNarrowingConversion)
-{
-    auto cfg = configFor(false);
-    cfg.mutable_ume()->mutable_joints(0)->set_reported_motor_id(257U);
-    auto bus = std::make_shared<LoopbackDamiaoBus>();
-    UmeRobotArm arm(cfg, bus);
-
-    EXPECT_FALSE(arm.init());
-    EXPECT_FALSE(bus->initialized);
-    EXPECT_EQ(arm.healthSnapshot().state, DeviceHealthState::Fault);
-}
-
-TEST(UmeRobotArmTest, EmergencyStopReportsUnconfirmedDisable)
-{
-    auto bus = std::make_shared<LoopbackDamiaoBus>();
-    UmeRobotArm arm(configFor(true), bus);
-    ASSERT_TRUE(arm.init());
-    ASSERT_TRUE(arm.start());
-
-    bus->setSendResult(false);
-    const auto result = arm.emergencyStop();
-
-    EXPECT_FALSE(result.ok());
-    EXPECT_EQ(result.code, ArmErrorCode::CommandFailed);
-    EXPECT_TRUE(arm.isEmergencyStopped());
-    EXPECT_TRUE(arm.isFault());
-    EXPECT_NE(
-        arm.healthSnapshot().error_message.find("zero/disable failed"),
-        std::string::npos);
-}
-
-TEST(UmeRobotArmTest, CheckedInDualArmConfigParsesAndKeepsHardwareDisabled)
-{
-    config::ArmRootConfig root;
-    ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
-        CMVR_UME_ARM_CONFIG_PATH, &root));
-    ASSERT_EQ(root.arm().robot_arms_size(), 2);
-    for (const auto& arm : root.arm().robot_arms()) {
-        ASSERT_TRUE(arm.has_ume());
-        EXPECT_EQ(arm.ume().joints_size(), 8);
-        EXPECT_FALSE(arm.ume().hardware_enabled());
-        EXPECT_TRUE(arm.ume().can().enable_fd());
-        EXPECT_TRUE(arm.ume().can().bitrate_switch());
-        EXPECT_GT(arm.ume().can().send_timeout_us(), 0U);
-        EXPECT_FALSE(arm.ume().can().receive_own_messages());
-    }
-}
-
-} // namespace
-} // namespace cmvr::device

cmvr-es/devices/canbus/CMakeLists.txt

@@ -31,21 +31,6 @@ target_link_libraries(socket_can_client_raw_test
         glog
         cmvr_es::proto
 )
-add_test(
-        NAME socket_can_client_raw_test
-        COMMAND socket_can_client_raw_test
-)
-set(_socket_can_client_raw_test_environment
-        "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
-)
-if(CMVR_TEST_SYSTEM_LIBSTDCXX)
-    list(APPEND _socket_can_client_raw_test_environment
-            "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
-endif()
-set_tests_properties(socket_can_client_raw_test PROPERTIES
-        TIMEOUT 10
-        ENVIRONMENT "${_socket_can_client_raw_test_environment}"
-)
 
 
 add_executable(protocol_data_test
@@ -108,3 +93,4 @@ target_link_libraries(can_receiver_test
         glog
         cmvr_es::proto
 )
+

cmvr-es/devices/canbus/abstract_canbus.h

@@ -3,14 +3,6 @@
 //
 
 #pragma once
-#include <algorithm>
-#include <chrono>
-#include <cstdint>
-#include <cstring>
-#include <sstream>
-#include <string>
-#include <sys/time.h>
-
 #include "../abstract_device.h"
 #include "cmvr/msgs/error_code.pb.h"
 #include "canbus/common/byte.h"
@@ -22,26 +14,20 @@ namespace cmvr::device {
     */
     struct CanFrame {
         /// Message id
-        uint32_t id{0};
+        uint32_t id;
         /// Message length
-        uint8_t len{0};
+        uint8_t len;
-        /// Message content. Classic CAN uses at most the first 8 bytes.
+        /// Message content
-        uint8_t data[64]{};
+        uint8_t data[8];
-        bool is_extended_id{false};
+        /// Time stamp
-        bool is_remote_frame{false};
+        struct timeval timestamp;
-        bool is_error_frame{false};
-        bool is_fd{false};
-        bool bitrate_switch{false};
-        bool error_state_indicator{false};
-        /// Local host receive time used for freshness and watchdog checks.
-        int64_t rx_monotonic_ns{0};
-        /// Legacy wall-clock field retained for source compatibility.
-        struct timeval timestamp{0, 0};
 
         /**
          * @brief Constructor
          */
-        CanFrame() = default;
+        CanFrame() : id(0), len(0), timestamp{0} {
+            std::memset(data, 0, sizeof(data));
+        }
 
         /**
          * @brief CanFrame string including essential information about the message.
@@ -51,15 +37,10 @@ namespace cmvr::device {
             std::stringstream output_stream("");
             output_stream << "id:0x" << Byte::byte_to_hex(id)
                     << ",len:" << static_cast<int>(len) << ",data:";
-            const auto printable_len =
+            for (uint8_t i = 0; i < len; ++i) {
-                std::min<std::size_t>(len, sizeof(data));
-            for (std::size_t i = 0; i < printable_len; ++i) {
                 output_stream << Byte::byte_to_hex(data[i]);
             }
-            output_stream << ",fd:" << is_fd
+            output_stream << ",";
-                          << ",brs:" << bitrate_switch
-                          << ",extended:" << is_extended_id
-                          << ",error:" << is_error_frame << ",";
             return output_stream.str();
         }
     };
@@ -86,28 +67,6 @@ namespace cmvr::device {
         virtual cmvr::msgs::ErrorCode send(const std::vector<CanFrame> &frames,
                                                int32_t *const frame_num) = 0;
 
-        /**
-         * @brief Send messages without starting a batch after an absolute
-         *        local deadline.
-         *
-         * Deadline-aware transports should override this method so their
-         * internal blocking budget is also capped by @p deadline. The default
-         * preserves source compatibility and at least rejects an already
-         * expired request before calling send().
-         */
-        virtual cmvr::msgs::ErrorCode sendUntil(
-            const std::vector<CanFrame>& frames,
-            int32_t* const frame_num,
-            const std::chrono::steady_clock::time_point deadline) {
-            if (std::chrono::steady_clock::now() >= deadline) {
-                if (frame_num) {
-                    *frame_num = 0;
-                }
-                return cmvr::msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
-            }
-            return send(frames, frame_num);
-        }
-
         /**
          * @brief Send a single message.
          * @param frames A single-element vector containing only one message.
@@ -116,9 +75,7 @@ namespace cmvr::device {
         virtual cmvr::msgs::ErrorCode sendSingleFrame(
             const std::vector<CanFrame> &frames) {
             if (frames.size() != 1U) {
-                CMVR_LOG(ERROR) << "frames size not equal to 1, actual frame size: "
+                CMVR_LOG(FATAL) << "frames size not equal to 1, actual frame size: " << frames.size();
-                                << frames.size();
-                return cmvr::msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
             }
             int32_t n = 1;
             return send(frames, &n);
@@ -134,17 +91,6 @@ namespace cmvr::device {
         virtual cmvr::msgs::ErrorCode receive(std::vector<CanFrame> *const frames,
                                                   int32_t *const frame_num) = 0;
 
-        /**
-         * @brief Discard frames already queued by the transport.
-         *
-         * Command/response protocols without a sequence field can use this
-         * immediately before sending a new request to reduce the risk that a
-         * response from an older cycle is accepted as fresh. Implementations
-         * must keep this call bounded. The conservative default reports that
-         * the transport cannot provide this guarantee.
-         */
-        virtual bool discardPendingFrames() { return false; }
-
         /**
          * @brief Get the error string.
          * @param status The status to get the error string.

cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc

@@ -12,13 +12,9 @@
 #include "socket_can_client_raw.h"
 #include "absl/strings/str_cat.h"
 
-#include <cerrno>
-#include <chrono>
-#include <limits>
-#include <poll.h>
-
 namespace cmvr {
     namespace device {
+#define CAN_ID_MASK 0x1FFFF800U  // can_filter mask
 #define CAN_STANDARD_MAX_ID 0x7FFU
 
         using cmvr::msgs::ErrorCode;
@@ -28,25 +24,8 @@ namespace cmvr {
             auto channel_id = cfg.channel_id();
             port_ = static_cast<CANCardParameter::CANChannelId>(channel_id);
             interface_ = CANCardParameter::NATIVE;
-            interface_name_ =
+
-                cfg.has_interface_name() && !cfg.interface_name().empty()
+            enable_can_err_check_ = false;
-                    ? cfg.interface_name()
-                    : cfg.dev_id();
-            enable_fd_ = cfg.has_enable_fd() && cfg.enable_fd();
-            default_bitrate_switch_ =
-                cfg.has_bitrate_switch() && cfg.bitrate_switch();
-            receive_own_messages_ =
-                cfg.has_receive_own_messages() && cfg.receive_own_messages();
-            receive_timeout_us_ =
-                cfg.has_receive_timeout_us() && cfg.receive_timeout_us() > 0
-                    ? cfg.receive_timeout_us()
-                    : 100000U;
-            send_timeout_us_ =
-                cfg.has_send_timeout_us() && cfg.send_timeout_us() > 0
-                    ? cfg.send_timeout_us()
-                    : 100000U;
-            enable_can_err_check_ =
-                cfg.has_enable_error_frames() && cfg.enable_error_frames();
         }
 
 
@@ -70,7 +49,7 @@ namespace cmvr {
         }
 
         SocketCanClientRaw::~SocketCanClientRaw() {
-            if (dev_handler_ >= 0) {
+            if (dev_handler_) {
                 stop();
             }
         }
@@ -80,8 +59,8 @@ namespace cmvr {
                 status_ = ErrorCode::OK;
                 return true;
             }
-            struct sockaddr_can addr {};
+            struct sockaddr_can addr;
-            struct ifreq ifr {};
+            struct ifreq ifr;
 
             // open device
             // guss net is the device minor number, if one card is 0,1
@@ -112,71 +91,17 @@ namespace cmvr {
                 if (ret < 0) {
                     CMVR_LOG(ERROR) << "add receive msg id filter error code: " << ret;
                     status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
-                    stop();
                     return false;
                 }
             }
 
-            // 2. Explicitly opt into CAN-FD only when configured. This socket
+            // 2. enable reception of can frames.
-            // option does not configure the physical link bitrate or state.
+            int enable = 1;
-            if (enable_fd_) {
+            ret = ::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable,
-                int enable = 1;
+                               sizeof(enable));
-                ret = ::setsockopt(dev_handler_, SOL_CAN_RAW,
+            if (ret < 0) {
-                                   CAN_RAW_FD_FRAMES, &enable, sizeof(enable));
+                CMVR_LOG(ERROR) << "enable reception of can frame error code: " << ret;
-                if (ret < 0) {
-                    CMVR_LOG(ERROR) << "enable CAN-FD frames failed: "
-                                    << std::strerror(errno);
-                    status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
-                    stop();
-                    return false;
-                }
-            }
-
-            const int receive_own = receive_own_messages_ ? 1 : 0;
-            if (::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
-                             &receive_own, sizeof(receive_own)) < 0) {
-                CMVR_LOG(ERROR) << "configure receive-own-messages failed: "
-                                << std::strerror(errno);
                 status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
-                stop();
-                return false;
-            }
-
-            if (enable_can_err_check_) {
-                const can_err_mask_t error_mask = CAN_ERR_MASK;
-                if (::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
-                                 &error_mask, sizeof(error_mask)) < 0) {
-                    CMVR_LOG(ERROR) << "configure CAN error filter failed: "
-                                    << std::strerror(errno);
-                    status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
-                    stop();
-                    return false;
-                }
-            }
-
-            struct timeval receive_timeout {
-                static_cast<time_t>(receive_timeout_us_ / 1000000U),
-                static_cast<suseconds_t>(receive_timeout_us_ % 1000000U)
-            };
-            if (::setsockopt(dev_handler_, SOL_SOCKET, SO_RCVTIMEO,
-                             &receive_timeout, sizeof(receive_timeout)) < 0) {
-                CMVR_LOG(ERROR) << "configure CAN receive timeout failed: "
-                                << std::strerror(errno);
-                status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
-                stop();
-                return false;
-            }
-
-            struct timeval send_timeout {
-                static_cast<time_t>(send_timeout_us_ / 1000000U),
-                static_cast<suseconds_t>(send_timeout_us_ % 1000000U)
-            };
-            if (::setsockopt(dev_handler_, SOL_SOCKET, SO_SNDTIMEO,
-                             &send_timeout, sizeof(send_timeout)) < 0) {
-                CMVR_LOG(ERROR) << "configure CAN send timeout failed: "
-                                << std::strerror(errno);
-                status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
-                stop();
                 return false;
             }
 
@@ -190,40 +115,14 @@ namespace cmvr {
                 interface_prefix = "can";
             }
 
-            const std::string can_name =
+            const std::string can_name = absl::StrCat(interface_prefix, port_);
-                interface_name_.empty()
-                    ? absl::StrCat(interface_prefix, port_)
-                    : interface_name_;
-            if (can_name.size() >= IFNAMSIZ) {
-                CMVR_LOG(ERROR) << "CAN interface name is too long: " << can_name;
-                status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
-                stop();
-                return false;
-            }
             std::strncpy(ifr.ifr_name, can_name.c_str(), IFNAMSIZ);
-            ifr.ifr_name[IFNAMSIZ - 1] = '\0';
             if (ioctl(dev_handler_, SIOCGIFINDEX, &ifr) < 0) {
-                CMVR_LOG(ERROR) << "CAN interface not found: " << can_name
+                CMVR_LOG(ERROR) << "ioctl error";
-                                << ", error=" << std::strerror(errno);
                 status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
-                stop();
                 return false;
             }
 
-            if (enable_fd_) {
-                struct ifreq mtu_request {};
-                std::strncpy(mtu_request.ifr_name, can_name.c_str(), IFNAMSIZ);
-                mtu_request.ifr_name[IFNAMSIZ - 1] = '\0';
-                if (::ioctl(dev_handler_, SIOCGIFMTU, &mtu_request) < 0 ||
-                    mtu_request.ifr_mtu != CANFD_MTU) {
-                    CMVR_LOG(ERROR) << "CAN-FD requested but interface MTU is not CANFD_MTU: "
-                                    << can_name;
-                    status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
-                    stop();
-                    return false;
-                }
-            }
-
             // bind socket to network interface
 
             addr.can_family = AF_CAN;
@@ -232,10 +131,8 @@ namespace cmvr {
                          sizeof(addr));
 
             if (ret < 0) {
-                CMVR_LOG(ERROR) << "bind socket to CAN interface failed: "
+                CMVR_LOG(ERROR) << "bind socket to network interface error code: " << ret;
-                                << std::strerror(errno);
                 status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
-                stop();
                 return false;
             }
 
@@ -245,11 +142,10 @@ namespace cmvr {
         }
 
         bool SocketCanClientRaw::stop() {
-            is_started_ = false;
+            if (is_started_) {
-            if (dev_handler_ >= 0) {
+                is_started_ = false;
-                const int fd = dev_handler_;
+
-                dev_handler_ = -1;
+                int ret = close(dev_handler_);
-                int ret = close(fd);
                 if (ret < 0) {
                     CMVR_LOG(ERROR) << "close error code:" << ret << ", " << getErrorString(ret);
                     return false;
@@ -263,190 +159,48 @@ namespace cmvr {
         // Synchronous transmission of CAN messages
         ErrorCode SocketCanClientRaw::send(const std::vector<CanFrame> &frames,
                                            int32_t *const frame_num) {
-            return sendWithDeadline_(
-                frames, frame_num,
-                std::chrono::steady_clock::now() +
-                    std::chrono::microseconds(send_timeout_us_));
-        }
-
-        ErrorCode SocketCanClientRaw::sendUntil(
-            const std::vector<CanFrame>& frames,
-            int32_t* const frame_num,
-            const std::chrono::steady_clock::time_point deadline) {
-            return sendWithDeadline_(
-                frames, frame_num,
-                std::min(
-                    deadline,
-                    std::chrono::steady_clock::now() +
-                        std::chrono::microseconds(send_timeout_us_)));
-        }
-
-        ErrorCode SocketCanClientRaw::sendWithDeadline_(
-            const std::vector<CanFrame>& frames,
-            int32_t* const frame_num,
-            const std::chrono::steady_clock::time_point send_deadline) {
             if (frame_num == nullptr) {
-                CMVR_LOG(ERROR) << "frame_num is null";
+                CMVR_LOG(FATAL) << "frame_num is null";
-                return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
             }
-            if (*frame_num < 0 ||
+            if (frames.size() != static_cast<size_t>(*frame_num)) {
-                frames.size() != static_cast<size_t>(*frame_num) ||
+                CMVR_LOG(FATAL) << "frames size does not match frame_num";
-                frames.size() > static_cast<std::size_t>(MAX_CAN_SEND_FRAME_LEN)) {
-                CMVR_LOG(ERROR) << "frames size does not match a valid frame_num";
-                return ErrorCode::CAN_CLIENT_ERROR_FRAME_NUM;
             }
 
             if (!is_started_) {
                 CMVR_LOG(ERROR) << "Nvidia can client has not been initiated! Please init first!";
                 return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
             }
-            if (std::chrono::steady_clock::now() >= send_deadline) {
+            for (size_t i = 0; i < frames.size() && i < MAX_CAN_SEND_FRAME_LEN; ++i) {
-                *frame_num = 0;
+                if (frames[i].len > CANBUS_MESSAGE_LENGTH || frames[i].len < 0) {
-                return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
+                    CMVR_LOG(ERROR) << "frames[" << i << "].len = " << frames[i].len
-            }
+                            << ", which is not equal to can message data length ("
-
+                            << CANBUS_MESSAGE_LENGTH << ").";
-            // Validate the complete batch before committing its first frame.
-            // This prevents a malformed later element from causing a valid
-            // prefix of a cyclic command batch to reach the bus.
-            for (size_t i = 0; i < frames.size(); ++i) {
-                const auto& source = frames[i];
-                const auto max_length =
-                    source.is_fd ? CANFD_MESSAGE_LENGTH
-                                 : CANBUS_MESSAGE_LENGTH;
-                if (source.len > max_length ||
-                    (source.is_remote_frame && source.is_fd) ||
-                    (source.is_fd && !enable_fd_)) {
-                    *frame_num = 0;
-                    CMVR_LOG(ERROR) << "invalid CAN frame at index " << i
-                                    << ", len=" << static_cast<int>(source.len)
-                                    << ", fd=" << source.is_fd;
                     return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
                 }
-            }
+                if (frames[i].id > CAN_STANDARD_MAX_ID) {
-
+                    send_frames_[i].can_id = (frames[i].id & CAN_EFF_MASK) | CAN_EFF_FLAG;
-            int32_t sent_count = 0;
-            for (size_t i = 0; i < frames.size(); ++i) {
-                const auto& source = frames[i];
-                if (std::chrono::steady_clock::now() >= send_deadline) {
-                    *frame_num = sent_count;
-                    CMVR_LOG(ERROR)
-                        << "can " << port_
-                        << " send batch timed out before frame " << i;
-                    return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
-                }
-
-                canid_t can_id = source.is_extended_id ||
-                                         source.id > CAN_STANDARD_MAX_ID
-                                     ? (source.id & CAN_EFF_MASK) | CAN_EFF_FLAG
-                                     : (source.id & CAN_SFF_MASK);
-                if (source.is_remote_frame) {
-                    can_id |= CAN_RTR_FLAG;
-                }
-                if (source.is_error_frame) {
-                    can_id = (source.id & CAN_ERR_MASK) | CAN_ERR_FLAG;
-                }
-
-                const void* payload = nullptr;
-                std::size_t expected = 0;
-                struct canfd_frame fd_frame {};
-                struct can_frame classic_frame {};
-                if (source.is_fd) {
-                    fd_frame.can_id = can_id;
-                    fd_frame.len = source.len;
-                    if (source.bitrate_switch || default_bitrate_switch_) {
-                        fd_frame.flags |= CANFD_BRS;
-                    }
-                    if (source.error_state_indicator) {
-                        fd_frame.flags |= CANFD_ESI;
-                    }
-                    std::memcpy(fd_frame.data, source.data, source.len);
-                    expected = CANFD_MTU;
-                    payload = &fd_frame;
                 } else {
-                    classic_frame.can_id = can_id;
+                    send_frames_[i].can_id = (frames[i].id & CAN_SFF_MASK);
-                    classic_frame.can_dlc = source.len;
-                    std::memcpy(
-                        classic_frame.data, source.data, source.len);
-                    expected = CAN_MTU;
-                    payload = &classic_frame;
                 }
+                // CMVR_LOG(INFO) << "send can id is " << send_frames_[i].can_id;
+                send_frames_[i].can_dlc = frames[i].len;
+                std::memcpy(send_frames_[i].data, frames[i].data, frames[i].len);
 
-                while (true) {
+                // Synchronous transmission of CAN messages
-                    const auto written = ::send(
+                int ret = static_cast<int>(
-                        dev_handler_, payload, expected,
+                    write(dev_handler_, &send_frames_[i], sizeof(send_frames_[i])));
-                        MSG_DONTWAIT | MSG_NOSIGNAL);
+                if (ret <= 0) {
-                    if (written == static_cast<ssize_t>(expected)) {
+                    CMVR_LOG(ERROR) << "can " << port_ << " send message failed, error code: " << ret;
-                        ++sent_count;
+                    return ErrorCode::CAN_CLIENT_ERROR_BASE;
-                        break;
-                    }
-                    if (written >= 0) {
-                        *frame_num = sent_count;
-                        CMVR_LOG(ERROR)
-                            << "can " << port_
-                            << " sent a partial frame";
-                        return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
-                    }
-                    if (errno == EINTR) {
-                        continue;
-                    }
-                    if (errno != EAGAIN && errno != EWOULDBLOCK) {
-                        *frame_num = sent_count;
-                        CMVR_LOG(ERROR) << "can " << port_
-                                        << " send message failed: "
-                                        << std::strerror(errno);
-                        return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
-                    }
-
-                    const auto now = std::chrono::steady_clock::now();
-                    if (now >= send_deadline) {
-                        *frame_num = sent_count;
-                        CMVR_LOG(ERROR)
-                            << "can " << port_
-                            << " send batch timed out";
-                        return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
-                    }
-                    const auto remaining =
-                        std::chrono::duration_cast<std::chrono::nanoseconds>(
-                            send_deadline - now);
-                    struct timespec timeout {
-                        static_cast<time_t>(
-                            remaining.count() / 1000000000LL),
-                        static_cast<long>(
-                            remaining.count() % 1000000000LL)
-                    };
-                    struct pollfd writable {
-                        dev_handler_, POLLOUT, 0
-                    };
-                    const int ready =
-                        ::ppoll(&writable, 1, &timeout, nullptr);
-                    if (ready == 0) {
-                        *frame_num = sent_count;
-                        CMVR_LOG(ERROR)
-                            << "can " << port_
-                            << " send batch timed out";
-                        return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
-                    }
-                    if (ready < 0 && errno != EINTR) {
-                        *frame_num = sent_count;
-                        CMVR_LOG(ERROR)
-                            << "can " << port_
-                            << " send poll failed: "
-                            << std::strerror(errno);
-                        return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
-                    }
                 }
             }
 
-            *frame_num = sent_count;
             return ErrorCode::OK;
         }
 
         // buf size must be 8 bytes, every time, we receive only one frame
         ErrorCode SocketCanClientRaw::receive(std::vector<CanFrame> *const frames,
                                               int32_t *const frame_num) {
-            if (frames == nullptr || frame_num == nullptr) {
-                return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
-            }
             if (!is_started_) {
                 CMVR_LOG(ERROR) << "Nvidia can client is not init! Please init first!";
                 return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
@@ -459,109 +213,39 @@ namespace cmvr {
                 return ErrorCode::CAN_CLIENT_ERROR_FRAME_NUM;
             }
 
-            frames->clear();
+            for (int32_t i = 0; i < *frame_num && i < MAX_CAN_RECV_FRAME_LEN; ++i) {
-            const int32_t requested = *frame_num;
-            *frame_num = 0;
-            for (int32_t i = 0; i < requested && i < MAX_CAN_RECV_FRAME_LEN; ++i) {
                 CanFrame cf;
-                struct canfd_frame raw {};
+                auto ret = read(dev_handler_, &recv_frames_[i], sizeof(recv_frames_[i]));
-                const auto ret = ::read(dev_handler_, &raw, CANFD_MTU);
 
                 if (ret < 0) {
-                    if (errno == EAGAIN || errno == EWOULDBLOCK ||
+                    CMVR_LOG(ERROR) << "receive message failed, error code: " << ret;
-                        errno == EINTR) {
+                    return ErrorCode::CAN_CLIENT_ERROR_BASE;
-                        return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
+                }
-                    }
+                if (recv_frames_[i].can_dlc > CANBUS_MESSAGE_LENGTH ||
-                    CMVR_LOG(ERROR) << "receive CAN message failed: "
+                    recv_frames_[i].can_dlc < 0) {
-                                    << std::strerror(errno);
+                    CMVR_LOG(ERROR) << "recv_frames_[" << i
+                            << "].can_dlc = " << recv_frames_[i].can_dlc
+                            << ", which is not equal to can message data length ("
+                            << CANBUS_MESSAGE_LENGTH << ").";
                     return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
                 }
-                if (ret != CAN_MTU && ret != CANFD_MTU) {
+                if (recv_frames_[i].can_id > CAN_STANDARD_MAX_ID) {
-                    CMVR_LOG(ERROR) << "unexpected SocketCAN MTU: " << ret;
+                    cf.id = enable_can_err_check_
-                    return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
+                                ? recv_frames_[i].can_id & CAN_EFF_MASK | CAN_ERR_FLAG
-                }
+                                : recv_frames_[i].can_id & CAN_EFF_MASK;
-
-                const canid_t raw_id = raw.can_id;
-                cf.is_extended_id = (raw_id & CAN_EFF_FLAG) != 0;
-                cf.is_remote_frame = (raw_id & CAN_RTR_FLAG) != 0;
-                cf.is_error_frame = (raw_id & CAN_ERR_FLAG) != 0;
-                if (cf.is_error_frame) {
-                    cf.id = raw_id & CAN_ERR_MASK;
-                } else if (cf.is_extended_id) {
-                    cf.id = raw_id & CAN_EFF_MASK;
                 } else {
-                    cf.id = raw_id & CAN_SFF_MASK;
+                    cf.id = (recv_frames_[i].can_id & CAN_SFF_MASK);
                 }
-
+                // CMVR_LOG(INFO) << "Socket can receive can id is " << recv_frames_[i].can_id;
-                cf.is_fd = ret == CANFD_MTU;
+                cf.len = recv_frames_[i].can_dlc;
-                if (cf.is_fd) {
+                std::memcpy(cf.data, recv_frames_[i].data, recv_frames_[i].can_dlc);
-                    cf.len = raw.len;
-                    cf.bitrate_switch = (raw.flags & CANFD_BRS) != 0;
-                    cf.error_state_indicator = (raw.flags & CANFD_ESI) != 0;
-                } else {
-                    const auto* classic =
-                        reinterpret_cast<const struct can_frame*>(&raw);
-                    cf.len = classic->can_dlc;
-                }
-                const auto max_length =
-                    cf.is_fd ? CANFD_MESSAGE_LENGTH : CANBUS_MESSAGE_LENGTH;
-                if (cf.len > max_length) {
-                    return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
-                }
-                std::memcpy(cf.data, raw.data, cf.len);
-
-                struct timespec monotonic {};
-                if (::clock_gettime(CLOCK_MONOTONIC, &monotonic) == 0) {
-                    cf.rx_monotonic_ns =
-                        static_cast<int64_t>(monotonic.tv_sec) * 1000000000LL +
-                        monotonic.tv_nsec;
-                }
-                ::gettimeofday(&cf.timestamp, nullptr);
                 frames->push_back(cf);
-                ++(*frame_num);
             }
             return ErrorCode::OK;
         }
 
-        bool SocketCanClientRaw::discardPendingFrames() {
+        std::string SocketCanClientRaw::getErrorString(const int32_t /*status*/) {
-            if (!is_started_ || dev_handler_ < 0) {
+            return "";
-                return false;
-            }
-
-            constexpr std::size_t kMaximumDrainFrames = 4096;
-            const auto deadline =
-                std::chrono::steady_clock::now() +
-                std::chrono::microseconds(send_timeout_us_);
-            std::size_t count = 0;
-            while (count < kMaximumDrainFrames &&
-                   std::chrono::steady_clock::now() < deadline) {
-                struct canfd_frame raw {};
-                const auto received = ::recv(
-                    dev_handler_, &raw, CANFD_MTU, MSG_DONTWAIT);
-                if (received == CAN_MTU || received == CANFD_MTU) {
-                    ++count;
-                    continue;
-                }
-                if (received < 0 &&
-                    (errno == EAGAIN || errno == EWOULDBLOCK)) {
-                    return true;
-                }
-                if (received < 0 && errno == EINTR) {
-                    continue;
-                }
-                CMVR_LOG(ERROR)
-                    << "failed while draining pending CAN frames: "
-                    << (received < 0 ? std::strerror(errno)
-                                     : "unexpected MTU");
-                return false;
-            }
-            CMVR_LOG(ERROR)
-                << "CAN receive queue did not drain within its bound";
-            return false;
-        }
-
-        std::string SocketCanClientRaw::getErrorString(const int32_t status) {
-            return std::strerror(status < 0 ? -status : status);
         }
     }
 }

cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h

@@ -12,13 +12,11 @@
 #include <sys/types.h>
 
 #include <linux/can.h>
-#include <linux/can/error.h>
 #include <linux/can/raw.h>
 
 #include <cstdio>
 #include <cstdlib>
 #include <cstring>
-#include <cstdint>
 #include <string>
 #include <vector>
 
@@ -53,10 +51,6 @@ namespace cmvr {
              */
             cmvr::msgs::ErrorCode send(const std::vector<CanFrame> &frames,
                                            int32_t *const frame_num) override;
-            cmvr::msgs::ErrorCode sendUntil(
-                const std::vector<CanFrame>& frames,
-                int32_t* const frame_num,
-                std::chrono::steady_clock::time_point deadline) override;
 
             /**
              * @brief Receive messages
@@ -66,7 +60,6 @@ namespace cmvr {
              */
             cmvr::msgs::ErrorCode receive(std::vector<CanFrame> *const frames,
                                               int32_t *const frame_num) override;
-            bool discardPendingFrames() override;
 
             /**
              * @brief Get the error string.
@@ -74,23 +67,14 @@ namespace cmvr {
              */
             std::string getErrorString(const int32_t status) override;
         private:
-            int dev_handler_{-1};
+            int dev_handler_ = 0;
             cmvr::msgs::CANCardParameter::CANChannelId port_;
             cmvr::msgs::CANCardParameter::CANInterface interface_;
-            std::string interface_name_;
+            can_frame send_frames_[MAX_CAN_SEND_FRAME_LEN];
-            bool enable_fd_{false};
+            can_frame recv_frames_[MAX_CAN_RECV_FRAME_LEN];
-            bool default_bitrate_switch_{false};
-            bool receive_own_messages_{false};
-            uint32_t receive_timeout_us_{100000};
-            uint32_t send_timeout_us_{100000};
 
             //
             bool enable_can_err_check_{false};
-
-            cmvr::msgs::ErrorCode sendWithDeadline_(
-                const std::vector<CanFrame>& frames,
-                int32_t* frame_num,
-                std::chrono::steady_clock::time_point deadline);
         };
     }
 }

cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc

@@ -1,226 +1,44 @@
+#include "common/base/logging/logger.h"
+//
+// Created by lgv on 2025/7/16.
+//
+#include "cmvr/msgs/error_code.pb.h"
+#include "cmvr/msgs/can_card_parameter.pb.h"
 #include "canbus/can_client/socket/socket_can_client_raw.h"
+#include "gtest/gtest.h"
+namespace cmvr {
+namespace device {
+    using cmvr::msgs::ErrorCode;
+    using cmvr::msgs::CANCardParameter;
 
-#include <algorithm>
+    TEST(SocketCanClientRawTest, simple_test) {
-#include <chrono>
+        CANCardParameter param;
-#include <filesystem>
+        param.set_brand(CANCardParameter::SOCKET_CAN_RAW);
-#include <iterator>
+        param.set_channel_id(CANCardParameter::CHANNEL_ID_ZERO);
-#include <string>
-#include <thread>
-#include <vector>
 
-#include <gtest/gtest.h>
+        cmvr::config::SocketCanConfig cfg;
+        cfg.set_channel_id(0);
+        SocketCanClientRaw socket_can_client(cfg);
 
-namespace cmvr::device {
+        // EXPECT_EQ(socket_can_client.start(), ErrorCode::CAN_CLIENT_ERROR_BASE);
-namespace {
+        socket_can_client.start();
-
+        std::vector<CanFrame> frames;
-std::size_t openFileDescriptorCount()
+        int32_t num = 0;
-{
+        EXPECT_EQ(socket_can_client.send(frames, &num),
-    std::error_code error;
+                  ErrorCode::OK);
-    std::size_t count = 0;
+        ++num;
-    for (std::filesystem::directory_iterator iterator(
+        EXPECT_EQ(socket_can_client.receive(&frames, &num),
-             "/proc/self/fd", error);
+                  ErrorCode::OK);
-         !error && iterator != std::filesystem::directory_iterator();
+        CMVR_LOG(INFO) << frames.at(0).CanFrameString();
-         iterator.increment(error)) {
+        CanFrame can_frame;
-        ++count;
+        can_frame.id = 0x123;
-    }
+        can_frame.len = 8;
-    return error ? 0U : count;
+        memset(can_frame.data, 0xA3, sizeof(can_frame.data));
-}
+        frames.clear();
-
+        frames.push_back(can_frame);
-config::SocketCanConfig vcanConfig(const bool enable_fd)
+        EXPECT_EQ(socket_can_client.sendSingleFrame(frames),
-{
+                  ErrorCode::OK);
-    config::SocketCanConfig config;
+        socket_can_client.stop();
-    config.set_interface_name("vcan0");
-    config.set_enable_fd(enable_fd);
-    config.set_bitrate_switch(enable_fd);
-    config.set_receive_own_messages(false);
-    config.set_receive_timeout_us(2000U);
-    config.set_send_timeout_us(2000U);
-    return config;
-}
-
-bool vcanAvailable()
-{
-    return ::if_nametoindex("vcan0") != 0U;
-}
-
-TEST(SocketCanClientRawTest, MissingClassicInterfaceFailsWithoutLeakingFd)
-{
-    config::SocketCanConfig config;
-    config.set_interface_name("cmvr_no_such_can");
-    config.set_enable_fd(false);
-    config.set_receive_timeout_us(100U);
-    config.set_send_timeout_us(100U);
-    SocketCanClientRaw client(config);
-
-    const auto before = openFileDescriptorCount();
-    ASSERT_GT(before, 0U);
-    for (int attempt = 0; attempt < 32; ++attempt) {
-        EXPECT_FALSE(client.start());
-        EXPECT_TRUE(client.stop());
-    }
-    const auto after = openFileDescriptorCount();
-    EXPECT_LE(after, before + 1U);
-}
-
-TEST(SocketCanClientRawTest, ClosedClientRejectsClassicSendAndReceive)
-{
-    config::SocketCanConfig config;
-    config.set_interface_name("cmvr_no_such_can");
-    config.set_enable_fd(false);
-    SocketCanClientRaw client(config);
-
-    CanFrame frame;
-    frame.id = 0x123U;
-    frame.len = 8U;
-    frame.is_fd = false;
-    std::fill(std::begin(frame.data), std::end(frame.data), 0xA3U);
-    std::vector<CanFrame> frames{frame};
-    int32_t count = 1;
-    EXPECT_EQ(
-        client.send(frames, &count),
-        msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED);
-
-    count = 1;
-    EXPECT_EQ(
-        client.receive(&frames, &count),
-        msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED);
-    EXPECT_NE(frame.CanFrameString().find("fd:0"), std::string::npos);
-}
-
-TEST(SocketCanClientRawTest, VcanTransmitsClassicAndCanFdBatches)
-{
-    if (!vcanAvailable()) {
-        GTEST_SKIP() << "vcan0 is not available in this network namespace";
-    }
-
-    SocketCanClientRaw classic_tx(vcanConfig(false));
-    SocketCanClientRaw fd_rx(vcanConfig(true));
-    ASSERT_TRUE(classic_tx.start());
-    ASSERT_TRUE(fd_rx.start());
-
-    CanFrame first;
-    first.id = 0x123U;
-    first.len = 8U;
-    first.data[0] = 0xA1U;
-    CanFrame second;
-    second.id = 0x456U;
-    second.len = 3U;
-    second.data[0] = 0xB2U;
-    std::vector<CanFrame> classic_frames{first, second};
-    int32_t count = 2;
-    ASSERT_EQ(
-        classic_tx.send(classic_frames, &count),
-        msgs::ErrorCode::OK);
-    ASSERT_EQ(count, 2);
-
-    for (const auto& expected : classic_frames) {
-        std::vector<CanFrame> received;
-        int32_t receive_count = 1;
-        ASSERT_EQ(
-            fd_rx.receive(&received, &receive_count),
-            msgs::ErrorCode::OK);
-        ASSERT_EQ(receive_count, 1);
-        ASSERT_EQ(received.size(), 1U);
-        EXPECT_FALSE(received.front().is_fd);
-        EXPECT_EQ(received.front().id, expected.id);
-        EXPECT_EQ(received.front().len, expected.len);
-        EXPECT_EQ(received.front().data[0], expected.data[0]);
-    }
-    ASSERT_TRUE(classic_tx.stop());
-    ASSERT_TRUE(fd_rx.stop());
-
-    SocketCanClientRaw fd_tx(vcanConfig(true));
-    SocketCanClientRaw second_fd_rx(vcanConfig(true));
-    ASSERT_TRUE(fd_tx.start());
-    ASSERT_TRUE(second_fd_rx.start());
-    CanFrame fd_first;
-    fd_first.id = 0x201U;
-    fd_first.len = 12U;
-    fd_first.is_fd = true;
-    fd_first.bitrate_switch = true;
-    fd_first.data[11] = 0xC3U;
-    CanFrame fd_second;
-    fd_second.id = 0x202U;
-    fd_second.len = 64U;
-    fd_second.is_fd = true;
-    fd_second.bitrate_switch = true;
-    fd_second.data[63] = 0xD4U;
-    std::vector<CanFrame> fd_frames{fd_first, fd_second};
-    count = 2;
-    ASSERT_EQ(fd_tx.send(fd_frames, &count), msgs::ErrorCode::OK);
-    ASSERT_EQ(count, 2);
-
-    for (const auto& expected : fd_frames) {
-        std::vector<CanFrame> received;
-        int32_t receive_count = 1;
-        ASSERT_EQ(
-            second_fd_rx.receive(&received, &receive_count),
-            msgs::ErrorCode::OK);
-        ASSERT_EQ(received.size(), 1U);
-        EXPECT_TRUE(received.front().is_fd);
-        EXPECT_TRUE(received.front().bitrate_switch);
-        EXPECT_EQ(received.front().id, expected.id);
-        EXPECT_EQ(received.front().len, expected.len);
-        EXPECT_EQ(
-            received.front().data[expected.len - 1U],
-            expected.data[expected.len - 1U]);
     }
 }
-
-TEST(SocketCanClientRawTest, VcanDrainAndBatchValidationAreFailClosed)
-{
-    if (!vcanAvailable()) {
-        GTEST_SKIP() << "vcan0 is not available in this network namespace";
-    }
-
-    SocketCanClientRaw tx(vcanConfig(false));
-    SocketCanClientRaw rx(vcanConfig(false));
-    ASSERT_TRUE(tx.start());
-    ASSERT_TRUE(rx.start());
-
-    CanFrame valid;
-    valid.id = 0x321U;
-    valid.len = 8U;
-    valid.data[0] = 0x5AU;
-    std::vector<CanFrame> one{valid};
-    int32_t count = 1;
-    ASSERT_EQ(tx.send(one, &count), msgs::ErrorCode::OK);
-    std::this_thread::sleep_for(std::chrono::milliseconds(1));
-    ASSERT_TRUE(rx.discardPendingFrames());
-
-    std::vector<CanFrame> received;
-    int32_t receive_count = 1;
-    EXPECT_EQ(
-        rx.receive(&received, &receive_count),
-        msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED);
-
-    CanFrame invalid = valid;
-    invalid.id = 0x322U;
-    invalid.len = 9U;
-    std::vector<CanFrame> invalid_batch{valid, invalid};
-    count = 2;
-    EXPECT_EQ(
-        tx.send(invalid_batch, &count),
-        msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED);
-    EXPECT_EQ(count, 0);
-    receive_count = 1;
-    EXPECT_EQ(
-        rx.receive(&received, &receive_count),
-        msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED);
-
-    count = 1;
-    EXPECT_EQ(
-        tx.sendUntil(
-            one, &count,
-            std::chrono::steady_clock::now() -
-                std::chrono::microseconds(1)),
-        msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED);
-    EXPECT_EQ(count, 0);
-    receive_count = 1;
-    EXPECT_EQ(
-        rx.receive(&received, &receive_count),
-        msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED);
 }
-
-} // namespace
-} // namespace cmvr::device

cmvr-es/devices/canbus/common/canbus_consts.h

@@ -26,14 +26,9 @@
 namespace cmvr {
     namespace device {
         const int32_t CAN_FRAME_SIZE = 8;
-        const int32_t CAN_FD_FRAME_SIZE = 64;
+        const int32_t MAX_CAN_SEND_FRAME_LEN = 1;
-        // One UME cycle may submit a complete arm worth of frames. The receive
-        // API intentionally remains one-frame-at-a-time so a caller never
-        // blocks waiting to fill an artificial batch.
-        const int32_t MAX_CAN_SEND_FRAME_LEN = 64;
         const int32_t MAX_CAN_RECV_FRAME_LEN = 1;     // 这个暂时改为 1 ,大量数据的时候改为 10
 
         const int32_t CANBUS_MESSAGE_LENGTH = 8; // according to ISO-11891-1
-        const int32_t CANFD_MESSAGE_LENGTH = 64;
     }
 }

cmvr-es/devices/motor/CMakeLists.txt

@@ -17,52 +17,4 @@ 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)

cmvr-es/devices/motor/bus_runtime/CMakeLists.txt

@@ -2,29 +2,20 @@ 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
+target_link_directories(motor_bus_runtime PRIVATE ${IGH_ETHERCAT_ROOT}/lib)
-        ${IGH_ETHERCAT_ROOT}/lib
-        ${CMVR_LIBMODBUS_ROOT}/lib
-)
 
 target_link_libraries(motor_bus_runtime
         PUBLIC
@@ -33,7 +24,6 @@ target_link_libraries(motor_bus_runtime
         cmvr_es::mujoco_world
         PRIVATE
         ethercat
-        modbus
         cmvr_es::device::canbus
         glog
 )

cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp

@@ -68,18 +68,16 @@ bool CanMotorBusRuntime::start()
         return false;
     }
 
-    // Start the receiver first so a protocol response cannot arrive before the
+    auto ret = sender_->Start();
-    // receive path is ready.
-    auto ret = receiver_->Start();
     if (ret != ErrorCode::OK) {
-        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_;
+        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_;
         stop();
         return false;
     }
 
-    ret = sender_->Start();
+    ret = receiver_->Start();
     if (ret != ErrorCode::OK) {
-        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_;
+        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_;
         stop();
         return false;
     }

cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h

@@ -1,209 +0,0 @@
-#ifndef CMVR_ES_CMVR_PLC_REGISTER_MAP_H
-#define CMVR_ES_CMVR_PLC_REGISTER_MAP_H
-
-#include <array>
-#include <cstddef>
-#include <cstdint>
-
-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<int>(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<std::uint16_t>(value >> 16U);
-    registers[offset + 1] = static_cast<std::uint16_t>(value & 0xffffU);
-}
-
-inline void encodeInt32(std::uint16_t* registers,
-                        const std::size_t offset,
-                        const std::int32_t value)
-{
-    encodeUint32(registers, offset, static_cast<std::uint32_t>(value));
-}
-
-inline std::uint32_t decodeUint32(const std::uint16_t* registers,
-                                  const std::size_t offset)
-{
-    return (static_cast<std::uint32_t>(registers[offset]) << 16U) |
-           static_cast<std::uint32_t>(registers[offset + 1]);
-}
-
-inline std::int32_t decodeInt32(const std::uint16_t* registers,
-                               const std::size_t offset)
-{
-    return static_cast<std::int32_t>(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

cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h

@@ -1,47 +0,0 @@
-#ifndef CMVR_ES_MODBUS_TCP_CLIENT_H
-#define CMVR_ES_MODBUS_TCP_CLIENT_H
-
-#include <cstdint>
-#include <atomic>
-#include <string>
-#include <vector>
-
-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<std::uint16_t>& values);
-    bool writeHoldingRegisters(int address, const std::uint16_t* values, int count);
-    bool writeHoldingRegisters(int address, const std::vector<std::uint16_t>& values);
-
-    const std::string& lastError() const { return last_error_; }
-
-private:
-    void setLastErrnoError_(const char* operation);
-
-    modbus_t* context_{nullptr};
-    std::atomic<int> socket_fd_{-1};
-    std::string last_error_;
-};
-
-} // namespace cmvr::device
-
-#endif // CMVR_ES_MODBUS_TCP_CLIENT_H

cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h

@@ -1,173 +0,0 @@
-#ifndef CMVR_ES_MODBUS_TCP_MOTOR_BUS_RUNTIME_H
-#define CMVR_ES_MODBUS_TCP_MOTOR_BUS_RUNTIME_H
-
-#include <array>
-#include <atomic>
-#include <chrono>
-#include <condition_variable>
-#include <cstdint>
-#include <memory>
-#include <mutex>
-#include <optional>
-#include <string>
-#include <thread>
-#include <unordered_map>
-#include <vector>
-
-#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<std::uint64_t>
-                               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<std::uint16_t>& 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<std::uint64_t>
-                                    expected_connection_epoch);
-    bool writeClientHeartbeatLocked_();
-    std::shared_ptr<std::mutex> axisMutex_(std::uint8_t motor_id) const;
-    std::shared_ptr<std::mutex> 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<std::uint8_t, std::uint32_t> motor_axes_;
-    mutable std::unordered_map<std::uint8_t, std::shared_ptr<std::mutex>> axis_mutexes_;
-    mutable std::unordered_map<std::uint8_t, std::shared_ptr<std::mutex>> safety_mutexes_;
-    std::unordered_map<std::uint8_t, std::uint32_t> command_sequences_;
-    std::unordered_map<std::uint8_t, std::uint64_t> 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<bool> running_{false};
-    std::atomic<bool> connected_{false};
-    std::atomic<std::uint32_t> plc_boot_id_{0};
-    std::atomic<std::uint64_t> connection_epoch_{0};
-    std::atomic<std::uint32_t> 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

cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_client.cpp

@@ -1,188 +0,0 @@
-#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h"
-
-#include <cerrno>
-#include <limits>
-#include <fcntl.h>
-#include <netdb.h>
-#include <poll.h>
-#include <sys/socket.h>
-#include <unistd.h>
-
-#include <modbus/modbus.h>
-
-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<int>(port));
-    if (!context_) {
-        last_error_ = "modbus_new_tcp failed";
-        return false;
-    }
-    if (modbus_set_slave(context_, static_cast<int>(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<int>(
-                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<std::uint16_t>& 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<std::size_t>(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<std::uint16_t>& values)
-{
-    if (values.size() > static_cast<std::size_t>(std::numeric_limits<int>::max())) {
-        last_error_ = "holding-register write is too large";
-        return false;
-    }
-    return writeHoldingRegisters(address, values.data(), static_cast<int>(values.size()));
-}
-
-void ModbusTcpClient::setLastErrnoError_(const char* operation)
-{
-    last_error_ = std::string(operation) + ": " + modbus_strerror(errno);
-}
-
-} // namespace cmvr::device

cmvr-es/devices/motor/drivers/modbus_plc_motor/CMakeLists.txt

@@ -1,21 +0,0 @@
-add_library(modbus_plc_motor_driver SHARED
-        src/cmvr_plc_motor_protocol.cpp
-        src/modbus_plc_motor.cpp
-)
-
-target_include_directories(modbus_plc_motor_driver
-        PUBLIC
-        ${CMAKE_CURRENT_SOURCE_DIR}/include
-)
-
-target_link_libraries(modbus_plc_motor_driver
-        PUBLIC
-        cmvr_es::device::motor_core
-        cmvr_es::device::motor_bus_runtime
-        PRIVATE
-        cmvr_es::proto
-        glog
-)
-
-add_library(cmvr_es::device::modbus_plc_motor_driver ALIAS modbus_plc_motor_driver)
-install(TARGETS modbus_plc_motor_driver LIBRARY DESTINATION lib)

cmvr-es/devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h

@@ -1,89 +0,0 @@
-#ifndef CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H
-#define CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H
-
-#include <cstdint>
-#include <memory>
-#include <mutex>
-#include <optional>
-#include <unordered_map>
-
-#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<ModbusTcpMotorBusRuntime> 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<std::int32_t> 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<ModbusTcpMotorBusRuntime> bus_runtime_;
-    std::mutex nodes_mutex_;
-    std::unordered_map<std::uint8_t, NodeState> nodes_;
-};
-
-} // namespace cmvr::device
-
-#endif // CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H

cmvr-es/devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h

@@ -1,21 +0,0 @@
-#ifndef CMVR_ES_MODBUS_PLC_MOTOR_H
-#define CMVR_ES_MODBUS_PLC_MOTOR_H
-
-#include "cmvr/config/motor_config/motor_config.pb.h"
-#include "devices/motor/abstract_motor.h"
-
-namespace cmvr::device {
-
-class ModbusPlcMotor final : public AbstractMotor {
-public:
-    explicit ModbusPlcMotor(const config::MotorConfigItem& config);
-
-    std::string typeName() const override { return "ModbusPlcMotor"; }
-    bool init() override;
-    bool torqueOff() override;
-    bool quickStop() override;
-};
-
-} // namespace cmvr::device
-
-#endif // CMVR_ES_MODBUS_PLC_MOTOR_H

cmvr-es/devices/motor/drivers/modbus_plc_motor/src/cmvr_plc_motor_protocol.cpp

@@ -1,582 +0,0 @@
-#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h"
-
-#include <algorithm>
-#include <cmath>
-#include <limits>
-
-#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<std::uint16_t>(status.command_state) <=
-               static_cast<std::uint16_t>(
-                   cmvr_plc::CommandState::CommunicationLost) &&
-           !cmvr_plc::isFailure(status.command_state);
-}
-
-} // namespace
-
-CmvrPlcMotorProtocol::CmvrPlcMotorProtocol(
-    std::shared_ptr<ModbusTcpMotorBusRuntime> 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<int>(node_id);
-        return false;
-    }
-    std::lock_guard<std::mutex> 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<std::mutex> 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<msgs::RunMode>(status.current_mode);
-        if (isSupportedMode_(mode)) {
-            return mode;
-        }
-    }
-    std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<int>(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<std::mutex> 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<int>(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<std::mutex> 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<double>::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<double>::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<std::mutex> 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<int>(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<std::int32_t> 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<double>(std::numeric_limits<std::int32_t>::min()) ||
-        scaled > static_cast<double>(std::numeric_limits<std::int32_t>::max())) {
-        return std::nullopt;
-    }
-    return static_cast<std::int32_t>(scaled);
-}
-
-double CmvrPlcMotorProtocol::fromMicroUnits_(const std::int32_t value)
-{
-    return static_cast<double>(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

cmvr-es/devices/motor/drivers/modbus_plc_motor/src/modbus_plc_motor.cpp

@@ -1,55 +0,0 @@
-#include "devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h"
-
-#include "common/base/logging/logger.h"
-#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h"
-#include <cmath>
-
-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<std::uint8_t>(config.id());
-}
-
-bool ModbusPlcMotor::init()
-{
-    if (!protocol_ ||
-        !std::dynamic_pointer_cast<CmvrPlcMotorProtocol>(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<CmvrPlcMotorProtocol>(protocol_);
-    return protocol && protocol->torqueOff(node_id_);
-}
-
-bool ModbusPlcMotor::quickStop()
-{
-    auto protocol = std::dynamic_pointer_cast<CmvrPlcMotorProtocol>(protocol_);
-    return protocol && protocol->quickStop(node_id_);
-}
-
-} // namespace cmvr::device

cmvr-es/devices/motor/manager/CMakeLists.txt

@@ -13,7 +13,6 @@ target_link_libraries(motor_manager
         cmvr_es::device::ti5_canopen_motor_driver
         cmvr_es::device::mujoco_motor_driver
         cmvr_es::device::ethercat_motor_driver
-        cmvr_es::device::modbus_plc_motor_driver
         cmvr_es::ik_solver
         glog
 )

cmvr-es/devices/motor/manager/include/motor_manager.h

@@ -78,10 +78,6 @@ private:
         const config::MotorGroupConfig& group_cfg,
         const std::vector<config::MotorConfigItem>& motor_cfgs,
         const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const;
-    std::vector<std::shared_ptr<AbstractMotor>> createModbusTcpMotors_(
-        const config::MotorGroupConfig& group_cfg,
-        const std::vector<config::MotorConfigItem>& motor_cfgs,
-        const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const;
 
 private:
     config::MotorConfig cfg_;

cmvr-es/devices/motor/manager/src/motor_manager.cpp

@@ -15,14 +15,11 @@
 #include "devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h"
 #include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
 #include "devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h"
-#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h"
 #include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
 #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h"
 #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
 #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
 #include "devices/motor/drivers/mujoco/include/mujoco_motor.h"
-#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h"
-#include "devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h"
 #include "devices/motor/drivers/ti5_canopen/include/ti5_motor.h"
 #include "devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h"
 
@@ -418,16 +415,6 @@ std::shared_ptr<AbstractMotorBusRuntime> MotorManager::createBusRuntime_(
             return std::make_shared<CanMotorBusRuntime>();
         case config::MOTOR_BUS_MUJOCO:
             return std::make_shared<MujocoMotorBusRuntime>();
-        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<ModbusTcpMotorBusRuntime>();
-            }
-            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<EthercatMotorBusRuntime>();
             if (group_cfg.vendor() == config::MOTOR_VENDOR_EYOU &&
@@ -467,8 +454,6 @@ std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createMotors_(
             return createMujocoMotors_(group_cfg, motor_cfgs, bus_runtime);
         case config::MOTOR_BUS_ETHERCAT:
             return createEthercatMotors_(group_cfg, motor_cfgs, bus_runtime);
-        case config::MOTOR_BUS_MODBUS_TCP:
-            return createModbusTcpMotors_(group_cfg, motor_cfgs, bus_runtime);
         default:
             CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: "
                             << config::MotorBusType_Name(group_cfg.bus_type())
@@ -611,50 +596,4 @@ std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createEthercatMotors_(
     return motors;
 }
 
-std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createModbusTcpMotors_(
-    const config::MotorGroupConfig& group_cfg,
-    const std::vector<config::MotorConfigItem>& motor_cfgs,
-    const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const
-{
-    auto modbus_runtime =
-        std::dynamic_pointer_cast<ModbusTcpMotorBusRuntime>(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<std::uint8_t>(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<CmvrPlcMotorProtocol>(modbus_runtime);
-    std::vector<std::shared_ptr<AbstractMotor>> motors;
-    motors.reserve(motor_cfgs.size());
-    for (const auto& cfg : motor_cfgs) {
-        auto motor = std::make_shared<ModbusPlcMotor>(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

cmvr-es/main.cpp

@@ -1,9 +1,5 @@
 #include <csignal>
-#include <cmath>
-#include <cstdlib>
-#include <iostream>
 #include <pthread.h>
-#include <string>
 
 #include "common/base/logging/logger.h"
 #include "runtime/include/cmvr_runtime.h"
@@ -18,91 +14,20 @@ bool blockShutdownSignals(sigset_t& shutdown_signals)
     return pthread_sigmask(SIG_BLOCK, &shutdown_signals, nullptr) == 0;
 }
 
-struct CommandLineOptions {
-    std::string config_path;
-    double control_period_s{0.001};
-    bool show_help{false};
-};
-
-void printUsage(const char* program)
-{
-    std::cout
-        << "Usage: " << program
-        << " [--config PATH] [--control-period-s SECONDS]\n"
-        << "\n"
-        << "With no --config argument, cmvr_es loads config/cmvr_es.pb.txt "
-           "beside the executable.\n";
-}
-
-bool parseCommandLine(
-    const int argc,
-    char* argv[],
-    CommandLineOptions& options)
-{
-    for (int index = 1; index < argc; ++index) {
-        const std::string argument = argv[index];
-        if (argument == "--help" || argument == "-h") {
-            options.show_help = true;
-            return true;
-        }
-        if (argument == "--config") {
-            if (++index >= argc || argv[index][0] == '\0') {
-                std::cerr << "--config requires a path\n";
-                return false;
-            }
-            options.config_path = argv[index];
-            continue;
-        }
-        if (argument == "--control-period-s") {
-            if (++index >= argc) {
-                std::cerr
-                    << "--control-period-s requires a numeric value\n";
-                return false;
-            }
-            char* end = nullptr;
-            const double value = std::strtod(argv[index], &end);
-            if (!end || *end != '\0' || !std::isfinite(value) ||
-                value <= 0.0 || value > 1.0) {
-                std::cerr
-                    << "--control-period-s must be in (0, 1]\n";
-                return false;
-            }
-            options.control_period_s = value;
-            continue;
-        }
-        std::cerr << "unknown argument: " << argument << '\n';
-        return false;
-    }
-    return true;
-}
-
 } // namespace
 
-int main(int argc, char* argv[])
+int main()
 {
-    CommandLineOptions options;
-    if (!parseCommandLine(argc, argv, options)) {
-        printUsage(argv[0]);
-        return 2;
-    }
-    if (options.show_help) {
-        printUsage(argv[0]);
-        return 0;
-    }
-
     sigset_t shutdown_signals;
     if (!blockShutdownSignals(shutdown_signals)) {
         return 1;
     }
 
     cmvr::Runtime runtime;
-    const bool initialized = options.config_path.empty()
+    if (!runtime.init()) {
-                                 ? runtime.init()
-                                 : runtime.init(options.config_path);
-    if (!initialized) {
         return 1;
     }
-    if (!runtime.startTasks(options.control_period_s)) {
+    if (!runtime.startTasks()) {
         return 1;
     }
 

cmvr-es/manager/control_authority/CMakeLists.txt

@@ -1,34 +0,0 @@
-add_library(control_authority STATIC
-        src/control_authority_manager.cpp
-)
-target_compile_features(control_authority PUBLIC cxx_std_17)
-target_include_directories(control_authority
-        PUBLIC
-        ${PROJECT_SOURCE_DIR}/cmvr-es
-)
-
-add_library(
-        cmvr_es::control_authority
-        ALIAS control_authority
-)
-install(TARGETS control_authority ARCHIVE DESTINATION lib)
-
-if(BUILD_TESTING)
-    add_executable(control_authority_manager_test
-            tests/control_authority_manager_test.cpp
-    )
-    target_link_libraries(control_authority_manager_test
-            PRIVATE
-            cmvr_es::control_authority
-            gtest
-            gtest_main
-            pthread
-    )
-    add_test(
-            NAME control_authority_manager_test
-            COMMAND control_authority_manager_test
-    )
-    set_tests_properties(control_authority_manager_test PROPERTIES
-            TIMEOUT 10
-    )
-endif()

cmvr-es/manager/control_authority/include/control_authority_manager.h

@@ -1,74 +0,0 @@
-#ifndef CMVR_ES_CONTROL_AUTHORITY_MANAGER_H
-#define CMVR_ES_CONTROL_AUTHORITY_MANAGER_H
-
-#include <chrono>
-#include <cstdint>
-#include <mutex>
-#include <string>
-#include <unordered_map>
-
-namespace cmvr::control {
-
-struct ControlLeaseToken {
-    std::string resource_id;
-    std::string owner_id;
-    std::uint64_t generation{0};
-
-    bool valid() const noexcept
-    {
-        return !resource_id.empty() &&
-               !owner_id.empty() &&
-               generation != 0U;
-    }
-};
-
-struct ControlAcquireResult {
-    bool acquired{false};
-    ControlLeaseToken token;
-    std::string detail;
-};
-
-// Process-wide, transport-independent control ownership. The generation in a
-// token prevents a delayed release from an old network session from releasing
-// a newer lease on the same arm.
-class ControlAuthorityManager {
-public:
-    using Duration = std::chrono::milliseconds;
-
-    static ControlAuthorityManager& instance();
-
-    ControlAcquireResult tryAcquire(
-        const std::string& resource_id,
-        const std::string& owner_id,
-        Duration ttl);
-    bool renew(const ControlLeaseToken& token, Duration ttl);
-    bool validate(const ControlLeaseToken& token);
-    void release(const ControlLeaseToken& token) noexcept;
-
-    // Safety/control paths which do not possess a lease use this query to
-    // reject mutating commands. Read-only state and stop/torque-off commands
-    // are intentionally allowed by their callers.
-    bool isLeased(const std::string& resource_id);
-    void revoke(const std::string& resource_id) noexcept;
-
-    // Test/process teardown hook. Runtime code should release/revoke exact
-    // resources instead of clearing unrelated ownership.
-    void clear() noexcept;
-
-private:
-    struct Entry {
-        std::string owner_id;
-        std::uint64_t generation{0};
-        std::chrono::steady_clock::time_point deadline;
-    };
-
-    bool expired_(const Entry& entry) const noexcept;
-
-    std::mutex mutex_;
-    std::unordered_map<std::string, Entry> entries_;
-    std::uint64_t next_generation_{0};
-};
-
-} // namespace cmvr::control
-
-#endif // CMVR_ES_CONTROL_AUTHORITY_MANAGER_H

cmvr-es/manager/control_authority/src/control_authority_manager.cpp

@@ -1,152 +0,0 @@
-#include "manager/control_authority/include/control_authority_manager.h"
-
-#include <utility>
-
-namespace cmvr::control {
-
-ControlAuthorityManager& ControlAuthorityManager::instance()
-{
-    static ControlAuthorityManager manager;
-    return manager;
-}
-
-ControlAcquireResult ControlAuthorityManager::tryAcquire(
-    const std::string& resource_id,
-    const std::string& owner_id,
-    const Duration ttl)
-{
-    if (resource_id.empty() || owner_id.empty() ||
-        ttl <= Duration::zero()) {
-        return {false, {}, "invalid control lease request"};
-    }
-
-    std::lock_guard lock(mutex_);
-    const auto existing = entries_.find(resource_id);
-    if (existing != entries_.end()) {
-        if (!expired_(existing->second)) {
-            return {
-                false,
-                {},
-                "control resource is already leased by " +
-                    existing->second.owner_id};
-        }
-        entries_.erase(existing);
-    }
-
-    ControlLeaseToken token;
-    token.resource_id = resource_id;
-    token.owner_id = owner_id;
-    token.generation = ++next_generation_;
-    entries_.emplace(
-        resource_id,
-        Entry{
-            owner_id,
-            token.generation,
-            std::chrono::steady_clock::now() + ttl});
-    return {true, std::move(token), {}};
-}
-
-bool ControlAuthorityManager::renew(
-    const ControlLeaseToken& token,
-    const Duration ttl)
-{
-    if (!token.valid() || ttl <= Duration::zero()) {
-        return false;
-    }
-    std::lock_guard lock(mutex_);
-    const auto found = entries_.find(token.resource_id);
-    if (found == entries_.end() ||
-        expired_(found->second) ||
-        found->second.owner_id != token.owner_id ||
-        found->second.generation != token.generation) {
-        if (found != entries_.end() && expired_(found->second)) {
-            entries_.erase(found);
-        }
-        return false;
-    }
-    found->second.deadline =
-        std::chrono::steady_clock::now() + ttl;
-    return true;
-}
-
-bool ControlAuthorityManager::validate(
-    const ControlLeaseToken& token)
-{
-    if (!token.valid()) {
-        return false;
-    }
-    std::lock_guard lock(mutex_);
-    const auto found = entries_.find(token.resource_id);
-    if (found == entries_.end()) {
-        return false;
-    }
-    if (expired_(found->second)) {
-        entries_.erase(found);
-        return false;
-    }
-    return found->second.owner_id == token.owner_id &&
-           found->second.generation == token.generation;
-}
-
-void ControlAuthorityManager::release(
-    const ControlLeaseToken& token) noexcept
-{
-    if (!token.valid()) {
-        return;
-    }
-    try {
-        std::lock_guard lock(mutex_);
-        const auto found = entries_.find(token.resource_id);
-        if (found != entries_.end() &&
-            found->second.owner_id == token.owner_id &&
-            found->second.generation == token.generation) {
-            entries_.erase(found);
-        }
-    } catch (...) {
-    }
-}
-
-bool ControlAuthorityManager::isLeased(
-    const std::string& resource_id)
-{
-    if (resource_id.empty()) {
-        return false;
-    }
-    std::lock_guard lock(mutex_);
-    const auto found = entries_.find(resource_id);
-    if (found == entries_.end()) {
-        return false;
-    }
-    if (expired_(found->second)) {
-        entries_.erase(found);
-        return false;
-    }
-    return true;
-}
-
-void ControlAuthorityManager::revoke(
-    const std::string& resource_id) noexcept
-{
-    try {
-        std::lock_guard lock(mutex_);
-        entries_.erase(resource_id);
-    } catch (...) {
-    }
-}
-
-void ControlAuthorityManager::clear() noexcept
-{
-    try {
-        std::lock_guard lock(mutex_);
-        entries_.clear();
-    } catch (...) {
-    }
-}
-
-bool ControlAuthorityManager::expired_(
-    const Entry& entry) const noexcept
-{
-    return std::chrono::steady_clock::now() >= entry.deadline;
-}
-
-} // namespace cmvr::control

cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp

@@ -1,91 +0,0 @@
-#include "manager/control_authority/include/control_authority_manager.h"
-
-#include <chrono>
-#include <thread>
-
-#include <gtest/gtest.h>
-
-namespace cmvr::control {
-namespace {
-
-using namespace std::chrono_literals;
-
-class ControlAuthorityManagerTest : public ::testing::Test {
-protected:
-    void SetUp() override
-    {
-        ControlAuthorityManager::instance().clear();
-    }
-
-    void TearDown() override
-    {
-        ControlAuthorityManager::instance().clear();
-    }
-};
-
-TEST_F(ControlAuthorityManagerTest, LeaseIsExclusiveAndExactReleaseRestoresAccess)
-{
-    auto& manager = ControlAuthorityManager::instance();
-    const auto first =
-        manager.tryAcquire("right_arm", "session-a", 100ms);
-    ASSERT_TRUE(first.acquired);
-    EXPECT_TRUE(manager.validate(first.token));
-    EXPECT_TRUE(manager.isLeased("right_arm"));
-
-    const auto conflict =
-        manager.tryAcquire("right_arm", "session-b", 100ms);
-    EXPECT_FALSE(conflict.acquired);
-
-    manager.release(first.token);
-    EXPECT_FALSE(manager.isLeased("right_arm"));
-    EXPECT_TRUE(
-        manager.tryAcquire("right_arm", "session-b", 100ms)
-            .acquired);
-}
-
-TEST_F(ControlAuthorityManagerTest, StaleGenerationCannotReleaseNewLease)
-{
-    auto& manager = ControlAuthorityManager::instance();
-    const auto old =
-        manager.tryAcquire("right_arm", "session-a", 100ms);
-    ASSERT_TRUE(old.acquired);
-    manager.release(old.token);
-    const auto current =
-        manager.tryAcquire("right_arm", "session-a", 100ms);
-    ASSERT_TRUE(current.acquired);
-    ASSERT_NE(
-        old.token.generation,
-        current.token.generation);
-
-    manager.release(old.token);
-    EXPECT_TRUE(manager.validate(current.token));
-}
-
-TEST_F(ControlAuthorityManagerTest, ExpiryAndRenewUseMonotonicLocalTime)
-{
-    auto& manager = ControlAuthorityManager::instance();
-    const auto lease =
-        manager.tryAcquire("right_arm", "session-a", 20ms);
-    ASSERT_TRUE(lease.acquired);
-    std::this_thread::sleep_for(10ms);
-    ASSERT_TRUE(manager.renew(lease.token, 30ms));
-    std::this_thread::sleep_for(20ms);
-    EXPECT_TRUE(manager.validate(lease.token));
-    std::this_thread::sleep_for(20ms);
-    EXPECT_FALSE(manager.validate(lease.token));
-    EXPECT_FALSE(manager.isLeased("right_arm"));
-}
-
-TEST_F(ControlAuthorityManagerTest, DifferentArmsCanBeLeasedIndependently)
-{
-    auto& manager = ControlAuthorityManager::instance();
-    EXPECT_TRUE(
-        manager.tryAcquire("right_arm", "session-a", 100ms)
-            .acquired);
-    EXPECT_TRUE(
-        manager.tryAcquire("left_arm", "session-b", 100ms)
-            .acquired);
-}
-
-} // namespace
-} // namespace cmvr::control

cmvr-es/manager/device_manager/CMakeLists.txt

@@ -42,39 +42,9 @@ if(BUILD_TESTING)
                 "${CMAKE_BINARY_DIR}/cmvr_compiler_runtime")
         list(JOIN _device_manager_test_library_dirs ":"
                 _device_manager_test_library_path)
-        set(_device_manager_snapshot_test_environment
-                "LD_LIBRARY_PATH=${_device_manager_test_library_path}")
-        if(CMVR_TEST_SYSTEM_LIBSTDCXX)
-            list(APPEND _device_manager_snapshot_test_environment
-                    "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
-        endif()
         set_tests_properties(device_manager_snapshot_test PROPERTIES
                 ENVIRONMENT
-                "${_device_manager_snapshot_test_environment}"
+                "LD_LIBRARY_PATH=${_device_manager_test_library_path}"
         )
     endif()
-
-    add_executable(device_manager_lifecycle_test
-            tests/device_manager_lifecycle_test.cpp
-    )
-    target_link_libraries(device_manager_lifecycle_test PRIVATE
-            cmvr_es::device_manager
-            gtest
-            gtest_main
-            pthread
-    )
-    add_test(
-            NAME device_manager_lifecycle_test
-            COMMAND device_manager_lifecycle_test
-    )
-    set(_device_manager_lifecycle_test_environment
-            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}")
-    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
-        list(APPEND _device_manager_lifecycle_test_environment
-                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
-    endif()
-    set_tests_properties(device_manager_lifecycle_test PROPERTIES
-            TIMEOUT 10
-            ENVIRONMENT "${_device_manager_lifecycle_test_environment}"
-    )
 endif()

cmvr-es/manager/device_manager/include/device_manager.h

@@ -27,10 +27,9 @@ namespace cmvr::device {
         static DeviceManager& getInstance();
         static void destroyInstance();
 
-        bool start();
+        void start();
-        bool restart();
+        void restart();
         void stop();
-        bool initialized() const noexcept { return initialized_; }
 
         void getDeviceList(std::list<std::pair<std::string, std::string>> &device_list);
         void registerDevice(const std::shared_ptr<AbstractDevice>& device);
@@ -55,19 +54,14 @@ namespace cmvr::device {
         std::unordered_map<std::string, DeviceRecord> devices_;
         std::unordered_map<std::string, ManagedDeviceSnapshot> device_statuses_;
         std::unique_ptr<DeviceFactory> dev_factory_;
-        bool initialized_{false};
 
         explicit DeviceManager(const config::DeviceManagerConfig &cfg);
         void log_device_plan_() const;
-        bool pre_scan_robot_arm_dependencies_() const;
+        void pre_scan_robot_arm_dependencies_() const;
-        bool init_devices_();
+        void init_devices_();
         void configure_mujoco_viewer_pip_();
-        void initialize_device_statuses_();
+        void start_devices_();
-        void mark_initializing_statuses_error_(const std::string& error_message);
+        void stop_devices_();
-        void update_device_status_(const std::string& device_id,
-                                   ManagedDeviceState state,
-                                   const std::string& error_message = {});
-        void stop_devices_(bool update_status = true);
     };
 } // cmvr
 

cmvr-es/manager/device_manager/src/device_manager.cpp

@@ -6,10 +6,7 @@
 #include "../include/device_manager.h"
 
 #include <algorithm>
-#include <chrono>
 #include <exception>
-#include <utility>
-#include <vector>
 
 #include "devices/agv/abstract_agv.h"
 #include "devices/arm/robot_arm.h"
@@ -34,51 +31,6 @@ namespace {
 
 using GroupJointSelection = std::unordered_map<std::string, std::unordered_set<std::string>>;
 using MotorJointSelections = std::unordered_map<std::string, GroupJointSelection>;
-constexpr std::size_t kMaxDeviceErrorLength = 512;
-
-std::uint64_t unixTimeMs() noexcept
-{
-    const auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
-        std::chrono::system_clock::now().time_since_epoch());
-    return elapsed.count() > 0
-               ? static_cast<std::uint64_t>(elapsed.count())
-               : 1U;
-}
-
-std::string truncateDeviceError(const std::string& message)
-{
-    return message.substr(0, kMaxDeviceErrorLength);
-}
-
-DeviceKind deviceTypeToKind(
-    const cmvr::config::DeviceConfigEntry::DeviceType type)
-{
-    switch (type) {
-        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_BIO_HEAD_ROBOT:
-            return DeviceKind::BioHead;
-        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM:
-            return DeviceKind::MotorSystem;
-        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM:
-            return DeviceKind::Arm;
-        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA:
-            return DeviceKind::Camera;
-        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_DEXHAND:
-            return DeviceKind::DexHand;
-        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MICROPHONE:
-            return DeviceKind::Microphone;
-        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_SPEAKER:
-            return DeviceKind::Speaker;
-        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_AGV:
-            return DeviceKind::AGV;
-        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD:
-            return DeviceKind::MujocoWorld;
-        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_VIEWER:
-            return DeviceKind::MujocoViewer;
-        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN:
-        default:
-            return DeviceKind::Unknown;
-    }
-}
 
 void logSection(const char* title)
 {
@@ -174,24 +126,12 @@ DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) {
     cfg_ = cfg;
 
     dev_factory_ = std::make_unique<DeviceFactory>();
-    initialize_device_statuses_();
     logSection("Device Plan");
     log_device_plan_();
-    const bool dependencies_valid = pre_scan_robot_arm_dependencies_();
+    pre_scan_robot_arm_dependencies_();
     logSection("Initialize Devices");
-    if (!dependencies_valid) {
+    init_devices_();
-        mark_initializing_statuses_error_(
+    configure_mujoco_viewer_pip_();
-            "device dependency validation failed");
-    }
-    const bool devices_initialized =
-        dependencies_valid ? init_devices_() : false;
-    initialized_ = dependencies_valid && devices_initialized;
-    if (initialized_) {
-        configure_mujoco_viewer_pip_();
-    } else {
-        CMVR_LOG(ERROR) << "[DeviceManager]: Initialization failed for at "
-                           "least one enabled device";
-    }
 }
 
 DeviceManager& DeviceManager::getInstance(const config::DeviceManagerConfig& cfg) {
@@ -216,133 +156,34 @@ void DeviceManager::destroyInstance() {
     MotorManager::clearActiveJoints();
 }
 
-bool DeviceManager::start(){
+void DeviceManager::start(){
-    std::lock_guard lifecycle_lock(lifecycle_mutex_);
+    for (auto& [id, record] : devices_) {
-    if (!initialized_) {
+        if (!record.device) {
-        CMVR_LOG(ERROR) << "[DeviceManager]: Refusing to start because "
-                           "initialization did not complete";
-        stop_devices_(false);
-        return false;
-    }
-
-    std::vector<std::pair<std::string, std::shared_ptr<AbstractDevice>>>
-        devices;
-    {
-        std::shared_lock lock(devices_mutex_);
-        devices.reserve(devices_.size());
-        for (const auto& [id, record] : devices_) {
-            devices.emplace_back(id, record.device);
-        }
-    }
-
-    bool all_started = true;
-    for (const auto& [id, device] : devices) {
-        if (!device) {
             CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id;
-            update_device_status_(
-                id, ManagedDeviceState::Error,
-                "cannot start null device: " + id);
-            all_started = false;
             continue;
         }
-        bool started = false;
+        if (record.device->start()) {
-        std::string error_message;
-        try {
-            started = device->start();
-            if (!started) {
-                error_message = "device start returned false: " + id;
-            }
-        } catch (const std::exception& error) {
-            error_message =
-                "device start threw for " + id + ": " + error.what();
-            CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id
-                            << " threw: " << error.what();
-        } catch (...) {
-            error_message =
-                "device start threw an unknown exception: " + id;
-            CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id
-                            << " threw an unknown exception";
-        }
-        if (started) {
-            update_device_status_(id, ManagedDeviceState::Running);
             CMVR_LOG(INFO) << "[DeviceManager]: Start device " << id << " Success";
         } else {
-            update_device_status_(
-                id, ManagedDeviceState::Error, error_message);
             CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id << " Failed";
-            all_started = false;
         }
     }
-    if (!all_started) {
-        CMVR_LOG(ERROR) << "[DeviceManager]: At least one enabled device failed "
-                           "to start; stopping all devices";
-        // Rollback is a physical cleanup operation. Preserve the start
-        // results in the status table so the failure is diagnosable; an
-        // explicit stop() records Stopped/Error transitions.
-        stop_devices_(false);
-    }
-    return all_started;
 }
 
-bool DeviceManager::restart() {
+void DeviceManager::restart() {
     stop();
-    return start();
+    start();
 }
 
 void DeviceManager::stop() {
-    std::lock_guard lifecycle_lock(lifecycle_mutex_);
+    for (auto& [id, record] : devices_) {
-    stop_devices_();
+        if (!record.device) {
-}
-
-void DeviceManager::stop_devices_(const bool update_status) {
-    std::vector<std::pair<std::string, std::shared_ptr<AbstractDevice>>>
-        devices;
-    {
-        std::shared_lock lock(devices_mutex_);
-        devices.reserve(devices_.size());
-        for (const auto& [id, record] : devices_) {
-            devices.emplace_back(id, record.device);
-        }
-    }
-
-    for (const auto& [id, device] : devices) {
-        if (!device) {
             CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id;
-            if (update_status) {
-                update_device_status_(
-                    id, ManagedDeviceState::Error,
-                    "cannot stop null device: " + id);
-            }
             continue;
         }
-        bool stopped = false;
+        if (record.device->stop()) {
-        std::string error_message;
-        try {
-            stopped = device->stop();
-            if (!stopped) {
-                error_message = "device stop returned false: " + id;
-            }
-        } catch (const std::exception& error) {
-            error_message =
-                "device stop threw for " + id + ": " + error.what();
-            CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id
-                            << " threw: " << error.what();
-        } catch (...) {
-            error_message =
-                "device stop threw an unknown exception: " + id;
-            CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id
-                            << " threw an unknown exception";
-        }
-        if (stopped) {
-            if (update_status) {
-                update_device_status_(id, ManagedDeviceState::Stopped);
-            }
             CMVR_LOG(INFO) << "[DeviceManager]: Stop device " << id << " Success";
         } else {
-            if (update_status) {
-                update_device_status_(
-                    id, ManagedDeviceState::Error, error_message);
-            }
             CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id << " Failed";
         }
     }
@@ -351,7 +192,6 @@ void DeviceManager::stop_devices_(const bool update_status) {
 template <class DeviceType>
 std::shared_ptr<DeviceType> DeviceManager::getDevice(const std::string& device_id)
 {
-    std::shared_lock lock(devices_mutex_);
     auto it = devices_.find(device_id);
     if (it == devices_.end()) {
         CMVR_LOG(WARNING) << "[DeviceManager]: Device ID " << device_id << " not found.";
@@ -379,7 +219,6 @@ std::shared_ptr<AbstractDevice> DeviceManager::getDeviceBase(const std::string&
 
 void DeviceManager::getDeviceList(std::list<std::pair<std::string, std::string>>& device_list){
     device_list.clear();
-    std::shared_lock lock(devices_mutex_);
     for (const auto& [device_id, record] : devices_) {
         device_list.emplace_back(device_id, record.type_name);
     }
@@ -405,115 +244,22 @@ void DeviceManager::registerDevice(const std::string& device_id,
         CMVR_LOG(ERROR) << "[DeviceManager]: Cannot register device with empty id";
         return;
     }
+    if (devices_.count(device_id)) {
+        CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate device ID " << device_id;
+        return;
+    }
     DeviceRecord record;
     record.id = device_id;
     record.kind = device->kind();
     record.type_name = device->typeName();
     record.device = device;
-    {
+    devices_.emplace(record.id, std::move(record));
-        std::unique_lock lock(devices_mutex_);
-        if (devices_.count(device_id)) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate device ID " << device_id;
-            return;
-        }
-
-        ManagedDeviceSnapshot status;
-        status.id = record.id;
-        status.kind = record.kind;
-        status.type_name = record.type_name;
-        status.enabled = true;
-        status.state = ManagedDeviceState::Registered;
-        status.status_updated_at_unix_ms = unixTimeMs();
-        devices_.emplace(record.id, std::move(record));
-        device_statuses_[device_id] = std::move(status);
-    }
     CMVR_LOG(INFO) << "[DeviceManager]: Register device success"
                    << ", id=" << device_id
                    << ", type=" << device->typeName()
                    << ", kind=" << toString(device->kind());
 }
 
-void DeviceManager::initialize_device_statuses_()
-{
-    std::unique_lock lock(devices_mutex_);
-    for (const auto& entry : cfg_.devices()) {
-        const auto kind = deviceTypeToKind(entry.type());
-        ManagedDeviceSnapshot status;
-        status.id = entry.id();
-        status.kind = kind;
-        status.type_name = toString(kind);
-        status.enabled = entry.enable();
-        status.state = entry.enable()
-                           ? ManagedDeviceState::Initializing
-                           : ManagedDeviceState::Disabled;
-        status.status_updated_at_unix_ms = unixTimeMs();
-
-        if (entry.id().empty()) {
-            status.state = ManagedDeviceState::Error;
-            status.abnormal = true;
-            status.error_message =
-                "configured device id must not be empty";
-        }
-
-        const auto [it, inserted] =
-            device_statuses_.emplace(entry.id(), std::move(status));
-        if (!inserted) {
-            auto& duplicate_status = it->second;
-            duplicate_status.enabled =
-                duplicate_status.enabled || entry.enable();
-            duplicate_status.state = ManagedDeviceState::Error;
-            duplicate_status.abnormal = true;
-            duplicate_status.error_message = truncateDeviceError(
-                "duplicate configured device id: " + entry.id());
-            duplicate_status.status_updated_at_unix_ms = unixTimeMs();
-        }
-    }
-}
-
-void DeviceManager::mark_initializing_statuses_error_(
-    const std::string& error_message)
-{
-    std::unique_lock lock(devices_mutex_);
-    for (auto& [id, status] : device_statuses_) {
-        if (status.state != ManagedDeviceState::Initializing) {
-            continue;
-        }
-        status.state = ManagedDeviceState::Error;
-        status.abnormal = true;
-        status.error_message = truncateDeviceError(
-            error_message + ": " + id);
-        status.status_updated_at_unix_ms = unixTimeMs();
-    }
-}
-
-void DeviceManager::update_device_status_(
-    const std::string& device_id,
-    const ManagedDeviceState state,
-    const std::string& error_message)
-{
-    std::unique_lock lock(devices_mutex_);
-    auto& status = device_statuses_[device_id];
-    if (status.id.empty()) {
-        status.id = device_id;
-    }
-    const auto device_it = devices_.find(device_id);
-    if (device_it != devices_.end()) {
-        status.kind = device_it->second.kind;
-        status.type_name = device_it->second.type_name;
-    }
-    status.enabled = true;
-    status.state = state;
-    status.abnormal = state == ManagedDeviceState::Error;
-    status.error_message =
-        state == ManagedDeviceState::Error
-            ? truncateDeviceError(
-                  error_message.empty()
-                      ? "device lifecycle operation failed: " + device_id
-                      : error_message)
-            : std::string{};
-    status.status_updated_at_unix_ms = unixTimeMs();
-}
-
 DeviceManagerSnapshot DeviceManager::snapshot() const
 {
     struct SnapshotSource {
@@ -524,14 +270,15 @@ DeviceManagerSnapshot DeviceManager::snapshot() const
     std::vector<SnapshotSource> sources;
     {
         std::shared_lock lock(devices_mutex_);
-        sources.reserve(device_statuses_.size());
+        sources.reserve(devices_.size());
-        for (const auto& [id, stored_status] : device_statuses_) {
+        for (const auto& [id, record] : devices_) {
             SnapshotSource source;
-            source.status = stored_status;
+            source.status.id = id;
-            const auto device_it = devices_.find(id);
+            source.status.kind = record.kind;
-            if (device_it != devices_.end()) {
+            source.status.type_name = record.type_name;
-                source.device = device_it->second.device;
+            source.status.enabled = true;
-            }
+            source.status.state = ManagedDeviceState::Ready;
+            source.device = record.device;
             sources.push_back(std::move(source));
         }
     }
@@ -555,23 +302,10 @@ DeviceManagerSnapshot DeviceManager::snapshot() const
                     "device health snapshot threw an unknown exception";
             }
         }
-        source.status.health.error_message =
+        source.status.abnormal =
-            truncateDeviceError(source.status.health.error_message);
-        const bool lifecycle_error =
-            source.status.state == ManagedDeviceState::Error;
-        const bool health_error =
             source.status.health.state == DeviceHealthState::Degraded ||
             source.status.health.state == DeviceHealthState::Fault;
-        source.status.abnormal = lifecycle_error || health_error;
+        source.status.error_message = source.status.health.error_message;
-        if (source.status.error_message.empty()) {
-            source.status.error_message =
-                source.status.health.error_message;
-        }
-        source.status.error_message =
-            truncateDeviceError(source.status.error_message);
-        if (source.status.status_updated_at_unix_ms == 0) {
-            source.status.status_updated_at_unix_ms = unixTimeMs();
-        }
         result.devices.push_back(std::move(source.status));
     }
 
@@ -620,11 +354,10 @@ void DeviceManager::log_device_plan_() const
     CMVR_LOG(INFO) << "[DeviceManager]: Device plan end";
 }
 
-bool DeviceManager::pre_scan_robot_arm_dependencies_() const
+void DeviceManager::pre_scan_robot_arm_dependencies_() const
 {
     MotorJointSelections selections;
     std::unordered_map<std::string, config::MotorRootConfig> motor_roots;
-    MotorManager::clearActiveJoints();
 
     for (const auto& entry : cfg_.devices()) {
         if (!entry.enable() || entry.type() != config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM) {
@@ -632,22 +365,22 @@ bool DeviceManager::pre_scan_robot_arm_dependencies_() const
         }
         if (entry.id().empty()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager device id is empty";
-            return false;
+            return;
         }
         if (entry.config_file().empty()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager config_file is empty: " << entry.id();
-            return false;
+            return;
         }
 
         config::MotorRootConfig root_cfg;
         if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load motor config: " << entry.config_file();
-            return false;
+            return;
         }
         if (!root_cfg.motor().id().empty() && root_cfg.motor().id() != entry.id()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: MotorManager entry id '" << entry.id()
                             << "' does not match config id '" << root_cfg.motor().id() << "'";
-            return false;
+            return;
         }
         motor_roots.emplace(entry.id(), std::move(root_cfg));
     }
@@ -658,17 +391,17 @@ bool DeviceManager::pre_scan_robot_arm_dependencies_() const
         }
         if (entry.id().empty()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm device id is empty";
-            return false;
+            return;
         }
         if (entry.config_file().empty()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm config_file is empty: " << entry.id();
-            return false;
+            return;
         }
 
         config::ArmRootConfig root_cfg;
         if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load arm config: " << entry.config_file();
-            return false;
+            return;
         }
 
         const config::RobotArmConfig* arm_cfg = nullptr;
@@ -681,30 +414,29 @@ bool DeviceManager::pre_scan_robot_arm_dependencies_() const
         if (!arm_cfg) {
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm ID '" << entry.id()
                             << "' not found in config: " << entry.config_file();
-            return false;
+            return;
         }
 
-        if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor ||
+        if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor) {
-            arm_cfg->backend_case() == config::RobotArmConfig::kUme) {
             continue;
         }
         if (arm_cfg->backend_case() != config::RobotArmConfig::kMotor) {
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm backend is not configured: " << entry.id();
-            return false;
+            return;
         }
 
         const auto& motor_config = arm_cfg->motor();
         if (motor_config.motor_system_id().empty()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_system_id: " << entry.id();
-            return false;
+            return;
         }
         if (motor_config.motor_group_ids_size() == 0) {
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_group_ids: " << entry.id();
-            return false;
+            return;
         }
         if (motor_config.joint_names_size() == 0) {
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing joint_names: " << entry.id();
-            return false;
+            return;
         }
 
         const auto motor_root_it = motor_roots.find(motor_config.motor_system_id());
@@ -712,7 +444,7 @@ bool DeviceManager::pre_scan_robot_arm_dependencies_() const
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id()
                             << "' depends on disabled or missing MotorManager: "
                             << motor_config.motor_system_id();
-            return false;
+            return;
         }
 
         std::unordered_set<std::string> allowed_groups;
@@ -720,7 +452,7 @@ bool DeviceManager::pre_scan_robot_arm_dependencies_() const
         for (const auto& group_id : motor_config.motor_group_ids()) {
             if (group_id.empty()) {
                 CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty motor_group_id: " << entry.id();
-                return false;
+                return;
             }
             allowed_groups.insert(group_id);
         }
@@ -729,7 +461,7 @@ bool DeviceManager::pre_scan_robot_arm_dependencies_() const
         for (const auto& joint_name : motor_config.joint_names()) {
             if (joint_name.empty()) {
                 CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty joint_name: " << entry.id();
-                return false;
+                return;
             }
 
             std::string matched_group;
@@ -748,7 +480,7 @@ bool DeviceManager::pre_scan_robot_arm_dependencies_() const
                 CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id()
                                 << "' joint '" << joint_name
                                 << "' not found in configured motor_group_ids";
-                return false;
+                return;
             }
             group_selection[matched_group].insert(joint_name);
         }
@@ -763,132 +495,46 @@ bool DeviceManager::pre_scan_robot_arm_dependencies_() const
         }
     }
 
+    MotorManager::clearActiveJoints();
     for (auto& [motor_system_id, group_selection] : selections) {
         MotorManager::setActiveJoints(motor_system_id, std::move(group_selection));
     }
-    return true;
 }
 
-bool DeviceManager::init_devices_() {
+void DeviceManager::init_devices_() {
-    bool all_initialized = true;
     for (const auto& entry : cfg_.devices()) {
         if (!entry.enable()) {
             continue;
         }
 
-        {
-            std::shared_lock lock(devices_mutex_);
-            const auto status_it = device_statuses_.find(entry.id());
-            if (status_it != device_statuses_.end() &&
-                status_it->second.state == ManagedDeviceState::Error) {
-                all_initialized = false;
-                continue;
-            }
-        }
-
         CMVR_LOG(INFO) << "[DeviceManager]: Initialize device begin"
                        << ", id=" << entry.id()
                        << ", type=" << deviceTypeToString(entry.type())
                        << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file());
 
-        DeviceRecord record;
+        DeviceRecord record = dev_factory_->create(entry);
-        try {
-            record = dev_factory_->create(entry);
-        } catch (const std::exception& error) {
-            update_device_status_(
-                entry.id(), ManagedDeviceState::Error,
-                "device creation threw for " + entry.id() + ": " +
-                    error.what());
-            CMVR_LOG(ERROR) << "[DeviceManager]: Device creation threw for "
-                            << entry.id() << ": " << error.what();
-            all_initialized = false;
-            continue;
-        } catch (...) {
-            update_device_status_(
-                entry.id(), ManagedDeviceState::Error,
-                "device creation threw an unknown exception: " +
-                    entry.id());
-            CMVR_LOG(ERROR) << "[DeviceManager]: Device creation threw an "
-                               "unknown exception for " << entry.id();
-            all_initialized = false;
-            continue;
-        }
         if (!record.device || record.id.empty()) {
-            update_device_status_(
-                entry.id(), ManagedDeviceState::Error,
-                "failed to create configured device: " + entry.id());
             CMVR_LOG(ERROR) << "[DeviceManager]: Failed to create device for entry id=" << entry.id();
-            all_initialized = false;
             continue;
         }
         CMVR_LOG(INFO) << "[DeviceManager]: Create device object success"
                        << ", id=" << record.id
                        << ", type=" << record.type_name
                        << ", kind=" << toString(record.kind);
-        bool duplicate_device = false;
+        if (devices_.count(record.id)) {
-        {
-            std::shared_lock lock(devices_mutex_);
-            duplicate_device = devices_.count(record.id) != 0;
-        }
-        if (duplicate_device) {
-            update_device_status_(
-                entry.id(), ManagedDeviceState::Error,
-                "duplicate configured device id: " + record.id);
             CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate " << record.type_name << " Device ID " << record.id;
-            all_initialized = false;
             continue;
         }
         CMVR_LOG(INFO) << "[DeviceManager]: Init device object begin"
                        << ", id=" << record.id
                        << ", type=" << record.type_name
                        << ", kind=" << toString(record.kind);
-        bool device_initialized = false;
+        if (!record.device->init()) {
-        std::string init_error_message;
-        try {
-            device_initialized = record.device->init();
-            if (!device_initialized) {
-                init_error_message =
-                    "device init returned false: " + record.id;
-            }
-        } catch (const std::exception& error) {
-            init_error_message =
-                "device init threw for " + record.id + ": " +
-                error.what();
-            CMVR_LOG(ERROR) << "[DeviceManager]: Init device object threw"
-                            << ", id=" << record.id
-                            << ", error=" << error.what();
-        } catch (...) {
-            init_error_message =
-                "device init threw an unknown exception: " + record.id;
-            CMVR_LOG(ERROR) << "[DeviceManager]: Init device object threw an "
-                               "unknown exception, id=" << record.id;
-        }
-        if (!device_initialized) {
-            update_device_status_(
-                entry.id(), ManagedDeviceState::Error,
-                init_error_message);
             CMVR_LOG(ERROR) << "[DeviceManager]: Init device object failed"
                             << ", id=" << record.id
                             << ", type=" << record.type_name
                             << ", kind=" << toString(record.kind)
                             << ", config_file=" << entry.config_file();
-            try {
-                if (!record.device->stop()) {
-                    CMVR_LOG(ERROR)
-                        << "[DeviceManager]: Cleanup after failed init "
-                           "returned false, id=" << record.id;
-                }
-            } catch (const std::exception& error) {
-                CMVR_LOG(ERROR)
-                    << "[DeviceManager]: Cleanup after failed init threw"
-                    << ", id=" << record.id
-                    << ", error=" << error.what();
-            } catch (...) {
-                CMVR_LOG(ERROR)
-                    << "[DeviceManager]: Cleanup after failed init threw an "
-                       "unknown exception, id=" << record.id;
-            }
-            all_initialized = false;
             continue;
         }
         CMVR_LOG(INFO) << "[DeviceManager]: Init device object success"
@@ -896,36 +542,8 @@ bool DeviceManager::init_devices_() {
                        << ", type=" << record.type_name
                        << ", kind=" << toString(record.kind)
                        << ", config_file=" << entry.config_file();
-        {
+        devices_.emplace(record.id, std::move(record));
-            std::unique_lock lock(devices_mutex_);
-            const auto id = record.id;
-            const auto kind = record.kind;
-            const auto type_name = record.type_name;
-            const auto [device_it, inserted] =
-                devices_.emplace(id, std::move(record));
-            if (!inserted) {
-                auto& status = device_statuses_[entry.id()];
-                status.state = ManagedDeviceState::Error;
-                status.abnormal = true;
-                status.error_message = truncateDeviceError(
-                    "duplicate configured device id: " + id);
-                status.status_updated_at_unix_ms = unixTimeMs();
-                all_initialized = false;
-                continue;
-            }
-
-            auto& status = device_statuses_[id];
-            status.id = id;
-            status.kind = kind;
-            status.type_name = type_name;
-            status.enabled = true;
-            status.state = ManagedDeviceState::Ready;
-            status.abnormal = false;
-            status.error_message.clear();
-            status.status_updated_at_unix_ms = unixTimeMs();
-        }
     }
-    return all_initialized;
 }
 
 void DeviceManager::configure_mujoco_viewer_pip_()

cmvr-es/manager/device_manager/tests/device_manager_lifecycle_test.cpp

@@ -1,124 +0,0 @@
-#include "manager/device_manager/include/device_manager.h"
-
-#include <stdexcept>
-
-#include <gtest/gtest.h>
-
-namespace {
-
-class LifecycleDevice final : public cmvr::device::AbstractDevice {
-public:
-    explicit LifecycleDevice(const std::string& id)
-        : AbstractDevice(id)
-    {
-    }
-
-    cmvr::device::DeviceKind kind() const noexcept override
-    {
-        return cmvr::device::DeviceKind::Camera;
-    }
-
-    std::string typeName() const override { return "LifecycleDevice"; }
-
-    bool start() override
-    {
-        ++start_calls;
-        if (throw_on_start) {
-            throw std::runtime_error("start failure");
-        }
-        return start_result;
-    }
-
-    bool stop() override
-    {
-        ++stop_calls;
-        return true;
-    }
-
-    bool start_result{true};
-    bool throw_on_start{false};
-    int start_calls{0};
-    int stop_calls{0};
-};
-
-class DeviceManagerLifecycleTest : public ::testing::Test {
-protected:
-    void SetUp() override
-    {
-        cmvr::device::DeviceManager::destroyInstance();
-    }
-
-    void TearDown() override
-    {
-        cmvr::device::DeviceManager::destroyInstance();
-    }
-};
-
-TEST_F(DeviceManagerLifecycleTest,
-       EnabledDeviceCreationFailureMarksInitializationFailed)
-{
-    cmvr::config::DeviceManagerConfig config;
-    auto* entry = config.add_devices();
-    entry->set_id("unsupported");
-    entry->set_type(
-        cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN);
-    entry->set_enable(true);
-
-    auto& manager =
-        cmvr::device::DeviceManager::getInstance(config);
-    EXPECT_FALSE(manager.initialized());
-    EXPECT_FALSE(manager.start());
-}
-
-TEST_F(DeviceManagerLifecycleTest, DisabledInvalidDeviceIsIgnored)
-{
-    cmvr::config::DeviceManagerConfig config;
-    auto* entry = config.add_devices();
-    entry->set_id("disabled");
-    entry->set_type(
-        cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN);
-    entry->set_enable(false);
-
-    auto& manager =
-        cmvr::device::DeviceManager::getInstance(config);
-    EXPECT_TRUE(manager.initialized());
-    EXPECT_TRUE(manager.start());
-}
-
-TEST_F(DeviceManagerLifecycleTest,
-       DeviceStartFailureIsReturnedAndTriggersStop)
-{
-    cmvr::config::DeviceManagerConfig config;
-    auto& manager =
-        cmvr::device::DeviceManager::getInstance(config);
-    ASSERT_TRUE(manager.initialized());
-
-    auto device =
-        std::make_shared<LifecycleDevice>("start_failure");
-    device->start_result = false;
-    manager.registerDevice(device);
-
-    EXPECT_FALSE(manager.start());
-    EXPECT_EQ(device->start_calls, 1);
-    EXPECT_EQ(device->stop_calls, 1);
-}
-
-TEST_F(DeviceManagerLifecycleTest,
-       DeviceStartExceptionIsReturnedAndTriggersStop)
-{
-    cmvr::config::DeviceManagerConfig config;
-    auto& manager =
-        cmvr::device::DeviceManager::getInstance(config);
-    ASSERT_TRUE(manager.initialized());
-
-    auto device =
-        std::make_shared<LifecycleDevice>("start_exception");
-    device->throw_on_start = true;
-    manager.registerDevice(device);
-
-    EXPECT_FALSE(manager.start());
-    EXPECT_EQ(device->start_calls, 1);
-    EXPECT_EQ(device->stop_calls, 1);
-}
-
-} // namespace

cmvr-es/manager/device_manager/tests/device_manager_snapshot_test.cpp

@@ -253,13 +253,6 @@ bool testConfiguredAndDynamicSnapshots()
     CHECK_TRUE(duplicate_status->error_message ==
                "duplicate configured device id: duplicate_device");
 
-    // Configuration failures deliberately make this manager ineligible for
-    // start(). Use a fresh, valid manager for dynamic registration and
-    // lifecycle transitions so the test does not weaken fail-closed startup.
-    DeviceManager::destroyInstance();
-    cmvr::config::DeviceManagerConfig dynamic_config;
-    auto& dynamic_manager = DeviceManager::getInstance(dynamic_config);
-
     auto healthy = std::make_shared<FakeDevice>("z_healthy");
     auto degraded = std::make_shared<FakeDevice>("a_degraded");
     degraded->health = {
@@ -271,18 +264,18 @@ bool testConfiguredAndDynamicSnapshots()
     auto health_throw = std::make_shared<FakeDevice>("b_health_throw");
     health_throw->throw_on_health = true;
 
-    dynamic_manager.registerDevice(healthy);
+    manager.registerDevice(healthy);
-    dynamic_manager.registerDevice(degraded);
+    manager.registerDevice(degraded);
-    dynamic_manager.registerDevice(start_fail);
+    manager.registerDevice(start_fail);
-    dynamic_manager.registerDevice(stop_fail);
+    manager.registerDevice(stop_fail);
-    dynamic_manager.registerDevice(health_throw);
+    manager.registerDevice(health_throw);
 
     // Duplicate registration must retain the original object and status.
-    dynamic_manager.registerDevice(
+    manager.registerDevice(
         std::make_shared<FakeDevice>("z_healthy", DeviceKind::Speaker));
-    CHECK_TRUE(dynamic_manager.getDeviceBase("z_healthy") == healthy);
+    CHECK_TRUE(manager.getDeviceBase("z_healthy") == healthy);
 
-    const auto registered = dynamic_manager.snapshot();
+    const auto registered = manager.snapshot();
     CHECK_TRUE(isSorted(registered));
     const auto* healthy_registered =
         findDevice(registered, "z_healthy");
@@ -311,8 +304,8 @@ bool testConfiguredAndDynamicSnapshots()
     CHECK_TRUE(thrown_health->health.error_message.size() <= 512);
     CHECK_TRUE(thrown_health->error_message.size() <= 512);
 
-    CHECK_TRUE(!dynamic_manager.start());
+    manager.start();
-    const auto running = dynamic_manager.snapshot();
+    const auto running = manager.snapshot();
     CHECK_TRUE(findDevice(running, "z_healthy")->state ==
                ManagedDeviceState::Running);
     CHECK_TRUE(findDevice(running, "m_start_fail")->state ==
@@ -326,16 +319,14 @@ bool testConfiguredAndDynamicSnapshots()
     CHECK_TRUE(healthy_registered->state ==
                ManagedDeviceState::Registered);
 
-    dynamic_manager.stop();
+    manager.stop();
-    const auto stopped = dynamic_manager.snapshot();
+    const auto stopped = manager.snapshot();
     CHECK_TRUE(findDevice(stopped, "z_healthy")->state ==
                ManagedDeviceState::Stopped);
     CHECK_TRUE(findDevice(stopped, "n_stop_fail")->state ==
                ManagedDeviceState::Error);
     CHECK_TRUE(findDevice(stopped, "n_stop_fail")->abnormal);
-    // Failed start rolls back every device once; explicit stop performs the
+    CHECK_TRUE(healthy->stop_calls.load() == 1);
-    // second best-effort stop.
-    CHECK_TRUE(healthy->stop_calls.load() == 2);
     return true;
 }
 

cmvr-es/manager/task_manager/CMakeLists.txt

@@ -15,29 +15,3 @@ target_link_libraries(task_manager
 
 add_library(cmvr_es::task_manager ALIAS task_manager)
 install(TARGETS task_manager LIBRARY DESTINATION lib)
-
-if(BUILD_TESTING)
-    add_executable(task_manager_lifecycle_test
-            tests/task_manager_lifecycle_test.cpp
-    )
-    target_link_libraries(task_manager_lifecycle_test PRIVATE
-            cmvr_es::task_manager
-            gtest
-            gtest_main
-            pthread
-    )
-    add_test(
-            NAME task_manager_lifecycle_test
-            COMMAND task_manager_lifecycle_test
-    )
-    set(_task_manager_lifecycle_test_environment
-            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}")
-    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
-        list(APPEND _task_manager_lifecycle_test_environment
-                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
-    endif()
-    set_tests_properties(task_manager_lifecycle_test PROPERTIES
-            TIMEOUT 10
-            ENVIRONMENT "${_task_manager_lifecycle_test_environment}"
-    )
-endif()

cmvr-es/manager/task_manager/include/task_manager.h

@@ -26,10 +26,9 @@ namespace cmvr::task {
 
         ~TaskManager();
 
-        bool startRunTask(double control_period_s = 0.001);
+        void startRunTask(double control_period_s = 0.001);
         void stopRunTask();
         bool running() const { return running_.load(); }
-        bool initialized() const noexcept { return initialized_; }
 
         std::shared_ptr<Task> getTask(const std::string& task_id) const;
         std::shared_ptr<TouchScreenTask> getTouchScreenTask(const std::string& task_id = "touch_screen") const;
@@ -38,7 +37,7 @@ namespace cmvr::task {
         explicit TaskManager(const config::TaskManagerConfig& cfg);
 
         void logTaskPlan() const;
-        bool initTasks();
+        void initTasks();
         void runTaskLoop(double control_period_s);
         static TaskRunMode toTaskRunMode(config::TaskConfigEntry::TaskRunMode run_mode);
 
@@ -50,10 +49,8 @@ namespace cmvr::task {
         std::unordered_map<std::string, double> task_period_s_;
         std::unordered_map<std::string, std::chrono::steady_clock::time_point> next_step_time_;
         mutable std::mutex tasks_mutex_;
-        std::mutex lifecycle_mutex_;
         std::atomic<bool> running_{false};
         std::thread run_thread_;
-        bool initialized_{false};
     };
 
 } // namespace cmvr::task

cmvr-es/manager/task_manager/src/task_manager.cpp

@@ -40,8 +40,6 @@ const char* taskConfigTypeToString(const config::TaskConfigEntry::TaskType type)
             return "TASK_TYPE_SELF_COLLISION";
         case config::TaskConfigEntry::TASK_TYPE_QUIC_EDGE:
             return "TASK_TYPE_QUIC_EDGE";
-        case config::TaskConfigEntry::TASK_TYPE_UME_TELEOP:
-            return "TASK_TYPE_UME_TELEOP";
         case config::TaskConfigEntry::TASK_TYPE_UNKNOWN:
         default:
             return "TASK_TYPE_UNKNOWN";
@@ -61,21 +59,6 @@ const char* taskConfigRunModeToString(const config::TaskConfigEntry::TaskRunMode
     }
 }
 
-void stopTaskNoThrow(const std::shared_ptr<Task>& task)
-{
-    if (!task) {
-        return;
-    }
-    try {
-        task->stop();
-    } catch (const std::exception& error) {
-        CMVR_LOG(ERROR) << "[TaskManager] task stop threw: "
-                        << error.what();
-    } catch (...) {
-        CMVR_LOG(ERROR) << "[TaskManager] task stop threw an unknown exception";
-    }
-}
-
 } // namespace
 
 std::shared_ptr<TaskManager> TaskManager::instance_ = nullptr;
@@ -87,11 +70,7 @@ TaskManager::TaskManager(const config::TaskManagerConfig& cfg)
     logSection("Task Plan");
     logTaskPlan();
     logSection("Initialize Tasks");
-    initialized_ = initTasks();
+    initTasks();
-    if (!initialized_) {
-        CMVR_LOG(ERROR) << "[TaskManager] Initialization failed for at least "
-                           "one enabled task";
-    }
 }
 
 TaskManager::~TaskManager()
@@ -147,20 +126,16 @@ std::shared_ptr<Task> TaskManager::getTask(const std::string& task_id) const
     return it->second;
 }
 
-bool TaskManager::startRunTask(const double control_period_s)
+void TaskManager::startRunTask(const double control_period_s)
 {
-    std::lock_guard lifecycle_lock(lifecycle_mutex_);
-    if (!initialized_) {
-        CMVR_LOG(ERROR) << "[TaskManager] refusing to start because "
-                           "initialization did not complete";
-        return false;
-    }
     if (!std::isfinite(control_period_s) || control_period_s <= 0.0) {
         CMVR_LOG(ERROR) << "[TaskManager] invalid control_period_s";
-        return false;
+        return;
     }
-    if (running_.load()) {
+
-        return true;
+    bool expected = false;
+    if (!running_.compare_exchange_strong(expected, true)) {
+        return;
     }
 
     std::vector<std::shared_ptr<Task>> tasks;
@@ -176,57 +151,39 @@ bool TaskManager::startRunTask(const double control_period_s)
 
     std::vector<std::shared_ptr<Task>> started_tasks;
     for (const auto& task : tasks) {
-        bool started = false;
+        if (!task->start()) {
-        try {
-            started = task->start();
-        } catch (const std::exception& error) {
-            CMVR_LOG(ERROR) << "[TaskManager] task start threw: "
-                            << task->id() << ", error=" << error.what();
-        } catch (...) {
-            CMVR_LOG(ERROR) << "[TaskManager] task start threw an unknown "
-                               "exception: " << task->id();
-        }
-        if (!started) {
             CMVR_LOG(ERROR) << "[TaskManager] task start failed: " << task->id();
-            stopTaskNoThrow(task);
+            for (const auto& started_task : started_tasks) {
-            for (auto it = started_tasks.rbegin();
+                try {
-                 it != started_tasks.rend(); ++it) {
+                    started_task->stop();
-                stopTaskNoThrow(*it);
+                } catch (...) {
+                }
             }
             running_.store(false);
-            return false;
+            return;
         }
         started_tasks.push_back(task);
     }
 
-    running_.store(true);
     try {
         run_thread_ = std::thread(&TaskManager::runTaskLoop, this, control_period_s);
     } catch (const std::exception& e) {
         CMVR_LOG(ERROR) << "[TaskManager] failed to start run thread: " << e.what();
         running_.store(false);
-        for (auto it = started_tasks.rbegin();
+        for (const auto& task : started_tasks) {
-             it != started_tasks.rend(); ++it) {
+            try {
-            stopTaskNoThrow(*it);
+                task->stop();
+            } catch (...) {
+            }
         }
-        return false;
+        return;
-    } catch (...) {
-        CMVR_LOG(ERROR) << "[TaskManager] failed to start run thread with an "
-                           "unknown exception";
-        running_.store(false);
-        for (auto it = started_tasks.rbegin();
-             it != started_tasks.rend(); ++it) {
-            stopTaskNoThrow(*it);
-        }
-        return false;
     }
-    return true;
 }
 
 void TaskManager::stopRunTask()
 {
-    std::lock_guard lifecycle_lock(lifecycle_mutex_);
+    bool expected = true;
-    if (!running_.exchange(false)) {
+    if (!running_.compare_exchange_strong(expected, false)) {
         return;
     }
 
@@ -245,20 +202,21 @@ void TaskManager::stopRunTask()
         }
     }
     for (const auto& task : tasks) {
-        stopTaskNoThrow(task);
+        try {
+            task->stop();
+        } catch (...) {
+        }
     }
 }
 
-bool TaskManager::initTasks()
+void TaskManager::initTasks()
 {
-    bool all_initialized = true;
     for (const auto& entry : cfg_.tasks()) {
         if (!entry.enable()) {
             continue;
         }
         if (entry.id().empty()) {
             CMVR_LOG(ERROR) << "[TaskManager] Task ID is empty";
-            all_initialized = false;
             continue;
         }
 
@@ -268,30 +226,9 @@ bool TaskManager::initTasks()
                        << ", run_mode=" << taskConfigRunModeToString(entry.run_mode())
                        << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file());
 
-        std::shared_ptr<Task> task;
+        auto task = TaskFactory::create(entry);
-        try {
-            task = TaskFactory::create(entry);
-        } catch (const std::exception& error) {
-            CMVR_LOG(ERROR) << "[TaskManager] Task creation threw: "
-                            << entry.id() << ", error=" << error.what();
-            all_initialized = false;
-            continue;
-        } catch (...) {
-            CMVR_LOG(ERROR) << "[TaskManager] Task creation threw an unknown "
-                               "exception: " << entry.id();
-            all_initialized = false;
-            continue;
-        }
         if (!task || task->id() != entry.id()) {
             CMVR_LOG(ERROR) << "[TaskManager] Task ID mismatch: " << entry.id();
-            all_initialized = false;
-            continue;
-        }
-        if (entry.run_mode() ==
-            config::TaskConfigEntry::TASK_RUN_MODE_UNKNOWN) {
-            CMVR_LOG(ERROR) << "[TaskManager] Task run_mode is unknown: "
-                            << entry.id();
-            all_initialized = false;
             continue;
         }
         const TaskRunMode configured_run_mode = toTaskRunMode(entry.run_mode());
@@ -299,39 +236,23 @@ bool TaskManager::initTasks()
             CMVR_LOG(ERROR) << "[TaskManager] Task run_mode mismatch: id=" << entry.id()
                             << ", configured=" << taskRunModeToString(configured_run_mode)
                             << ", actual=" << taskRunModeToString(task->runMode());
-            all_initialized = false;
             continue;
         }
         double control_period_s = entry.control_period_s();
         if (configured_run_mode == TaskRunMode::PERIODIC_STEP &&
             (!std::isfinite(control_period_s) || control_period_s <= 0.0)) {
             CMVR_LOG(ERROR) << "[TaskManager] invalid control_period_s for task: " << entry.id();
-            all_initialized = false;
             continue;
         }
-        bool task_initialized = false;
+        if (!task->init()) {
-        try {
-            task_initialized = task->init();
-        } catch (const std::exception& error) {
-            CMVR_LOG(ERROR) << "[TaskManager] Task init threw: "
-                            << entry.id() << ", error=" << error.what();
-        } catch (...) {
-            CMVR_LOG(ERROR) << "[TaskManager] Task init threw an unknown "
-                               "exception: " << entry.id();
-        }
-        if (!task_initialized) {
             CMVR_LOG(ERROR) << "[TaskManager] Task init failed: " << entry.id()
                             << ", status=" << task->detailStatusString();
-            stopTaskNoThrow(task);
-            all_initialized = false;
             continue;
         }
         {
             std::lock_guard lock(tasks_mutex_);
             if (tasks_.count(entry.id())) {
                 CMVR_LOG(ERROR) << "[TaskManager] Duplicate task ID: " << entry.id();
-                stopTaskNoThrow(task);
-                all_initialized = false;
                 continue;
             }
             if (configured_run_mode == TaskRunMode::PERIODIC_STEP) {
@@ -340,7 +261,6 @@ bool TaskManager::initTasks()
             tasks_.emplace(entry.id(), std::move(task));
         }
     }
-    return all_initialized;
 }
 
 void TaskManager::logTaskPlan() const

cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp

@@ -1,194 +0,0 @@
-#include "manager/task_manager/include/task_manager.h"
-
-#include <memory>
-#include <stdexcept>
-
-#include <gtest/gtest.h>
-
-#include "task/task_factory.h"
-
-namespace {
-
-struct TaskBehavior {
-    bool init_result{true};
-    bool start_result{true};
-    bool throw_on_start{false};
-};
-
-TaskBehavior task_behavior;
-
-class LifecycleTask final : public cmvr::task::Task {
-public:
-    explicit LifecycleTask(std::string id)
-        : id_(std::move(id))
-    {
-    }
-
-    const std::string& id() const override { return id_; }
-    cmvr::task::TaskRunMode runMode() const override
-    {
-        return cmvr::task::TaskRunMode::BLOCKING_SERVICE;
-    }
-
-    bool init() override
-    {
-        ++init_calls;
-        state_ = task_behavior.init_result
-                     ? cmvr::task::TaskState::IDLE
-                     : cmvr::task::TaskState::FAILED;
-        return task_behavior.init_result;
-    }
-
-    bool start() override
-    {
-        ++start_calls;
-        if (task_behavior.throw_on_start) {
-            throw std::runtime_error("start failure");
-        }
-        state_ = task_behavior.start_result
-                     ? cmvr::task::TaskState::RUNNING
-                     : cmvr::task::TaskState::FAILED;
-        return task_behavior.start_result;
-    }
-
-    bool step(double) override { return true; }
-
-    void stop() override
-    {
-        ++stop_calls;
-        state_ = cmvr::task::TaskState::STOPPED;
-    }
-
-    cmvr::task::TaskState state() const override { return state_; }
-    bool isBusy() const override
-    {
-        return state_ == cmvr::task::TaskState::RUNNING;
-    }
-    bool isFinished() const override
-    {
-        return state_ == cmvr::task::TaskState::STOPPED;
-    }
-    bool isFailed() const override
-    {
-        return state_ == cmvr::task::TaskState::FAILED;
-    }
-    std::string stateString() const override
-    {
-        return cmvr::task::taskStateToString(state_);
-    }
-    std::string detailStatusString() const override
-    {
-        return stateString();
-    }
-
-    int init_calls{0};
-    int start_calls{0};
-    int stop_calls{0};
-
-private:
-    std::string id_;
-    cmvr::task::TaskState state_{
-        cmvr::task::TaskState::UNINITIALIZED};
-};
-
-std::shared_ptr<LifecycleTask> created_task;
-
-cmvr::config::TaskManagerConfig enabledTaskConfig()
-{
-    cmvr::config::TaskManagerConfig config;
-    auto* entry = config.add_tasks();
-    entry->set_id("lifecycle_task");
-    entry->set_type(
-        cmvr::config::TaskConfigEntry::TASK_TYPE_UME_TELEOP);
-    entry->set_enable(true);
-    entry->set_run_mode(
-        cmvr::config::TaskConfigEntry::
-            TASK_RUN_MODE_BLOCKING_SERVICE);
-    return config;
-}
-
-class TaskManagerLifecycleTest : public ::testing::Test {
-protected:
-    void SetUp() override
-    {
-        cmvr::task::TaskManager::destroyInstance();
-        task_behavior = {};
-        created_task.reset();
-        cmvr::task::TaskFactory::registerCreator(
-            cmvr::config::TaskConfigEntry::TASK_TYPE_UME_TELEOP,
-            [](const cmvr::config::TaskConfigEntry& entry) {
-                created_task =
-                    std::make_shared<LifecycleTask>(entry.id());
-                return created_task;
-            });
-    }
-
-    void TearDown() override
-    {
-        cmvr::task::TaskManager::destroyInstance();
-        created_task.reset();
-    }
-};
-
-TEST_F(TaskManagerLifecycleTest,
-       EnabledTaskInitFailureMarksInitializationFailed)
-{
-    task_behavior.init_result = false;
-    auto& manager =
-        cmvr::task::TaskManager::getInstance(enabledTaskConfig());
-
-    ASSERT_NE(created_task, nullptr);
-    EXPECT_FALSE(manager.initialized());
-    EXPECT_FALSE(manager.startRunTask());
-    EXPECT_FALSE(manager.running());
-    EXPECT_EQ(created_task->init_calls, 1);
-    EXPECT_EQ(created_task->start_calls, 0);
-    EXPECT_EQ(created_task->stop_calls, 1);
-}
-
-TEST_F(TaskManagerLifecycleTest,
-       TaskStartFailureIsReturnedAndRunningRemainsFalse)
-{
-    task_behavior.start_result = false;
-    auto& manager =
-        cmvr::task::TaskManager::getInstance(enabledTaskConfig());
-
-    ASSERT_TRUE(manager.initialized());
-    ASSERT_NE(created_task, nullptr);
-    EXPECT_FALSE(manager.startRunTask());
-    EXPECT_FALSE(manager.running());
-    EXPECT_EQ(created_task->start_calls, 1);
-    EXPECT_EQ(created_task->stop_calls, 1);
-}
-
-TEST_F(TaskManagerLifecycleTest,
-       TaskStartExceptionIsReturnedAndRunningRemainsFalse)
-{
-    task_behavior.throw_on_start = true;
-    auto& manager =
-        cmvr::task::TaskManager::getInstance(enabledTaskConfig());
-
-    ASSERT_TRUE(manager.initialized());
-    ASSERT_NE(created_task, nullptr);
-    EXPECT_FALSE(manager.startRunTask());
-    EXPECT_FALSE(manager.running());
-    EXPECT_EQ(created_task->start_calls, 1);
-    EXPECT_EQ(created_task->stop_calls, 1);
-}
-
-TEST_F(TaskManagerLifecycleTest, SuccessfulStartAndStopAreReported)
-{
-    auto& manager =
-        cmvr::task::TaskManager::getInstance(enabledTaskConfig());
-
-    ASSERT_TRUE(manager.initialized());
-    ASSERT_NE(created_task, nullptr);
-    EXPECT_TRUE(manager.startRunTask());
-    EXPECT_TRUE(manager.running());
-    manager.stopRunTask();
-    EXPECT_FALSE(manager.running());
-    EXPECT_EQ(created_task->start_calls, 1);
-    EXPECT_EQ(created_task->stop_calls, 1);
-}
-
-} // namespace

cmvr-es/runtime/CMakeLists.txt

@@ -13,39 +13,8 @@ target_link_libraries(cmvr_runtime PUBLIC
         cmvr_es::task_manager
         cmvr_es::service
         cmvr_es::quic_edge_task
-        cmvr_es::ume_teleop_task
         cmvr_es::mujoco_viewer
 )
 
 add_library(cmvr_es::runtime ALIAS cmvr_runtime)
 install(TARGETS cmvr_runtime LIBRARY DESTINATION lib ARCHIVE DESTINATION lib)
-
-if(BUILD_TESTING)
-    add_executable(runtime_lifecycle_test
-            tests/runtime_lifecycle_test.cpp
-    )
-    target_compile_features(runtime_lifecycle_test PRIVATE cxx_std_17)
-    target_include_directories(runtime_lifecycle_test PRIVATE
-            ${CMAKE_SOURCE_DIR}/cmvr-es
-    )
-    target_link_libraries(runtime_lifecycle_test PRIVATE
-            cmvr_es::runtime
-            gtest
-            gtest_main
-            pthread
-    )
-    add_test(
-            NAME runtime_lifecycle_test
-            COMMAND runtime_lifecycle_test
-    )
-    set(_runtime_lifecycle_test_environment
-            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}")
-    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
-        list(APPEND _runtime_lifecycle_test_environment
-                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
-    endif()
-    set_tests_properties(runtime_lifecycle_test PROPERTIES
-            TIMEOUT 20
-            ENVIRONMENT "${_runtime_lifecycle_test_environment}"
-    )
-endif()

cmvr-es/runtime/src/cmvr_runtime.cpp

@@ -12,7 +12,6 @@
 #include "common/io/proto_file_io.h"
 #include "task/grpc_server_task/include/grpc_server_task.h"
 #include "task/quic_edge_task/include/quic_edge_task.h"
-#include "task/ume_teleop_task/include/ume_teleop_task.h"
 
 namespace cmvr {
 namespace {
@@ -101,27 +100,13 @@ bool Runtime::init_(const std::string& config_path,
         return false;
     }
     CMVR_LOG(INFO) << "[Startup] Initialize DeviceManager";
-    auto& device_manager =
+    device::DeviceManager::getInstance(device_manager_root.device_manager());
-        device::DeviceManager::getInstance(
-            device_manager_root.device_manager());
-    if (!device_manager.initialized()) {
-        CMVR_LOG(ERROR) << "[Startup] DeviceManager initialization failed";
-        device_manager.stop();
-        device::DeviceManager::destroyInstance();
-        return false;
-    }
-    const auto rollback_device_manager = [&device_manager]() {
-        device_manager.stop();
-        device::DeviceManager::destroyInstance();
-    };
 
     task::registerGrpcServerTaskFactory();
     task::registerQuicEdgeTaskFactory();
-    task::registerUmeTeleopTaskFactory();
 
     if (app_config.task_manager_config_file().empty()) {
         CMVR_LOG(ERROR) << "TaskManager config file is empty";
-        rollback_device_manager();
         return false;
     }
     logSection("TaskManager");
@@ -131,19 +116,11 @@ bool Runtime::init_(const std::string& config_path,
     if (!ConfigHelper::loadConfigFile(app_config.task_manager_config_file(), task_manager_root)) {
         CMVR_LOG(ERROR) << "Failed to load TaskManager config: "
                         << app_config.task_manager_config_file();
-        rollback_device_manager();
         return false;
     }
 
     CMVR_LOG(INFO) << "[Startup] Initialize TaskManager";
-    auto& task_manager =
+    task::TaskManager::getInstance(task_manager_root.task_manager());
-        task::TaskManager::getInstance(task_manager_root.task_manager());
-    if (!task_manager.initialized()) {
-        CMVR_LOG(ERROR) << "[Startup] TaskManager initialization failed";
-        task::TaskManager::destroyInstance();
-        rollback_device_manager();
-        return false;
-    }
     initialized_ = true;
     return true;
 }
@@ -157,26 +134,9 @@ bool Runtime::startTasks(const double control_period_s)
         return true;
     }
 
-    // Device init() constructs and validates resources; start() owns worker
-    // threads. Start devices before any task can publish commands or sample
-    // them. UME start remains passive and never enables actuators.
-    logSection("Start Devices");
-    CMVR_LOG(INFO) << "[Startup] Start devices";
-    if (!device::DeviceManager::getInstance().start()) {
-        CMVR_LOG(ERROR) << "[Startup] One or more enabled devices failed "
-                           "to start; tasks will not be started";
-        return false;
-    }
-
     logSection("Start Tasks");
     CMVR_LOG(INFO) << "[Startup] Start tasks";
-    if (!task::TaskManager::getInstance().startRunTask(control_period_s)) {
+    task::TaskManager::getInstance().startRunTask(control_period_s);
-        CMVR_LOG(ERROR) << "[Startup] One or more enabled tasks failed "
-                           "to start; stopping devices";
-        device::DeviceManager::getInstance().stop();
-        tasks_started_ = false;
-        return false;
-    }
     tasks_started_ = true;
     return true;
 }

cmvr-es/runtime/tests/runtime_lifecycle_test.cpp

@@ -1,266 +0,0 @@
-#include "runtime/include/cmvr_runtime.h"
-
-#include <arpa/inet.h>
-#include <netinet/in.h>
-#include <sys/socket.h>
-#include <unistd.h>
-
-#include <algorithm>
-#include <array>
-#include <cstdlib>
-#include <filesystem>
-#include <string>
-
-#include <gtest/gtest.h>
-
-#include "cmvr/config/cmvr_es_config/cmvr_es_config.pb.h"
-#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
-#include "cmvr/config/grpc_server_config/grpc_server_config.pb.h"
-#include "cmvr/config/logger_config/logger_config.pb.h"
-#include "cmvr/config/task_manager_config/task_manager_config.pb.h"
-#include "common/io/proto_file_io.h"
-
-namespace {
-
-class TempConfigTree {
-public:
-    TempConfigTree()
-    {
-        std::array<char, 64> pattern{};
-        const std::string value =
-            "/tmp/cmvr-runtime-lifecycle-XXXXXX";
-        std::copy(value.begin(), value.end(), pattern.begin());
-        char* created = ::mkdtemp(pattern.data());
-        if (created) {
-            root_ = created;
-        }
-    }
-
-    ~TempConfigTree()
-    {
-        if (root_.empty()) {
-            return;
-        }
-        std::error_code error;
-        std::filesystem::remove_all(root_, error);
-    }
-
-    bool valid() const { return !root_.empty(); }
-
-    template <typename Message>
-    bool write(const std::string& name, const Message& message) const
-    {
-        return ProtoMessageIo::setProtoToAsciiFile(
-            message, (root_ / name).string());
-    }
-
-    std::string path(const std::string& name) const
-    {
-        return (root_ / name).string();
-    }
-
-    bool writeLogger() const
-    {
-        cmvr::config::LoggerRootConfig root;
-        auto* logger = root.mutable_logger();
-        logger->set_minimum_level(
-            cmvr::config::LOG_LEVEL_INFO);
-        auto* route = logger->add_routes();
-        route->set_level(cmvr::config::LOG_LEVEL_INFO);
-        route->set_terminal(false);
-        route->set_file(false);
-        return write("logger.pb.txt", root);
-    }
-
-    bool writeRoot() const
-    {
-        cmvr::config::CMVRESRootConfig root;
-        auto* config = root.mutable_cmvr_es();
-        config->set_logger_config_file("logger.pb.txt");
-        config->set_device_manager_config_file(
-            "device_manager.pb.txt");
-        config->set_task_manager_config_file(
-            "task_manager.pb.txt");
-        return write("cmvr_es.pb.txt", root);
-    }
-
-private:
-    std::filesystem::path root_;
-};
-
-class OccupiedTcpPort {
-public:
-    OccupiedTcpPort()
-    {
-        fd_ = ::socket(AF_INET, SOCK_STREAM | SOCK_CLOEXEC, 0);
-        if (fd_ < 0) {
-            return;
-        }
-
-        sockaddr_in address{};
-        address.sin_family = AF_INET;
-        address.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
-        address.sin_port = 0;
-        if (::bind(fd_, reinterpret_cast<sockaddr*>(&address),
-                   sizeof(address)) != 0 ||
-            ::listen(fd_, 1) != 0) {
-            ::close(fd_);
-            fd_ = -1;
-            return;
-        }
-
-        socklen_t length = sizeof(address);
-        if (::getsockname(fd_,
-                          reinterpret_cast<sockaddr*>(&address),
-                          &length) != 0) {
-            ::close(fd_);
-            fd_ = -1;
-            return;
-        }
-        port_ = ntohs(address.sin_port);
-    }
-
-    ~OccupiedTcpPort()
-    {
-        if (fd_ >= 0) {
-            ::close(fd_);
-        }
-    }
-
-    bool valid() const { return fd_ >= 0 && port_ != 0; }
-    std::string port() const { return std::to_string(port_); }
-
-private:
-    int fd_{-1};
-    unsigned short port_{0};
-};
-
-TEST(RuntimeLifecycleTest, EnabledDeviceInitFailureFailsRuntimeInit)
-{
-    TempConfigTree tree;
-    ASSERT_TRUE(tree.valid());
-    ASSERT_TRUE(tree.writeLogger());
-    ASSERT_TRUE(tree.writeRoot());
-
-    cmvr::config::DeviceManagerRootConfig devices;
-    auto* entry =
-        devices.mutable_device_manager()->add_devices();
-    entry->set_id("unsupported");
-    entry->set_type(
-        cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN);
-    entry->set_enable(true);
-    ASSERT_TRUE(tree.write("device_manager.pb.txt", devices));
-
-    cmvr::config::TaskManagerRootConfig tasks;
-    ASSERT_TRUE(tree.write("task_manager.pb.txt", tasks));
-
-    cmvr::Runtime runtime;
-    EXPECT_FALSE(runtime.init(tree.path("cmvr_es.pb.txt")));
-    EXPECT_FALSE(runtime.initialized());
-    EXPECT_FALSE(runtime.tasksStarted());
-}
-
-TEST(RuntimeLifecycleTest, EnabledTaskInitFailureFailsRuntimeInit)
-{
-    TempConfigTree tree;
-    ASSERT_TRUE(tree.valid());
-    ASSERT_TRUE(tree.writeLogger());
-    ASSERT_TRUE(tree.writeRoot());
-
-    cmvr::config::DeviceManagerRootConfig devices;
-    ASSERT_TRUE(tree.write("device_manager.pb.txt", devices));
-
-    cmvr::config::TaskManagerRootConfig tasks;
-    auto* entry = tasks.mutable_task_manager()->add_tasks();
-    entry->set_id("unsupported");
-    entry->set_type(
-        cmvr::config::TaskConfigEntry::TASK_TYPE_UNKNOWN);
-    entry->set_enable(true);
-    entry->set_run_mode(
-        cmvr::config::TaskConfigEntry::
-            TASK_RUN_MODE_BLOCKING_SERVICE);
-    ASSERT_TRUE(tree.write("task_manager.pb.txt", tasks));
-
-    cmvr::Runtime runtime;
-    EXPECT_FALSE(runtime.init(tree.path("cmvr_es.pb.txt")));
-    EXPECT_FALSE(runtime.initialized());
-    EXPECT_FALSE(runtime.tasksStarted());
-}
-
-TEST(RuntimeLifecycleTest,
-     TaskConfigLoadFailureRollsBackDeviceManagerSingleton)
-{
-    TempConfigTree first_tree;
-    ASSERT_TRUE(first_tree.valid());
-    ASSERT_TRUE(first_tree.writeLogger());
-    ASSERT_TRUE(first_tree.writeRoot());
-
-    cmvr::config::DeviceManagerRootConfig first_devices;
-    first_devices.mutable_device_manager()->set_name("first");
-    ASSERT_TRUE(first_tree.write(
-        "device_manager.pb.txt", first_devices));
-    // Deliberately do not create task_manager.pb.txt.
-
-    cmvr::Runtime runtime;
-    ASSERT_FALSE(
-        runtime.init(first_tree.path("cmvr_es.pb.txt")));
-    ASSERT_FALSE(runtime.initialized());
-
-    TempConfigTree second_tree;
-    ASSERT_TRUE(second_tree.valid());
-    ASSERT_TRUE(second_tree.writeLogger());
-    ASSERT_TRUE(second_tree.writeRoot());
-
-    cmvr::config::DeviceManagerRootConfig second_devices;
-    second_devices.mutable_device_manager()->set_name("second");
-    ASSERT_TRUE(second_tree.write(
-        "device_manager.pb.txt", second_devices));
-    cmvr::config::TaskManagerRootConfig second_tasks;
-    ASSERT_TRUE(second_tree.write(
-        "task_manager.pb.txt", second_tasks));
-
-    ASSERT_TRUE(
-        runtime.init(second_tree.path("cmvr_es.pb.txt")));
-    EXPECT_EQ(runtime.deviceManager().name(), "second");
-}
-
-TEST(RuntimeLifecycleTest, GrpcBindFailureDoesNotMarkTasksStarted)
-{
-    OccupiedTcpPort occupied_port;
-    ASSERT_TRUE(occupied_port.valid());
-
-    TempConfigTree tree;
-    ASSERT_TRUE(tree.valid());
-    ASSERT_TRUE(tree.writeLogger());
-    ASSERT_TRUE(tree.writeRoot());
-
-    cmvr::config::DeviceManagerRootConfig devices;
-    ASSERT_TRUE(tree.write("device_manager.pb.txt", devices));
-
-    cmvr::config::GRPCServerRootConfig grpc;
-    auto* grpc_config = grpc.mutable_grpc_server();
-    grpc_config->set_id("grpc_server");
-    grpc_config->set_host("127.0.0.1");
-    grpc_config->set_port(occupied_port.port());
-    ASSERT_TRUE(tree.write("grpc.pb.txt", grpc));
-
-    cmvr::config::TaskManagerRootConfig tasks;
-    auto* entry = tasks.mutable_task_manager()->add_tasks();
-    entry->set_id("grpc_server");
-    entry->set_type(
-        cmvr::config::TaskConfigEntry::TASK_TYPE_GRPC_SERVER);
-    entry->set_enable(true);
-    entry->set_run_mode(
-        cmvr::config::TaskConfigEntry::
-            TASK_RUN_MODE_BLOCKING_SERVICE);
-    entry->set_config_file("grpc.pb.txt");
-    ASSERT_TRUE(tree.write("task_manager.pb.txt", tasks));
-
-    cmvr::Runtime runtime;
-    ASSERT_TRUE(runtime.init(tree.path("cmvr_es.pb.txt")));
-    EXPECT_FALSE(runtime.startTasks());
-    EXPECT_FALSE(runtime.tasksStarted());
-    EXPECT_FALSE(runtime.taskManager().running());
-}
-
-} // namespace

cmvr-es/service/CMakeLists.txt

@@ -7,9 +7,6 @@ add_library(service
         grpc/src/grpc_head_service.cpp
         grpc/src/grpc_dexhand_service.cpp
         grpc/src/grpc_arm_service.cpp
-        grpc/src/grpc_arm_teleop_service.cpp
-        grpc/src/grpc_robot_arm_teleop_backend.cpp
-        grpc/src/grpc_motor_service.cpp
         grpc/src/grpc_agv_service.cpp
         grpc/src/grpc_hlc_service.cpp
         ../task/grpc_server_task/src/grpc_server_task.cpp
@@ -20,7 +17,6 @@ target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(service PRIVATE
         cmvr_es::proto
         osqp
-        cmvr_es::control_authority
         cmvr_es::device_manager
         cmvr_es::task_manager
         cmvr_es::algorithms::controller
@@ -46,131 +42,6 @@ if(BUILD_TESTING)
             COMMAND grpc_camera_stream_policy_test
     )
     set_tests_properties(grpc_camera_stream_policy_test PROPERTIES TIMEOUT 10)
-
-    add_executable(grpc_arm_teleop_service_test
-            grpc/tests/grpc_arm_teleop_service_test.cpp
-    )
-    target_include_directories(grpc_arm_teleop_service_test
-            PRIVATE
-            ${CMAKE_SOURCE_DIR}/cmvr-es
-    )
-    target_link_libraries(grpc_arm_teleop_service_test
-            PRIVATE
-            service
-            cmvr_es::proto
-            gtest
-            gtest_main
-            pthread
-    )
-    add_test(
-            NAME grpc_arm_teleop_service_test
-            COMMAND grpc_arm_teleop_service_test
-    )
-    set(_grpc_arm_teleop_test_environment
-            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
-    )
-    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
-        list(APPEND _grpc_arm_teleop_test_environment
-                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
-    endif()
-    set_tests_properties(grpc_arm_teleop_service_test PROPERTIES
-            TIMEOUT 20
-            ENVIRONMENT "${_grpc_arm_teleop_test_environment}"
-    )
-
-    add_executable(grpc_robot_arm_teleop_backend_test
-            grpc/tests/grpc_robot_arm_teleop_backend_test.cpp
-    )
-    target_include_directories(grpc_robot_arm_teleop_backend_test
-            PRIVATE
-            ${CMAKE_SOURCE_DIR}/cmvr-es
-    )
-    target_link_libraries(grpc_robot_arm_teleop_backend_test
-            PRIVATE
-            service
-            cmvr_es::proto
-            gtest
-            gtest_main
-            pthread
-    )
-    add_test(
-            NAME grpc_robot_arm_teleop_backend_test
-            COMMAND grpc_robot_arm_teleop_backend_test
-    )
-    set_tests_properties(grpc_robot_arm_teleop_backend_test PROPERTIES
-            TIMEOUT 10
-            ENVIRONMENT "${_grpc_arm_teleop_test_environment}"
-    )
-
-    add_executable(grpc_motor_service_test
-            grpc/tests/grpc_motor_service_test.cpp
-    )
-    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()
 
 # --------------------------------------------------------

cmvr-es/service/arm_teleop_client/CMakeLists.txt

@@ -1,38 +0,0 @@
-find_package(Threads REQUIRED)
-
-add_library(arm_teleop_client STATIC
-        src/grpc_arm_teleop_client.cpp
-)
-target_compile_features(arm_teleop_client PUBLIC cxx_std_17)
-target_include_directories(arm_teleop_client PUBLIC ${PROJECT_SOURCE_DIR}/cmvr-es)
-target_link_libraries(arm_teleop_client
-        PUBLIC
-        cmvr_es::proto
-        PRIVATE
-        Threads::Threads
-)
-
-add_library(cmvr_es::arm_teleop_client ALIAS arm_teleop_client)
-install(TARGETS arm_teleop_client ARCHIVE DESTINATION lib)
-
-if(BUILD_TESTING)
-    add_executable(grpc_arm_teleop_client_test
-            tests/grpc_arm_teleop_client_test.cpp
-    )
-    target_compile_features(grpc_arm_teleop_client_test PRIVATE cxx_std_17)
-    target_link_libraries(grpc_arm_teleop_client_test
-            PRIVATE
-            cmvr_es::arm_teleop_client
-            Threads::Threads
-    )
-    add_test(NAME grpc_arm_teleop_client_test COMMAND grpc_arm_teleop_client_test)
-    set(_arm_teleop_client_test_environment
-            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}")
-    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
-        list(APPEND _arm_teleop_client_test_environment
-                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
-    endif()
-    set_tests_properties(grpc_arm_teleop_client_test PROPERTIES
-            TIMEOUT 10
-            ENVIRONMENT "${_arm_teleop_client_test_environment}")
-endif()

cmvr-es/service/arm_teleop_client/include/grpc_arm_teleop_client.h

@@ -1,80 +0,0 @@
-#ifndef CMVR_ES_GRPC_ARM_TELEOP_CLIENT_H
-#define CMVR_ES_GRPC_ARM_TELEOP_CLIENT_H
-
-#include <cstdint>
-#include <functional>
-#include <memory>
-#include <mutex>
-
-#include <grpcpp/channel.h>
-#include <grpcpp/client_context.h>
-#include <grpcpp/support/status.h>
-#include <grpcpp/support/sync_stream.h>
-
-#include "cmvr/api/arm_teleop_v1.grpc.pb.h"
-
-namespace cmvr::teleop {
-
-// One synchronous gRPC stream/session. Connection retry and worker ownership
-// belong to UmeTeleopTask; robot algorithms and kinematics belong to UME.
-class GrpcArmTeleopClient final {
-public:
-    using Api = api::armteleop::v1::ArmTeleopService;
-    using ClientFrame = api::armteleop::v1::ClientFrame;
-    using ServerFrame = api::armteleop::v1::ServerFrame;
-    using OpenSession = api::armteleop::v1::OpenSession;
-    using JointSetpoint = api::armteleop::v1::JointSetpoint;
-    using ClientHeartbeat = api::armteleop::v1::ClientHeartbeat;
-    using StopSession = api::armteleop::v1::StopSession;
-    using FrameCallback = std::function<void(const ServerFrame&)>;
-    using CancelPredicate = std::function<bool()>;
-
-    explicit GrpcArmTeleopClient(
-        std::shared_ptr<grpc::ChannelInterface> channel);
-    ~GrpcArmTeleopClient();
-
-    GrpcArmTeleopClient(const GrpcArmTeleopClient&) = delete;
-    GrpcArmTeleopClient& operator=(const GrpcArmTeleopClient&) = delete;
-
-    // Blocks until the peer closes the stream or tryCancel() is called. The
-    // OpenSession frame is always the first client frame.
-    grpc::Status runSession(const OpenSession& open_session,
-                            FrameCallback callback = {},
-                            CancelPredicate cancel_requested = {});
-
-    // These methods only transport already-computed protocol values.
-    bool sendSetpoint(const JointSetpoint& setpoint,
-                      std::uint64_t expected_session_generation = 0);
-    bool sendHeartbeat(const ClientHeartbeat& heartbeat,
-                       std::uint64_t expected_session_generation = 0);
-    bool sendStop(const StopSession& stop,
-                  std::uint64_t expected_session_generation = 0);
-
-    bool isSessionActive() const;
-    std::uint64_t activeSessionGeneration() const;
-
-    // Thread-safe and intentionally named after the gRPC primitive used. It
-    // interrupts a blocked Read/Write/Finish so the owning Task can join.
-    void tryCancel();
-
-private:
-    using Stream = grpc::ClientReaderWriterInterface<ClientFrame, ServerFrame>;
-
-    bool writeFrame(const ClientFrame& frame,
-                    std::uint64_t expected_session_generation);
-    void clearSession(const std::shared_ptr<grpc::ClientContext>& context,
-                      const std::shared_ptr<Stream>& stream);
-
-    std::unique_ptr<Api::StubInterface> stub_;
-
-    mutable std::mutex lifecycle_mutex_;
-    std::mutex write_mutex_;
-    std::shared_ptr<grpc::ClientContext> active_context_;
-    std::shared_ptr<Stream> active_stream_;
-    std::uint64_t next_session_generation_{0};
-    std::uint64_t active_session_generation_{0};
-};
-
-} // namespace cmvr::teleop
-
-#endif // CMVR_ES_GRPC_ARM_TELEOP_CLIENT_H

cmvr-es/service/arm_teleop_client/src/grpc_arm_teleop_client.cpp

@@ -1,223 +0,0 @@
-#include "service/arm_teleop_client/include/grpc_arm_teleop_client.h"
-
-#include <exception>
-#include <string>
-#include <utility>
-
-namespace cmvr::teleop {
-namespace {
-
-grpc::Status clientStatus(const grpc::StatusCode code, const char* detail)
-{
-    return grpc::Status(code, detail);
-}
-
-} // namespace
-
-GrpcArmTeleopClient::GrpcArmTeleopClient(
-    std::shared_ptr<grpc::ChannelInterface> channel)
-{
-    if (channel) {
-        stub_ = Api::NewStub(channel);
-    }
-}
-
-GrpcArmTeleopClient::~GrpcArmTeleopClient()
-{
-    tryCancel();
-}
-
-grpc::Status GrpcArmTeleopClient::runSession(
-    const OpenSession& open_session,
-    FrameCallback callback,
-    CancelPredicate cancel_requested)
-{
-    if (!stub_) {
-        return clientStatus(
-            grpc::StatusCode::FAILED_PRECONDITION,
-            "arm teleop client has no channel");
-    }
-    if (cancel_requested && cancel_requested()) {
-        return clientStatus(
-            grpc::StatusCode::CANCELLED,
-            "arm teleop session cancelled before start");
-    }
-
-    auto context = std::make_shared<grpc::ClientContext>();
-    {
-        std::lock_guard lock(lifecycle_mutex_);
-        if (active_context_) {
-            return clientStatus(
-                grpc::StatusCode::ALREADY_EXISTS,
-                "arm teleop session is already active");
-        }
-        // Publish the context before opening/writing the stream so tryCancel()
-        // can interrupt every blocking phase of the synchronous RPC.
-        active_context_ = context;
-    }
-    // Closes the small race where the owning Task requests stop immediately
-    // before active_context_ becomes visible to tryCancel().
-    if (cancel_requested && cancel_requested()) {
-        context->TryCancel();
-    }
-
-    auto unique_stream = stub_->Teleoperate(context.get());
-    if (!unique_stream) {
-        clearSession(context, {});
-        return clientStatus(
-            grpc::StatusCode::UNAVAILABLE,
-            "failed to create arm teleop stream");
-    }
-    auto stream = std::shared_ptr<Stream>(std::move(unique_stream));
-
-    ClientFrame first_frame;
-    *first_frame.mutable_open() = open_session;
-    {
-        std::lock_guard write_lock(write_mutex_);
-        if (!stream->Write(first_frame)) {
-            const grpc::Status status = stream->Finish();
-            clearSession(context, stream);
-            return status.ok()
-                       ? clientStatus(
-                             grpc::StatusCode::UNAVAILABLE,
-                             "peer closed before OpenSession was written")
-                       : status;
-        }
-    }
-
-    {
-        std::lock_guard lock(lifecycle_mutex_);
-        // Cancellation can race the initial Write. Keeping the stream visible
-        // is safe; subsequent writes will fail and runSession will clean it.
-        if (active_context_ == context) {
-            active_stream_ = stream;
-            active_session_generation_ = ++next_session_generation_;
-        }
-    }
-
-    bool callback_failed = false;
-    std::string callback_error;
-    ServerFrame frame;
-    while (stream->Read(&frame)) {
-        if (!callback) {
-            continue;
-        }
-        try {
-            callback(frame);
-        } catch (const std::exception& error) {
-            callback_failed = true;
-            callback_error = error.what();
-            context->TryCancel();
-            break;
-        } catch (...) {
-            callback_failed = true;
-            callback_error = "server-frame callback raised an unknown exception";
-            context->TryCancel();
-            break;
-        }
-    }
-
-    {
-        std::lock_guard write_lock(write_mutex_);
-        stream->WritesDone();
-    }
-    const grpc::Status status = stream->Finish();
-    clearSession(context, stream);
-
-    if (callback_failed) {
-        return grpc::Status(
-            grpc::StatusCode::INTERNAL,
-            "arm teleop callback failed: " + callback_error);
-    }
-    return status;
-}
-
-bool GrpcArmTeleopClient::sendSetpoint(
-    const JointSetpoint& setpoint,
-    const std::uint64_t expected_session_generation)
-{
-    ClientFrame frame;
-    *frame.mutable_setpoint() = setpoint;
-    return writeFrame(frame, expected_session_generation);
-}
-
-bool GrpcArmTeleopClient::sendHeartbeat(
-    const ClientHeartbeat& heartbeat,
-    const std::uint64_t expected_session_generation)
-{
-    ClientFrame frame;
-    *frame.mutable_heartbeat() = heartbeat;
-    return writeFrame(frame, expected_session_generation);
-}
-
-bool GrpcArmTeleopClient::sendStop(
-    const StopSession& stop,
-    const std::uint64_t expected_session_generation)
-{
-    ClientFrame frame;
-    *frame.mutable_stop() = stop;
-    return writeFrame(frame, expected_session_generation);
-}
-
-bool GrpcArmTeleopClient::isSessionActive() const
-{
-    std::lock_guard lock(lifecycle_mutex_);
-    return active_stream_ != nullptr;
-}
-
-std::uint64_t GrpcArmTeleopClient::activeSessionGeneration() const
-{
-    std::lock_guard lock(lifecycle_mutex_);
-    return active_session_generation_;
-}
-
-void GrpcArmTeleopClient::tryCancel()
-{
-    std::shared_ptr<grpc::ClientContext> context;
-    {
-        std::lock_guard lock(lifecycle_mutex_);
-        context = active_context_;
-    }
-    if (context) {
-        context->TryCancel();
-    }
-}
-
-bool GrpcArmTeleopClient::writeFrame(
-    const ClientFrame& frame,
-    const std::uint64_t expected_session_generation)
-{
-    std::shared_ptr<Stream> stream;
-    {
-        std::lock_guard lock(lifecycle_mutex_);
-        if (expected_session_generation != 0 &&
-            expected_session_generation != active_session_generation_) {
-            return false;
-        }
-        stream = active_stream_;
-    }
-    if (!stream) {
-        return false;
-    }
-
-    // gRPC permits one read and one write concurrently, but concurrent writes
-    // must be serialized by the application.
-    std::lock_guard write_lock(write_mutex_);
-    return stream->Write(frame);
-}
-
-void GrpcArmTeleopClient::clearSession(
-    const std::shared_ptr<grpc::ClientContext>& context,
-    const std::shared_ptr<Stream>& stream)
-{
-    std::lock_guard lock(lifecycle_mutex_);
-    if (active_context_ == context) {
-        active_context_.reset();
-    }
-    if (!stream || active_stream_ == stream) {
-        active_stream_.reset();
-        active_session_generation_ = 0;
-    }
-}
-
-} // namespace cmvr::teleop

cmvr-es/service/arm_teleop_client/tests/grpc_arm_teleop_client_test.cpp

@@ -1,220 +0,0 @@
-#include <atomic>
-#include <chrono>
-#include <condition_variable>
-#include <cstdio>
-#include <iostream>
-#include <memory>
-#include <mutex>
-#include <string>
-#include <thread>
-
-#include <grpcpp/grpcpp.h>
-#include <unistd.h>
-
-#include "cmvr/api/arm_teleop_v1.grpc.pb.h"
-#include "service/arm_teleop_client/include/grpc_arm_teleop_client.h"
-
-namespace {
-
-using namespace std::chrono_literals;
-namespace api = cmvr::api::armteleop::v1;
-
-class TestArmTeleopService final : public api::ArmTeleopService::Service {
-public:
-    grpc::Status Teleoperate(
-        grpc::ServerContext*,
-        grpc::ServerReaderWriter<api::ServerFrame, api::ClientFrame>* stream) override
-    {
-        api::ClientFrame frame;
-        if (!stream->Read(&frame) || !frame.has_open()) {
-            return grpc::Status(
-                grpc::StatusCode::INVALID_ARGUMENT,
-                "OpenSession must be first");
-        }
-
-        {
-            std::lock_guard lock(mutex_);
-            open_received_ = true;
-        }
-        condition_.notify_all();
-
-        api::ServerFrame opened;
-        opened.mutable_status()->set_session_id("client-test-session");
-        opened.mutable_status()->set_phase(api::SESSION_PHASE_OPENED);
-        if (!stream->Write(opened)) {
-            return grpc::Status::OK;
-        }
-
-        while (stream->Read(&frame)) {
-            if (frame.has_heartbeat()) {
-                heartbeat_received_.store(true);
-                condition_.notify_all();
-            }
-        }
-        handler_finished_.store(true);
-        condition_.notify_all();
-        return grpc::Status::OK;
-    }
-
-    bool waitForOpen(const std::chrono::milliseconds timeout)
-    {
-        std::unique_lock lock(mutex_);
-        return condition_.wait_for(lock, timeout, [this] { return open_received_; });
-    }
-
-    bool waitForHeartbeat(const std::chrono::milliseconds timeout)
-    {
-        std::unique_lock lock(mutex_);
-        return condition_.wait_for(
-            lock, timeout, [this] { return heartbeat_received_.load(); });
-    }
-
-    bool waitForHandlerFinish(const std::chrono::milliseconds timeout)
-    {
-        std::unique_lock lock(mutex_);
-        return condition_.wait_for(
-            lock, timeout, [this] { return handler_finished_.load(); });
-    }
-
-private:
-    std::mutex mutex_;
-    std::condition_variable condition_;
-    bool open_received_{false};
-    std::atomic<bool> heartbeat_received_{false};
-    std::atomic<bool> handler_finished_{false};
-};
-
-int fail(const std::string& detail)
-{
-    std::cerr << "grpc_arm_teleop_client_test: " << detail << '\n';
-    return 1;
-}
-
-} // namespace
-
-int main()
-{
-    TestArmTeleopService service;
-    const std::string socket_path =
-        "/tmp/cmvr_arm_teleop_client_test_" +
-        std::to_string(static_cast<long long>(::getpid())) + ".sock";
-    std::remove(socket_path.c_str());
-    const std::string endpoint = "unix:" + socket_path;
-
-    grpc::ServerBuilder builder;
-    builder.AddListeningPort(
-        endpoint,
-        grpc::InsecureServerCredentials());
-    builder.RegisterService(&service);
-    std::unique_ptr<grpc::Server> server = builder.BuildAndStart();
-    if (!server) {
-        return fail("failed to start in-process gRPC server");
-    }
-
-    auto channel = grpc::CreateChannel(
-        endpoint,
-        grpc::InsecureChannelCredentials());
-    cmvr::teleop::GrpcArmTeleopClient client(channel);
-
-    api::OpenSession open;
-    open.set_protocol_major(1);
-    open.set_protocol_minor(0);
-    open.set_client_instance_id("grpc-client-test");
-    open.mutable_expected_robot()->set_robot_id("test-arm");
-    open.set_watchdog_timeout_ms(100);
-    open.set_requested_lease_ms(500);
-
-    std::mutex frame_mutex;
-    std::condition_variable frame_condition;
-    bool opened_received = false;
-    grpc::Status session_status;
-    std::thread session_thread([&] {
-        session_status = client.runSession(
-            open,
-            [&](const api::ServerFrame& frame) {
-                if (frame.has_status() &&
-                    frame.status().phase() == api::SESSION_PHASE_OPENED) {
-                    {
-                        std::lock_guard lock(frame_mutex);
-                        opened_received = true;
-                    }
-                    frame_condition.notify_all();
-                }
-            });
-    });
-
-    if (!service.waitForOpen(2s)) {
-        client.tryCancel();
-        session_thread.join();
-        server->Shutdown();
-        return fail("server did not receive OpenSession");
-    }
-    {
-        std::unique_lock lock(frame_mutex);
-        if (!frame_condition.wait_for(lock, 2s, [&] { return opened_received; })) {
-            client.tryCancel();
-            session_thread.join();
-            server->Shutdown();
-            return fail("client did not receive OPENED status");
-        }
-    }
-    if (!client.isSessionActive()) {
-        client.tryCancel();
-        session_thread.join();
-        server->Shutdown();
-        return fail("client did not expose an active session");
-    }
-    const std::uint64_t generation =
-        client.activeSessionGeneration();
-    if (generation == 0) {
-        client.tryCancel();
-        session_thread.join();
-        server->Shutdown();
-        return fail("active stream did not expose a session generation");
-    }
-
-    api::ClientHeartbeat heartbeat;
-    heartbeat.set_sequence(1);
-    if (client.sendHeartbeat(heartbeat, generation + 1)) {
-        client.tryCancel();
-        session_thread.join();
-        server->Shutdown();
-        return fail("stale session generation was allowed to write");
-    }
-    if (!client.sendHeartbeat(heartbeat, generation) ||
-        !service.waitForHeartbeat(2s)) {
-        client.tryCancel();
-        session_thread.join();
-        server->Shutdown();
-        return fail("heartbeat did not traverse the active stream");
-    }
-
-    const auto cancel_begin = std::chrono::steady_clock::now();
-    client.tryCancel();
-    session_thread.join();
-    const auto cancel_elapsed = std::chrono::steady_clock::now() - cancel_begin;
-
-    if (cancel_elapsed > 2s) {
-        server->Shutdown();
-        return fail("TryCancel did not unblock and join the session promptly");
-    }
-    if (session_status.error_code() != grpc::StatusCode::CANCELLED) {
-        server->Shutdown();
-        return fail(
-            "cancelled session returned unexpected status: " +
-            std::to_string(session_status.error_code()));
-    }
-    if (client.isSessionActive()) {
-        server->Shutdown();
-        return fail("client retained an active stream after cancellation");
-    }
-    if (!service.waitForHandlerFinish(2s)) {
-        server->Shutdown();
-        return fail("server handler did not observe client cancellation");
-    }
-
-    server->Shutdown();
-    std::remove(socket_path.c_str());
-    std::cout << "grpc_arm_teleop_client_test: PASS\n";
-    return 0;
-}

cmvr-es/service/grpc/include/grpc_arm_teleop_service.h

@@ -1,91 +0,0 @@
-#pragma once
-
-#include <chrono>
-#include <cstdint>
-#include <memory>
-#include <string>
-#include <utility>
-
-#include <grpcpp/grpcpp.h>
-
-#include "cmvr/api/arm_teleop_v1.grpc.pb.h"
-#include "manager/control_authority/include/control_authority_manager.h"
-
-namespace cmvr::service {
-
-namespace arm_teleop = cmvr::api::armteleop::v1;
-
-struct ArmTeleopBackendResult {
-    bool success{false};
-    grpc::StatusCode status_code{grpc::StatusCode::INTERNAL};
-    std::string detail;
-
-    static ArmTeleopBackendResult ok()
-    {
-        return {true, grpc::StatusCode::OK, {}};
-    }
-
-    static ArmTeleopBackendResult failure(
-        const grpc::StatusCode code,
-        std::string message)
-    {
-        return {false, code, std::move(message)};
-    }
-};
-
-struct ArmTeleopBackendSnapshot {
-    arm_teleop::JointState joint_state;
-    arm_teleop::RobotSafetyState safety;
-};
-
-// Execution boundary for ArmTeleopService. The first implementation registers a
-// disabled backend in production and injects a fake backend in tests. A future
-// RobotArm adapter must live behind this interface so the gRPC reader thread can
-// remain a bounded mailbox producer and never touch hardware. Implementations
-// must keep every call bounded and non-blocking with respect to hardware I/O;
-// snapshot() must return cached state rather than synchronously polling a bus.
-class ArmTeleopBackend {
-public:
-    virtual ~ArmTeleopBackend() = default;
-
-    virtual bool available() const noexcept = 0;
-    virtual std::string unavailableReason() const { return {}; }
-    virtual arm_teleop::RobotManifest manifest() const = 0;
-    virtual bool supportsForceFeedback() const noexcept = 0;
-    virtual ArmTeleopBackendResult open(
-        const arm_teleop::OpenSession& request) = 0;
-    // The deadline is computed from the receiver's local monotonic clock.
-    // Implementations must re-check it immediately before committing a
-    // hardware command; the protobuf valid_for duration is never interpreted
-    // as a cross-machine absolute timestamp.
-    virtual ArmTeleopBackendResult applySetpoint(
-        const arm_teleop::JointSetpoint& setpoint,
-        std::chrono::steady_clock::time_point deadline) = 0;
-    virtual ArmTeleopBackendResult stop(
-        arm_teleop::StopReason reason,
-        const std::string& detail) = 0;
-    virtual ArmTeleopBackendSnapshot snapshot() const = 0;
-};
-
-std::shared_ptr<ArmTeleopBackend> makeDisabledArmTeleopBackend();
-
-class ArmTeleopServiceImpl final
-    : public arm_teleop::ArmTeleopService::Service {
-public:
-    explicit ArmTeleopServiceImpl(
-        std::shared_ptr<ArmTeleopBackend> backend =
-            makeDisabledArmTeleopBackend(),
-        control::ControlAuthorityManager* authority = nullptr);
-    ~ArmTeleopServiceImpl() override = default;
-
-    grpc::Status Teleoperate(
-        grpc::ServerContext* context,
-        grpc::ServerReaderWriter<arm_teleop::ServerFrame,
-                                 arm_teleop::ClientFrame>* stream) override;
-
-private:
-    std::shared_ptr<ArmTeleopBackend> backend_;
-    control::ControlAuthorityManager* authority_{nullptr};
-};
-
-} // namespace cmvr::service

cmvr-es/service/grpc/include/grpc_motor_service.h

@@ -1,197 +0,0 @@
-#pragma once
-
-#include <chrono>
-#include <cstdint>
-#include <memory>
-#include <mutex>
-#include <optional>
-#include <string>
-#include <unordered_map>
-
-#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<api::CyclicControlResponse,
-                                 api::CyclicPositionRequest>* stream) override;
-    grpc::Status streamCyclicVelocity(
-        grpc::ServerContext* context,
-        grpc::ServerReaderWriter<api::CyclicControlResponse,
-                                 api::CyclicVelocityRequest>* 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<device::AbstractMotor> motor;
-        std::shared_ptr<MotorControlState> control;
-    };
-
-    struct MotorControlEntry {
-        std::weak_ptr<device::AbstractMotor> owner;
-        std::shared_ptr<MotorControlState> state;
-    };
-
-    class ControlLease {
-    public:
-        ControlLease(std::shared_ptr<MotorControlState> 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<MotorControlState> state_;
-        std::uint64_t generation_{0};
-        int uncaught_on_entry_{0};
-    };
-
-    grpc::Status resolveMotor(const api::MotorTarget& target,
-                              ResolvedMotor& resolved) const;
-    std::shared_ptr<MotorControlState> stateFor(
-        const std::shared_ptr<device::AbstractMotor>& motor) const;
-    std::unique_ptr<ControlLease> 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<api::CyclicControlResponse,
-                                 api::CyclicPositionRequest>* stream,
-        std::optional<api::MotorTarget>& cleanup_target);
-    grpc::Status streamCyclicVelocityImpl(
-        grpc::ServerContext* context,
-        grpc::ServerReaderWriter<api::CyclicControlResponse,
-                                 api::CyclicVelocityRequest>* stream,
-        std::optional<api::MotorTarget>& cleanup_target);
-
-    void bestEffortQuickStop(const api::MotorTarget& target,
-                             const std::string& error) noexcept;
-    void latchUnsafeAfterFailedStop(
-        const std::shared_ptr<MotorControlState>& state,
-        const std::string& error) const;
-    void fillMotorStatus(const ResolvedMotor& resolved,
-                         api::MotorStatus* status) const;
-    void setLastError(const std::shared_ptr<MotorControlState>& state,
-                      const std::string& error) const;
-
-    device::DeviceManager& dmgr_;
-    mutable std::mutex states_mutex_;
-    mutable std::unordered_map<const device::AbstractMotor*,
-                               MotorControlEntry> states_;
-};
-
-} // namespace cmvr::service

cmvr-es/service/grpc/include/grpc_robot_arm_teleop_backend.h

@@ -1,18 +0,0 @@
-#pragma once
-
-#include <memory>
-
-#include "cmvr/config/grpc_server_config/grpc_server_config.pb.h"
-#include "devices/arm/robot_arm.h"
-#include "service/grpc/include/grpc_arm_teleop_service.h"
-
-namespace cmvr::service {
-
-// Creates a fail-closed adapter from the process RobotArm abstraction to the
-// session-based ArmTeleop backend. available() remains false unless the config,
-// RobotModel and RobotArm capability all pass static validation.
-std::shared_ptr<ArmTeleopBackend> makeRobotArmTeleopBackend(
-    std::shared_ptr<device::RobotArm> arm,
-    const config::ArmTeleopBackendConfig& config);
-
-} // namespace cmvr::service

cmvr-es/service/grpc/src/grpc_arm_service.cpp

@@ -1,13 +1,8 @@
 #include "service/grpc/include/grpc_arm_service.h"
 
-#include <atomic>
-#include <chrono>
-#include <utility>
-
 #include <google/protobuf/util/time_util.h>
 
 #include "common/base/logging/logger.h"
-#include "manager/control_authority/include/control_authority_manager.h"
 
 using google::protobuf::util::TimeUtil;
 
@@ -124,64 +119,6 @@ grpc::Status setDeviceNotFound(Response* response, const std::string& device_id)
     return grpc::Status(grpc::StatusCode::NOT_FOUND, message);
 }
 
-grpc::Status setControlLeaseConflict(
-    api::CommandHeader_Feedback* response,
-    const std::string& device_id)
-{
-    const std::string message =
-        "RobotArm control is leased by another active control operation: " +
-        device_id;
-    fillFeedback(response, false, message);
-    return grpc::Status(
-        grpc::StatusCode::FAILED_PRECONDITION, message);
-}
-
-template <typename Response>
-grpc::Status setControlLeaseConflict(
-    Response* response,
-    const std::string& device_id)
-{
-    return setControlLeaseConflict(
-        response->mutable_header(), device_id);
-}
-
-class ScopedUnaryControlLease final {
-public:
-    ScopedUnaryControlLease(
-        const std::string& device_id,
-        const char* operation)
-        : manager_(control::ControlAuthorityManager::instance())
-    {
-        static std::atomic<std::uint64_t> sequence{0};
-        const std::string owner =
-            std::string("grpc-arm-unary:") + operation + ":" +
-            std::to_string(
-                sequence.fetch_add(
-                    1U, std::memory_order_relaxed) +
-                1U);
-        auto acquired = manager_.tryAcquire(
-            device_id,
-            owner,
-            std::chrono::duration_cast<
-                control::ControlAuthorityManager::Duration>(
-                std::chrono::hours(24)));
-        acquired_ = acquired.acquired;
-        token_ = std::move(acquired.token);
-    }
-
-    ~ScopedUnaryControlLease()
-    {
-        manager_.release(token_);
-    }
-
-    bool acquired() const noexcept { return acquired_; }
-
-private:
-    control::ControlAuthorityManager& manager_;
-    control::ControlLeaseToken token_;
-    bool acquired_{false};
-};
-
 } // namespace
 
 gRPCArmServiceImpl::gRPCArmServiceImpl()
@@ -199,10 +136,6 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        // A safety-disable command preempts any network teleoperation lease.
-        // The teleoperation executor must fail its next renew before it can
-        // dispatch another setpoint.
-        control::ControlAuthorityManager::instance().revoke(device_id);
         const auto result = arm->torqueOff();
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
         if (result.ok()) {
@@ -225,11 +158,6 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        ScopedUnaryControlLease control_lease(
-            device_id, "torqueOn");
-        if (!control_lease.acquired()) {
-            return setControlLeaseConflict(response, device_id);
-        }
         const auto result = arm->torqueOn();
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
         if (result.ok()) {
@@ -252,11 +180,6 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        ScopedUnaryControlLease control_lease(
-            device_id, "moveJ");
-        if (!control_lease.acquired()) {
-            return setControlLeaseConflict(response, device_id);
-        }
         const auto result = arm->moveJ(toJointPositionCommand(request->target()),
                                        toMotionOptions(request->options()));
         if (result.ok()) {
@@ -280,11 +203,6 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        ScopedUnaryControlLease control_lease(
-            device_id, "moveL");
-        if (!control_lease.acquired()) {
-            return setControlLeaseConflict(response, device_id);
-        }
         const auto result = arm->moveL(toCartesianPose(request->target()),
                                        toMotionOptions(request->options()),
                                        toFrameType(request->frame()));
@@ -309,11 +227,6 @@ grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        ScopedUnaryControlLease control_lease(
-            device_id, "speedJ");
-        if (!control_lease.acquired()) {
-            return setControlLeaseConflict(response, device_id);
-        }
         const auto result = arm->speedJ(toJointVelocityCommand(request->velocity()),
                                         request->acceleration(),
                                         request->duration());
@@ -340,11 +253,6 @@ grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        ScopedUnaryControlLease control_lease(
-            device_id, "speedL");
-        if (!control_lease.acquired()) {
-            return setControlLeaseConflict(response, device_id);
-        }
         const auto result = arm->speedL(toCartesianVelocity(request->velocity()),
                                         request->acceleration(),
                                         request->duration(),
@@ -372,11 +280,6 @@ grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        ScopedUnaryControlLease control_lease(
-            device_id, "servoJ");
-        if (!control_lease.acquired()) {
-            return setControlLeaseConflict(response, device_id);
-        }
         const auto result = arm->servoJ(toJointPositionCommand(request->target()));
         if (result.ok()) {
             CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (servoJ): success, id=" << device_id
@@ -399,7 +302,6 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        control::ControlAuthorityManager::instance().revoke(device_id);
         const auto result = arm->stopMotion();
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
         if (result.ok()) {
@@ -477,11 +379,6 @@ grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        ScopedUnaryControlLease control_lease(
-            device_id, "calibrateZeroQ");
-        if (!control_lease.acquired()) {
-            return setControlLeaseConflict(response, device_id);
-        }
         const auto result = arm->calibrateZeroQ(request->joint_name());
         if (result.ok()) {
             CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (calibrateZeroQ): success, id=" << device_id
@@ -520,11 +417,6 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        ScopedUnaryControlLease control_lease(
-            device_id, "clearFault");
-        if (!control_lease.acquired()) {
-            return setControlLeaseConflict(response, device_id);
-        }
         const auto result = arm->clearFault();
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
         return resultToStatus(result);
@@ -534,3 +426,4 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context,
     }
 }
 } // namespace cmvr::service
+

cmvr-es/service/grpc/src/grpc_robot_arm_teleop_backend.cpp

@@ -1,649 +0,0 @@
-#include "service/grpc/include/grpc_robot_arm_teleop_backend.h"
-
-#include <algorithm>
-#include <chrono>
-#include <cmath>
-#include <cctype>
-#include <cstdint>
-#include <limits>
-#include <mutex>
-#include <sstream>
-#include <unordered_set>
-#include <utility>
-
-namespace cmvr::service {
-namespace {
-
-using Clock = std::chrono::steady_clock;
-
-constexpr double kMinimumServoPeriodS = 0.0001;
-constexpr double kMaximumServoPeriodS = 0.1;
-
-bool isDigest(const std::string& value)
-{
-    return value.size() == 64 &&
-           std::all_of(
-               value.begin(), value.end(), [](const unsigned char value) {
-                   return std::isxdigit(value) != 0;
-               });
-}
-
-arm_teleop::EffortSource toProtoEffortSource(
-    const device::JointEffortSource source)
-{
-    switch (source) {
-        case device::JointEffortSource::MotorEstimate:
-            return arm_teleop::EFFORT_SOURCE_MOTOR_ESTIMATE;
-        case device::JointEffortSource::JointSensor:
-            return arm_teleop::EFFORT_SOURCE_JOINT_SENSOR;
-        case device::JointEffortSource::ForceTorqueSensor:
-            return arm_teleop::EFFORT_SOURCE_FORCE_TORQUE_SENSOR;
-        case device::JointEffortSource::Observer:
-            return arm_teleop::EFFORT_SOURCE_OBSERVER;
-        case device::JointEffortSource::Unspecified:
-        default:
-            return arm_teleop::EFFORT_SOURCE_UNSPECIFIED;
-    }
-}
-
-grpc::StatusCode toStatusCode(const device::ArmErrorCode code)
-{
-    switch (code) {
-        case device::ArmErrorCode::InvalidArgument:
-        case device::ArmErrorCode::InvalidDof:
-            return grpc::StatusCode::INVALID_ARGUMENT;
-        case device::ArmErrorCode::OutOfJointLimit:
-        case device::ArmErrorCode::OutOfVelocityLimit:
-        case device::ArmErrorCode::OutOfAccelerationLimit:
-        case device::ArmErrorCode::OutOfWorkspace:
-            return grpc::StatusCode::OUT_OF_RANGE;
-        case device::ArmErrorCode::NotConnected:
-        case device::ArmErrorCode::RobotNotReady:
-        case device::ArmErrorCode::RobotNotPowered:
-        case device::ArmErrorCode::RobotInFault:
-        case device::ArmErrorCode::RobotInProtectiveStop:
-        case device::ArmErrorCode::RobotInEmergencyStop:
-        case device::ArmErrorCode::CommandRejected:
-        case device::ArmErrorCode::UnsupportedCommand:
-            return grpc::StatusCode::FAILED_PRECONDITION;
-        case device::ArmErrorCode::Timeout:
-            return grpc::StatusCode::DEADLINE_EXCEEDED;
-        case device::ArmErrorCode::ConnectionFailed:
-            return grpc::StatusCode::UNAVAILABLE;
-        case device::ArmErrorCode::AlreadyConnected:
-            return grpc::StatusCode::ALREADY_EXISTS;
-        case device::ArmErrorCode::CommandFailed:
-        case device::ArmErrorCode::UnknownError:
-        case device::ArmErrorCode::OK:
-        default:
-            return grpc::StatusCode::INTERNAL;
-    }
-}
-
-ArmTeleopBackendResult fromArmResult(
-    const device::Result& result,
-    const char* operation)
-{
-    if (result.ok()) {
-        return ArmTeleopBackendResult::ok();
-    }
-    std::string detail(operation);
-    detail += " failed";
-    if (!result.message.empty()) {
-        detail += ": " + result.message;
-    }
-    return ArmTeleopBackendResult::failure(
-        toStatusCode(result.code), std::move(detail));
-}
-
-class RobotArmTeleopBackend final : public ArmTeleopBackend {
-public:
-    RobotArmTeleopBackend(
-        std::shared_ptr<device::RobotArm> arm,
-        config::ArmTeleopBackendConfig config)
-        : arm_(std::move(arm)),
-          config_(std::move(config)),
-          require_powered_(
-              !config_.has_require_powered() ||
-              config_.require_powered())
-    {
-        validateStaticConfiguration();
-    }
-
-    bool available() const noexcept override
-    {
-        return unavailable_reason_.empty();
-    }
-
-    std::string unavailableReason() const override
-    {
-        return unavailable_reason_;
-    }
-
-    arm_teleop::RobotManifest manifest() const override
-    {
-        return manifest_;
-    }
-
-    bool supportsForceFeedback() const noexcept override
-    {
-        // A non-zero effort vector is not enough. The RobotArm must explicitly
-        // identify a verified effort source.
-        return available() && arm_->jointEffortSource() !=
-                                  device::JointEffortSource::Unspecified;
-    }
-
-    ArmTeleopBackendResult open(
-        const arm_teleop::OpenSession& request) override
-    {
-        std::lock_guard operation_lock(operation_mutex_);
-        if (!available()) {
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::FAILED_PRECONDITION,
-                unavailable_reason_);
-        }
-        if (session_open_) {
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::ALREADY_EXISTS,
-                "RobotArm teleoperation servo mode is already open");
-        }
-
-        const auto state = arm_->getRobotState();
-        cacheState(state);
-        const auto safety_result = validateSafety(state);
-        if (!safety_result.success) {
-            return safety_result;
-        }
-        if (!validMeasuredPosition(state.actual_joint_state)) {
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::FAILED_PRECONDITION,
-                "RobotArm initial joint position cache is invalid");
-        }
-        if (request.requested_command_rate_hz() == 0) {
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::INVALID_ARGUMENT,
-                "requested_command_rate_hz must be non-zero");
-        }
-        const double requested_period_s =
-            1.0 /
-            static_cast<double>(request.requested_command_rate_hz());
-        // A client may request a slower command stream, but it may not claim a
-        // rate faster than the reviewed RobotArm servo period.
-        if (requested_period_s + 1e-12 <
-            config_.servo_period_s()) {
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::FAILED_PRECONDITION,
-                "requested command rate exceeds configured RobotArm servo rate");
-        }
-        minimum_dispatch_period_ =
-            std::chrono::duration_cast<Clock::duration>(
-                std::chrono::duration<double>(
-                    std::max(
-                        requested_period_s,
-                        config_.servo_period_s())));
-
-        device::ServoOptions options;
-        options.period = config_.servo_period_s();
-        const auto start = Clock::now();
-        const auto result = arm_->startServoMode(options);
-        const auto elapsed = Clock::now() - start;
-        if (!result.ok()) {
-            return fromArmResult(result, "startServoMode");
-        }
-        if (elapsed > std::chrono::microseconds(
-                          config_.max_apply_duration_us())) {
-            bestEffortStop();
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::DEADLINE_EXCEEDED,
-                "startServoMode exceeded max_apply_duration_us");
-        }
-
-        initial_position_ = state.actual_joint_state.position;
-        last_position_.clear();
-        last_dispatch_time_ = Clock::time_point{};
-        session_open_ = true;
-        return ArmTeleopBackendResult::ok();
-    }
-
-    ArmTeleopBackendResult applySetpoint(
-        const arm_teleop::JointSetpoint& setpoint,
-        const Clock::time_point deadline) override
-    {
-        std::lock_guard operation_lock(operation_mutex_);
-        if (Clock::now() >= deadline) {
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::DEADLINE_EXCEEDED,
-                "setpoint expired before RobotArm backend validation");
-        }
-        if (!session_open_) {
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::FAILED_PRECONDITION,
-                "RobotArm teleoperation servo mode is not open");
-        }
-
-        const auto input_result = validateSetpointInput(setpoint);
-        if (!input_result.success) {
-            return input_result;
-        }
-
-        const auto state = arm_->getRobotState();
-        cacheState(state);
-        const auto safety_result = validateSafety(state);
-        if (!safety_result.success) {
-            return safety_result;
-        }
-
-        const std::vector<double> target(
-            setpoint.position_rad().begin(),
-            setpoint.position_rad().end());
-        const auto& reference =
-            last_position_.empty() ? initial_position_ : last_position_;
-        const double allowed_step =
-            last_position_.empty()
-                ? config_.max_initial_position_step_rad()
-                : config_.max_position_step_rad();
-        for (std::size_t index = 0; index < target.size(); ++index) {
-            const double delta = std::abs(target[index] - reference[index]);
-            if (delta > allowed_step) {
-                std::ostringstream detail;
-                detail << "joint " << model_.joint_names[index]
-                       << " position step " << delta
-                       << " exceeds configured limit " << allowed_step;
-                return ArmTeleopBackendResult::failure(
-                    grpc::StatusCode::OUT_OF_RANGE, detail.str());
-            }
-            // Once a target has been accepted, also enforce the RobotModel
-            // velocity limit on target-to-target motion.
-            if (!last_position_.empty() &&
-                delta /
-                        std::chrono::duration<double>(
-                            minimum_dispatch_period_)
-                            .count() >
-                    model_.joint_limits[index].max_velocity) {
-                std::ostringstream detail;
-                detail << "joint " << model_.joint_names[index]
-                       << " target delta exceeds RobotModel velocity limit";
-                return ArmTeleopBackendResult::failure(
-                    grpc::StatusCode::OUT_OF_RANGE, detail.str());
-            }
-        }
-
-        const auto dispatch_time = Clock::now();
-        if (last_dispatch_time_ != Clock::time_point{} &&
-            dispatch_time - last_dispatch_time_ <
-                minimum_dispatch_period_) {
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::RESOURCE_EXHAUSTED,
-                "setpoint arrived before the negotiated RobotArm dispatch period");
-        }
-
-        device::JointPositionCommand command;
-        command.position = target;
-        // Robot state validation and command preparation may consume the
-        // remaining validity window. Re-check on the receiver's monotonic
-        // timeline at the last point before the RobotArm commit.
-        const auto start = Clock::now();
-        if (start >= deadline) {
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::DEADLINE_EXCEEDED,
-                "setpoint expired before RobotArm command dispatch");
-        }
-        if (deadline - start <
-            std::chrono::microseconds(
-                config_.max_apply_duration_us())) {
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::DEADLINE_EXCEEDED,
-                "setpoint lacks the configured RobotArm apply-time budget");
-        }
-        last_dispatch_time_ = start;
-        const auto result = arm_->servoJ(command);
-        const auto elapsed = Clock::now() - start;
-        if (!result.ok()) {
-            return fromArmResult(result, "servoJ");
-        }
-
-        // servoJ has already accepted this target even when the local timing
-        // contract is exceeded; remember it before returning the failure so a
-        // caller can never treat an older target as the last applied command.
-        last_position_ = target;
-        if (elapsed > std::chrono::microseconds(
-                          config_.max_apply_duration_us())) {
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::DEADLINE_EXCEEDED,
-                "servoJ exceeded max_apply_duration_us");
-        }
-        return ArmTeleopBackendResult::ok();
-    }
-
-    ArmTeleopBackendResult stop(
-        const arm_teleop::StopReason,
-        const std::string&) override
-    {
-        std::lock_guard operation_lock(operation_mutex_);
-        const auto motion_result = arm_ ? arm_->stopMotion()
-                                        : device::Result::success();
-        // This is deliberately called even when stopMotion fails.
-        const auto servo_result = arm_ ? arm_->stopServoMode()
-                                       : device::Result::success();
-        session_open_ = false;
-        initial_position_.clear();
-        last_position_.clear();
-        last_dispatch_time_ = Clock::time_point{};
-        minimum_dispatch_period_ = Clock::duration::zero();
-        if (arm_) {
-            cacheState(arm_->getRobotState());
-        }
-        if (!motion_result.ok()) {
-            return fromArmResult(motion_result, "stopMotion");
-        }
-        return fromArmResult(servo_result, "stopServoMode");
-    }
-
-    ArmTeleopBackendSnapshot snapshot() const override
-    {
-        std::lock_guard cache_lock(cache_mutex_);
-        auto result = cached_snapshot_;
-        if (cache_time_ != Clock::time_point{}) {
-            const auto age =
-                std::chrono::duration_cast<std::chrono::microseconds>(
-                    Clock::now() - cache_time_)
-                    .count();
-            result.joint_state.set_sample_age_us(
-                age > 0 ? static_cast<std::uint64_t>(age) : 0U);
-        }
-        return result;
-    }
-
-private:
-    void validateStaticConfiguration()
-    {
-        if (!config_.enable()) {
-            unavailable_reason_ =
-                "RobotArm teleoperation backend is explicitly disabled";
-            return;
-        }
-        if (!arm_) {
-            unavailable_reason_ =
-                "configured RobotArm device was not found";
-            return;
-        }
-        if (config_.device_id().empty() ||
-            config_.device_id() != arm_->id()) {
-            unavailable_reason_ =
-                "arm_teleop device_id must exactly match RobotArm.id";
-            return;
-        }
-        if (!arm_->supportsTeleopGroupServo()) {
-            unavailable_reason_ =
-                "RobotArm teleop group-servo capability is not enabled";
-            return;
-        }
-        if (!isDigest(config_.model_sha256()) ||
-            !isDigest(config_.calibration_sha256())) {
-            unavailable_reason_ =
-                "arm_teleop model and calibration SHA256 values must be 64 hex characters";
-            return;
-        }
-        if (config_.base_frame().empty() ||
-            config_.tool_frame().empty()) {
-            unavailable_reason_ =
-                "arm_teleop base_frame and tool_frame are required";
-            return;
-        }
-        if (!std::isfinite(config_.servo_period_s()) ||
-            config_.servo_period_s() < kMinimumServoPeriodS ||
-            config_.servo_period_s() > kMaximumServoPeriodS) {
-            unavailable_reason_ =
-                "arm_teleop servo_period_s must be in [0.0001, 0.1]";
-            return;
-        }
-        const double period_us =
-            config_.servo_period_s() * 1000000.0;
-        if (config_.max_apply_duration_us() == 0 ||
-            static_cast<double>(config_.max_apply_duration_us()) >
-                period_us) {
-            unavailable_reason_ =
-                "arm_teleop max_apply_duration_us must be non-zero and no greater than one servo period";
-            return;
-        }
-        if (!std::isfinite(config_.max_initial_position_step_rad()) ||
-            config_.max_initial_position_step_rad() <= 0.0 ||
-            !std::isfinite(config_.max_position_step_rad()) ||
-            config_.max_position_step_rad() <= 0.0) {
-            unavailable_reason_ =
-                "arm_teleop position step limits must be finite and positive";
-            return;
-        }
-
-        model_ = arm_->getRobotModel();
-        if (!model_.valid() ||
-            model_.joint_limits.size() != model_.dof) {
-            unavailable_reason_ =
-                "RobotModel must contain one safety limit for every joint";
-            return;
-        }
-        std::unordered_set<std::string> names;
-        for (std::size_t index = 0; index < model_.dof; ++index) {
-            const auto& name = model_.joint_names[index];
-            const auto& limit = model_.joint_limits[index];
-            if (name.empty() || !names.insert(name).second ||
-                !std::isfinite(limit.lower) ||
-                !std::isfinite(limit.upper) ||
-                !std::isfinite(limit.max_velocity) ||
-                limit.lower >= limit.upper ||
-                limit.max_velocity <= 0.0) {
-                unavailable_reason_ =
-                    "RobotModel joint names and position/velocity limits are invalid";
-                return;
-            }
-        }
-
-        manifest_.set_robot_id(config_.device_id());
-        manifest_.set_model_sha256(config_.model_sha256());
-        manifest_.set_calibration_sha256(
-            config_.calibration_sha256());
-        for (const auto& name : model_.joint_names) {
-            manifest_.add_joint_names(name);
-        }
-        manifest_.set_position_unit("rad");
-        manifest_.set_velocity_unit("rad/s");
-        manifest_.set_effort_unit("N*m");
-        manifest_.set_base_frame(config_.base_frame());
-        manifest_.set_tool_frame(config_.tool_frame());
-    }
-
-    ArmTeleopBackendResult validateSafety(
-        const device::ArmState& state) const
-    {
-        if (!state.connected) {
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::FAILED_PRECONDITION,
-                "RobotArm is not connected");
-        }
-        if (require_powered_ && !state.powered_on) {
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::FAILED_PRECONDITION,
-                "RobotArm is not powered on");
-        }
-        if (state.emergency_stopped ||
-            state.safety_mode == device::SafetyMode::EmergencyStop ||
-            state.safety_mode == device::SafetyMode::SystemEmergencyStop) {
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::FAILED_PRECONDITION,
-                "RobotArm emergency stop is active");
-        }
-        if (state.protective_stopped ||
-            state.safety_mode == device::SafetyMode::ProtectiveStop ||
-            state.safety_mode == device::SafetyMode::SafeguardStop) {
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::FAILED_PRECONDITION,
-                "RobotArm protective stop is active");
-        }
-        if (state.fault ||
-            state.robot_mode == device::RobotMode::Fault ||
-            state.safety_mode == device::SafetyMode::Fault) {
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::FAILED_PRECONDITION,
-                "RobotArm fault is active");
-        }
-        return ArmTeleopBackendResult::ok();
-    }
-
-    bool validMeasuredPosition(
-        const device::JointGroupState& state) const
-    {
-        if (!state.position_valid ||
-            state.position.size() != model_.dof) {
-            return false;
-        }
-        for (std::size_t index = 0; index < model_.dof; ++index) {
-            const double value = state.position[index];
-            const auto& limit = model_.joint_limits[index];
-            if (!std::isfinite(value) ||
-                value < limit.lower || value > limit.upper) {
-                return false;
-            }
-        }
-        return true;
-    }
-
-    ArmTeleopBackendResult validateSetpointInput(
-        const arm_teleop::JointSetpoint& setpoint) const
-    {
-        if (setpoint.position_rad_size() !=
-                static_cast<int>(model_.dof) ||
-            setpoint.velocity_rad_s_size() !=
-                static_cast<int>(model_.dof)) {
-            return ArmTeleopBackendResult::failure(
-                grpc::StatusCode::INVALID_ARGUMENT,
-                "setpoint dimensions do not match RobotModel");
-        }
-        for (std::size_t index = 0; index < model_.dof; ++index) {
-            const double position =
-                setpoint.position_rad(static_cast<int>(index));
-            const double velocity =
-                setpoint.velocity_rad_s(static_cast<int>(index));
-            const auto& limit = model_.joint_limits[index];
-            if (!std::isfinite(position) || !std::isfinite(velocity)) {
-                return ArmTeleopBackendResult::failure(
-                    grpc::StatusCode::INVALID_ARGUMENT,
-                    "setpoint position and velocity must be finite");
-            }
-            if (position < limit.lower || position > limit.upper) {
-                return ArmTeleopBackendResult::failure(
-                    grpc::StatusCode::OUT_OF_RANGE,
-                    "setpoint position exceeds RobotModel joint limit");
-            }
-            if (std::abs(velocity) > limit.max_velocity) {
-                return ArmTeleopBackendResult::failure(
-                    grpc::StatusCode::OUT_OF_RANGE,
-                    "setpoint velocity exceeds RobotModel joint limit");
-            }
-        }
-        return ArmTeleopBackendResult::ok();
-    }
-
-    void cacheState(const device::ArmState& state) const
-    {
-        ArmTeleopBackendSnapshot snapshot;
-        auto* joint = &snapshot.joint_state;
-        const auto& source = state.actual_joint_state;
-        joint->set_sample_sequence(source.sequence);
-        for (const double value : source.position) {
-            joint->add_position_rad(value);
-        }
-        for (const double value : source.velocity) {
-            joint->add_velocity_rad_s(value);
-        }
-
-        const auto effort_source =
-            toProtoEffortSource(arm_->jointEffortSource());
-        const bool effort_valid =
-            source.effort_valid &&
-            source.effort.size() == model_.dof &&
-            effort_source != arm_teleop::EFFORT_SOURCE_UNSPECIFIED &&
-            std::all_of(
-                source.effort.begin(), source.effort.end(),
-                [](const double value) { return std::isfinite(value); });
-        if (effort_valid) {
-            for (const double value : source.effort) {
-                joint->add_effort_nm(value);
-            }
-        }
-        joint->set_position_valid(validMeasuredPosition(source));
-        joint->set_velocity_valid(
-            source.velocity_valid &&
-            source.velocity.size() == model_.dof &&
-            std::all_of(
-                source.velocity.begin(), source.velocity.end(),
-                [](const double value) { return std::isfinite(value); }));
-        joint->set_effort_valid(effort_valid);
-        joint->set_effort_source(
-            effort_valid ? effort_source
-                         : arm_teleop::EFFORT_SOURCE_UNSPECIFIED);
-
-        auto* safety = &snapshot.safety;
-        safety->set_connected(state.connected);
-        safety->set_powered_on(state.powered_on);
-        safety->set_protective_stopped(state.protective_stopped);
-        safety->set_emergency_stopped(state.emergency_stopped);
-        safety->set_fault(
-            state.fault ||
-            state.robot_mode == device::RobotMode::Fault ||
-            state.safety_mode == device::SafetyMode::Fault);
-        if (safety->fault()) {
-            safety->set_fault_detail("RobotArm reports a fault");
-        }
-
-        std::lock_guard cache_lock(cache_mutex_);
-        cached_snapshot_ = std::move(snapshot);
-        cache_time_ = Clock::now();
-    }
-
-    void bestEffortStop() noexcept
-    {
-        try {
-            arm_->stopMotion();
-        } catch (...) {
-        }
-        try {
-            arm_->stopServoMode();
-        } catch (...) {
-        }
-        session_open_ = false;
-        initial_position_.clear();
-        last_position_.clear();
-        last_dispatch_time_ = Clock::time_point{};
-        minimum_dispatch_period_ = Clock::duration::zero();
-    }
-
-    std::shared_ptr<device::RobotArm> arm_;
-    config::ArmTeleopBackendConfig config_;
-    bool require_powered_{true};
-    device::RobotModel model_;
-    arm_teleop::RobotManifest manifest_;
-    std::string unavailable_reason_;
-
-    mutable std::mutex operation_mutex_;
-    bool session_open_{false};
-    std::vector<double> initial_position_;
-    std::vector<double> last_position_;
-    Clock::time_point last_dispatch_time_{};
-    Clock::duration minimum_dispatch_period_{Clock::duration::zero()};
-
-    mutable std::mutex cache_mutex_;
-    mutable ArmTeleopBackendSnapshot cached_snapshot_;
-    mutable Clock::time_point cache_time_{};
-};
-
-} // namespace
-
-std::shared_ptr<ArmTeleopBackend> makeRobotArmTeleopBackend(
-    std::shared_ptr<device::RobotArm> arm,
-    const config::ArmTeleopBackendConfig& config)
-{
-    return std::make_shared<RobotArmTeleopBackend>(
-        std::move(arm), config);
-}
-
-} // namespace cmvr::service