merge: apply xtkuang code outside hardware-specific drivers

CMakeLists.txt

@@ -9,11 +9,29 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
 #set(CMAKE_CXX_STANDARD_REQUIRED True)
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
-# Tests are opt-in so production builds keep the existing footprint.
+# 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)
@@ -32,38 +50,9 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake")
 include(FindExternalLib)
 set(ARCH "x86")
 setup_external_libs(${ARCH})
-
-# Intel oneVPL / VA-API runtime. The shared libraries in lib/ are installed
-# by setup_external_libs(); the VA-API driver plugin directory is installed
-# separately because it must retain its dri layout.
-set(INTEL_MEDIA_STACK_ROOT
-        "${PROJECT_SOURCE_DIR}/dependency/${ARCH}/third_party/intel-media-stack/vpl-2.17"
-)
-set(INTEL_MEDIA_DRIVER_DIR "${INTEL_MEDIA_STACK_ROOT}/lib/dri")
-set(INTEL_IHD_DRIVER "${INTEL_MEDIA_DRIVER_DIR}/iHD_drv_video.so")
-
-if(NOT EXISTS "${INTEL_IHD_DRIVER}")
-    message(FATAL_ERROR "Intel iHD VA-API driver not found: ${INTEL_IHD_DRIVER}")
+if(BUILD_TESTING AND CMVR_EXTERNAL_LIBRARY_DIRS)
+    list(JOIN CMVR_EXTERNAL_LIBRARY_DIRS ":" CMVR_TEST_EXTERNAL_LIBRARY_PATH)
 endif()
-
-message(STATUS "Intel media stack: ${INTEL_MEDIA_STACK_ROOT}")
-install(
-        DIRECTORY "${INTEL_MEDIA_DRIVER_DIR}/"
-        DESTINATION lib/dri
-)
-install(CODE [=[
-    find_program(CMVR_PATCHELF_EXECUTABLE patchelf REQUIRED)
-    set(_cmvr_ihd_driver
-            "${CMAKE_INSTALL_PREFIX}/lib/dri/iHD_drv_video.so"
-    )
-    execute_process(
-            COMMAND "${CMVR_PATCHELF_EXECUTABLE}"
-                    --set-rpath "$ORIGIN/.."
-                    "${_cmvr_ihd_driver}"
-            COMMAND_ERROR_IS_FATAL ANY
-    )
-]=])
-
 # 在调用 setup_external_libs 之后
 message(STATUS "CMAKE_EXE_LINKER_FLAGS: ${CMAKE_EXE_LINKER_FLAGS}")
 message(STATUS "CMAKE_SHARED_LINKER_FLAGS: ${CMAKE_SHARED_LINKER_FLAGS}")
@@ -81,8 +70,9 @@ file(GLOB_RECURSE PROTO_FILES ${PROTO_IMPORT_DIR}/*.proto)
 
 
 set(Protobuf_PROTOC_EXECUTABLE "${CMAKE_INSTALL_PREFIX}/bin/protoc" CACHE FILEPATH "" FORCE)
-set_property(TARGET gRPC::grpc_cpp_plugin
-        PROPERTY IMPORTED_LOCATION "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin"
+set_target_properties(gRPC::grpc_cpp_plugin PROPERTIES
+        IMPORTED_LOCATION "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin"
+        IMPORTED_LOCATION_RELEASE "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin"
 )
 
 # 1) 先做 OBJECT：只负责生成/编译 pb.cc
@@ -161,17 +151,6 @@ target_link_libraries(cmvr_es PRIVATE
 )
 
 install(TARGETS cmvr_es RUNTIME DESTINATION bin)
-
-add_executable(cmvr_config_server
-        cmvr-es/config_server/config_server_main.cpp
-        cmvr-es/config_server/config_file_service.cpp
-)
-target_link_libraries(cmvr_config_server PRIVATE
-        cmvr_es::proto
-        protobuf::libprotobuf
-        gRPC::grpc++
-)
-install(TARGETS cmvr_config_server RUNTIME DESTINATION bin)
 install(CODE [[
     file(REMOVE_RECURSE
         "${CMAKE_INSTALL_PREFIX}/bin/config"

README.md

@@ -138,3 +138,14 @@ $IGH_ETHERCAT_ROOT/bin/ethercat pdos
 sudo script/ethercat/stop_ethercat.sh eno1
 sudo script/ethercat/stop_ethercat.sh eno1 --restore-network
 ```
+
+## 组件文档
+
+具体能力、配置、协议和安全边界由对应代码目录下的 README 维护：
+
+- [MotorService gRPC 接口](cmvr-es/service/README.md#motorservice)
+- [电机设备模块](cmvr-es/devices/motor/README.md)
+- [AUBO 控制柜 Standard 数字 IO](cmvr-es/devices/arm/aubo_arm/README.md)
+- [配置与部署规则](cmvr-es/config/README.md)
+
+AUBO JSON 接口只访问控制柜 Standard 数字 IO，不访问安全 IO。

cmake/FindExternalLib.cmake

@@ -139,6 +139,7 @@ function(setup_external_libs ARCH)
         list(REMOVE_DUPLICATES LIBRARY_DIRS)
         link_directories(${LIBRARY_DIRS})
     endif()
+    set(CMVR_EXTERNAL_LIBRARY_DIRS "${LIBRARY_DIRS}" PARENT_SCOPE)
 
     # ---- install third-party shared libs into <prefix>/lib ----
     if(INSTALL_SO_FILES)

cmvr-es/CMakeLists.txt

@@ -10,7 +10,6 @@ add_subdirectory(manager/control_authority_manager)
 add_subdirectory(manager/safety_manager)
 add_subdirectory(manager/device_manager)
 add_subdirectory(service/grpc/stop_all)
-add_subdirectory(manager/media_source_hub)
 add_subdirectory(manager/media_source_manager)
 add_subdirectory(service/quic_edge)
 add_subdirectory(task)

cmvr-es/common/CMakeLists.txt

@@ -28,9 +28,3 @@ target_link_libraries(common PUBLIC
 
 add_library(cmvr_es::common ALIAS common)
 install(TARGETS common LIBRARY DESTINATION lib)
-
-add_executable(support_functions_test
-        math/support_functions_test.cpp
-)
-target_include_directories(support_functions_test PRIVATE ${CMAKE_SOURCE_DIR}/cmvr-es)
-target_link_libraries(support_functions_test PRIVATE gtest gtest_main glog)

cmvr-es/common/README.md

@@ -54,7 +54,7 @@ AGV 通用类型应参考 [`types/agv/agv_types.h`](types/agv/agv_types.h)，机
 - AAC、Opus、PCM 明确 payload format、采样率和声道数；
 - 不把 QUIC、gRPC 或浏览器专有字段加入通用帧。
 
-设备媒体接入流程见 [`../manager/README.md`](../manager/README.md) 的 MediaSourceHub 章节。
+设备媒体接入流程见 [`../manager/README.md`](../manager/README.md) 的 MediaSourceManager 章节。
 
 ## 环形队列选择
 

cmvr-es/common/base/logging/logger.cpp

@@ -167,6 +167,15 @@ void Logger::shutdown()
     initialized_ = false;
 }
 
+void Logger::flush()
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (log_file_.is_open()) {
+        log_file_.flush();
+        last_flush_ = std::chrono::steady_clock::now();
+    }
+}
+
 bool Logger::enabled(const Level level) const
 {
     std::lock_guard<std::mutex> lock(mutex_);
@@ -200,7 +209,8 @@ void Logger::write(const Level level,
         rotateIfNeeded_();
         log_file_ << line << '\n';
         const auto now = std::chrono::steady_clock::now();
-        if (level == Level::ERROR || level == Level::FATAL || now - last_flush_ >= flush_interval_) {
+        if (level == Level::ERROR || level == Level::FATAL ||
+            now - last_flush_ >= flush_interval_) {
             log_file_.flush();
             last_flush_ = now;
         }

cmvr-es/common/base/logging/logger.h

@@ -43,6 +43,7 @@ public:
                     const std::string& application_name,
                     const std::filesystem::path& executable_directory);
     void shutdown();
+    void flush();
     bool enabled(Level level) const;
     void write(Level level, const char* source_file, int source_line, const std::string& message);
 

cmvr-es/devices/agv/src1100/CMakeLists.txt

@@ -1,12 +0,0 @@
-add_library(src1100_agv SHARED src/src1100_agv.cpp)
-
-target_include_directories(src1100_agv PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
-
-target_link_libraries(src1100_agv
-        PUBLIC
-        cmvr_es::proto
-        jsoncpp
-)
-
-add_library(cmvr_es::device::src1100_agv ALIAS src1100_agv)
-install(TARGETS src1100_agv LIBRARY DESTINATION lib)

cmvr-es/devices/agv/src1100/include/src1100_agv.h

@@ -1,179 +0,0 @@
-#ifndef CMVR_ES_SRC1100_AGV_H
-#define CMVR_ES_SRC1100_AGV_H
-
-#include <atomic>
-#include <condition_variable>
-#include <cstdint>
-#include <deque>
-#include <mutex>
-#include <string>
-#include <thread>
-#include <vector>
-
-#include <json/json.h>
-
-#include "cmvr/config/agv_config/agv_config.pb.h"
-#include "devices/agv/abstract_agv.h"
-
-namespace cmvr::device {
-
-class Src1100Agv final : public AbstractAGV {
-public:
-    explicit Src1100Agv(const config::Src1100AgvConfig& cfg);
-    ~Src1100Agv() override;
-
-    std::string typeName() const override { return "Src1100Agv"; }
-
-    bool init() override;
-    bool start() override;
-    bool stop() override;
-    bool update() override;
-
-    AgvRuntimeState runtimeState() const override;
-    AgvNavigationStatus navigationStatus() const override;
-
-    AgvResult emergencyStop() override;
-    AgvResult clearFault() override;
-
-    AgvResult navigateToPose(
-        const math::Pose2d& pose,
-        const AgvMotionOptions& options = {},
-        const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
-    AgvResult navigateToStation(
-        const std::string& station_id,
-        const AgvMotionOptions& options = {},
-        const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
-    AgvResult followPath(const std::vector<AgvPathSegment>& path) override;
-    AgvResult pauseNavigation() override;
-    AgvResult resumeNavigation() override;
-    AgvResult cancelNavigation() override;
-
-    AgvResult setVelocity(const AgvVelocity& velocity) override;
-
-    AgvResult listMaps(std::vector<std::string>& maps) const override;
-    AgvResult listStations(std::vector<AgvStation>& stations) const override;
-    AgvResult switchMap(const std::string& map_name) override;
-    AgvResult uploadMap(const std::string& map_name, const std::string& content) override;
-    AgvResult downloadMap(const std::string& map_name, std::string& content) const override;
-    AgvResult startMapping(const AgvMappingOptions& options = {}) override;
-    AgvResult getMappingData(int start_index, AgvMappingData& data) const override;
-    AgvResult getUnifiedMapUpdate(
-        std::uint64_t after_sequence,
-        const AgvMapStreamOptions& options,
-        AgvUnifiedMapUpdate& update) const override;
-    AgvResult stopMapping() override;
-
-private:
-    struct Ports {
-        int status{19204};
-        int control{19205};
-        int navigation{19206};
-        int config{19207};
-        int other{19210};
-        int push{19301};
-    };
-
-    AgvResult connect_();
-    AgvResult disconnect_();
-    AgvResult connectSocket_(int& sock, int port);
-    AgvResult ensureOtherSocket_();
-    void closeSocket_(int& sock) const;
-    bool connected_() const;
-
-    AgvResult sendCommand_(int sock,
-                           std::uint16_t command,
-                           const Json::Value& payload,
-                           Json::Value* response) const;
-    AgvResult sendCommandRaw_(int sock,
-                              std::uint16_t command,
-                              const Json::Value& payload,
-                              std::string* response_payload) const;
-    AgvResult sendCommandNoResponse_(int sock, std::uint16_t command, const Json::Value& payload) const;
-    AgvResult configurePush_();
-    void startPushThread_();
-    void stopPushThread_();
-    void pushLoop_();
-    AgvRuntimeState queryRuntimeState_() const;
-    void updateCachedRuntimeState_(const Json::Value& payload);
-    void startMapUpdateThread_();
-    void stopMapUpdateThread_();
-    void mapUpdateLoop_();
-    AgvResult refreshMapCacheOnce_(const AgvMapStreamOptions& options) const;
-    AgvResult parseMapFileToUpdates_(
-        const std::string& file_name,
-        const std::string& content,
-        const AgvMapStreamOptions& options,
-        std::vector<AgvUnifiedMapUpdate>& updates) const;
-    AgvResult parseSrc1100MapArchive_(
-        const std::string& file_name,
-        const std::string& content,
-        const AgvMapStreamOptions& options,
-        std::vector<AgvUnifiedMapUpdate>& updates) const;
-    AgvResult parseSrc1100Map2D_(
-        const std::string& file_name,
-        const std::string& content,
-        const AgvMapStreamOptions& options,
-        AgvUnifiedMapUpdate& update) const;
-    AgvResult parseSrc1100Map3D_(
-        const std::string& file_name,
-        const std::string& content,
-        const AgvMapStreamOptions& options,
-        AgvUnifiedMapUpdate& update) const;
-    void cacheMapUpdates_(std::vector<AgvUnifiedMapUpdate> updates) const;
-    bool findCachedMapUpdate_(
-        std::uint64_t after_sequence,
-        const AgvMapStreamOptions& options,
-        AgvUnifiedMapUpdate& update) const;
-    bool mapUpdateMatches_(
-        const AgvUnifiedMapUpdate& update,
-        const AgvMapStreamOptions& options) const;
-
-    static std::vector<std::uint8_t> buildFrame_(std::uint16_t command, const std::string& payload);
-    static std::string toJsonString_(const Json::Value& value);
-    static bool parseJson_(const std::string& input, Json::Value& output, std::string& error);
-    static std::string extractJson_(const std::string& raw);
-    static AgvResult receiveFrame_(int sock, std::uint16_t& command, std::string& payload);
-    static int optionalInt_(const AgvAdapterParams& params, const std::string& key, int fallback);
-    static double optionalDouble_(const AgvAdapterParams& params, const std::string& key, double fallback);
-    static void applyMotionOptions_(Json::Value& payload, const AgvMotionOptions& options);
-    static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params);
-    static AgvResult resultFromResponse_(const Json::Value& response);
-
-    config::Src1100AgvConfig config_;
-    std::string ip_;
-    int recv_timeout_ms_{1000};
-    Ports ports_;
-    bool state_push_enabled_{false};
-    bool map_update_enabled_{false};
-    int map_update_interval_ms_{1000};
-    std::size_t map_update_history_size_{8};
-
-    mutable std::mutex mutex_;
-    int sock_status_{-1};
-    int sock_control_{-1};
-    int sock_navigation_{-1};
-    int sock_config_{-1};
-    int sock_other_{-1};
-    int sock_push_{-1};
-    std::string last_error_;
-
-    std::atomic<bool> push_running_{false};
-    std::thread push_thread_;
-    mutable std::mutex runtime_state_mutex_;
-    AgvRuntimeState cached_runtime_state_;
-    bool cached_runtime_state_valid_{false};
-
-    mutable std::atomic<bool> map_update_running_{false};
-    mutable std::thread map_update_thread_;
-    mutable std::mutex map_update_mutex_;
-    mutable std::condition_variable map_update_cv_;
-    mutable std::deque<AgvUnifiedMapUpdate> cached_map_updates_;
-    mutable std::uint64_t map_sequence_{0};
-    mutable int next_mapping_index_{0};
-    mutable std::size_t last_map_content_hash_{0};
-    mutable std::string map_session_id_;
-};
-
-} // namespace cmvr::device
-
-#endif // CMVR_ES_SRC1100_AGV_H

cmvr-es/devices/biohead/abstract_biohead.h

@@ -1,6 +1,11 @@
 #ifndef ABSTRACT_BIOHEAD_H
 #define ABSTRACT_BIOHEAD_H
 #pragma once
+
+#include <cstdint>
+#include <mutex>
+#include <utility>
+
 #include "../abstract_device.h"
 
 namespace cmvr::device {
@@ -58,6 +63,8 @@ namespace cmvr::device {
     // 抽象头部类
     class AbstractBiohead : public AbstractDevice {
     public:
+        using OperationalToken = std::uint64_t;
+
         AbstractBiohead() = default;
         ~AbstractBiohead() override = default;
 
@@ -76,20 +83,140 @@ namespace cmvr::device {
         virtual void expressionSadness() {};
         virtual void expressionYawn() {};
 
+        // Capture under the process-wide StopAll admission gate. Commands
+        // from an older generation are rejected after operational stop.
+        OperationalToken beginOperationalActivity() const noexcept
+        {
+            std::lock_guard lock(operational_mutex_);
+            return operational_generation_;
+        }
+
+        virtual bool setExpressionPoseIfCurrent(
+            OperationalToken token,
+            FacialExpressionState& expression_state,
+            double vel = 0.5,
+            double acc = 0.1)
+        {
+            return runIfOperationalActivityCurrent_(token, [&] {
+                setExpressionPose(expression_state, vel, acc);
+            });
+        }
+
+        virtual bool streamFacialPoseIfCurrent(
+            OperationalToken token,
+            FacialExpressionState& expression_state,
+            double vel,
+            double acc)
+        {
+            return runIfOperationalActivityCurrent_(token, [&] {
+                streamFacialPose(expression_state, vel, acc);
+            });
+        }
+
+        virtual bool speakStartIfCurrent(OperationalToken token)
+        {
+            return runIfOperationalActivityCurrent_(token, [&] {
+                speakstart();
+            });
+        }
+
+        virtual bool expressionHappyIfCurrent(OperationalToken token)
+        {
+            return runIfOperationalActivityCurrent_(token, [&] {
+                expressionHappy();
+            });
+        }
+
+        virtual bool expressionSurprisedIfCurrent(OperationalToken token)
+        {
+            return runIfOperationalActivityCurrent_(token, [&] {
+                expressionSurprised();
+            });
+        }
+
+        virtual bool expressionTiredIfCurrent(OperationalToken token)
+        {
+            return runIfOperationalActivityCurrent_(token, [&] {
+                expressionTired();
+            });
+        }
+
+        virtual bool expressionAngryIfCurrent(OperationalToken token)
+        {
+            return runIfOperationalActivityCurrent_(token, [&] {
+                expressionAngry();
+            });
+        }
+
+        virtual bool expressionSadnessIfCurrent(OperationalToken token)
+        {
+            return runIfOperationalActivityCurrent_(token, [&] {
+                expressionSadness();
+            });
+        }
+
+        virtual bool expressionYawnIfCurrent(OperationalToken token)
+        {
+            return runIfOperationalActivityCurrent_(token, [&] {
+                expressionYawn();
+            });
+        }
+
+        // Stops expression motion and speaking without closing the device.
+        // True confirms that old activity was fenced and the hold completed.
+        virtual bool stopOperationalActivity()
+        {
+            invalidateOperationalActivities_();
+            speakstop();
+            (void)runOperationalStop_([&] { eStop(); });
+            return false;
+        }
+
         FacialExpressionState expression_state_;
-        std::atomic<bool> emergency_stop_requested = false;
-
+    protected:
+        template <typename Operation>
+        bool runIfOperationalActivityCurrent_(
+            const OperationalToken token,
+            Operation&& operation)
+        {
+            std::lock_guard lock(operational_mutex_);
+            if (token == 0U || token != operational_generation_) {
+                return false;
+            }
+            std::forward<Operation>(operation)();
+            return true;
+        }
 
+        template <typename Operation>
+        bool runOperationalStop_(Operation&& operation)
+        {
+            std::lock_guard lock(operational_mutex_);
+            std::forward<Operation>(operation)();
+            return true;
+        }
 
+        bool operationalActivityCurrent_(
+            const OperationalToken token) const noexcept
+        {
+            std::lock_guard lock(operational_mutex_);
+            return token != 0U && token == operational_generation_;
+        }
 
+        void invalidateOperationalActivities_() noexcept
+        {
+            std::lock_guard lock(operational_mutex_);
+            ++operational_generation_;
+            if (operational_generation_ == 0U) {
+                ++operational_generation_;
+            }
+        }
 
+    private:
+        mutable std::mutex operational_mutex_;
+        OperationalToken operational_generation_{1U};
     };
 
 } // namespace cmvr::device
 
 #endif // ABSTRACT_BIOHEAD_H
 
-
-
-
-

cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h

@@ -4,10 +4,12 @@
 #include "../../abstract_biohead.h"
 #include "../../../../hardware/include/esp32_serial_port.h"
 #include "cmvr/config/biohead_config/biohead_config.pb.h"
+#include <atomic>
 #include <vector>
 #include <string>
 #include <memory>
 #include <mutex>
+#include <condition_variable>
 
 namespace cmvr::device {
 
@@ -19,7 +21,7 @@ namespace cmvr::device {
     class BioHeadRobot : public AbstractBiohead {
     public:
         explicit BioHeadRobot(const config::BioHeadRobotConfig &config);
-        ~BioHeadRobot() override = default;
+        ~BioHeadRobot() override;
 
         std::string typeName() const override { return "BioHeadRobot"; }
         bool init() override;
@@ -30,7 +32,25 @@ namespace cmvr::device {
         void streamFacialPose(FacialExpressionState& expression_state, double vel, double acc) override;
         void speakstart() override;
         void speakstop() override;
-        void speakthread();
+        bool stopOperationalActivity() override;
+
+        bool setExpressionPoseIfCurrent(
+            OperationalToken token,
+            FacialExpressionState& expression_state,
+            double vel = 0.5,
+            double acc = 0.1) override;
+        bool streamFacialPoseIfCurrent(
+            OperationalToken token,
+            FacialExpressionState& expression_state,
+            double vel,
+            double acc) override;
+        bool speakStartIfCurrent(OperationalToken token) override;
+        bool expressionHappyIfCurrent(OperationalToken token) override;
+        bool expressionSurprisedIfCurrent(OperationalToken token) override;
+        bool expressionTiredIfCurrent(OperationalToken token) override;
+        bool expressionAngryIfCurrent(OperationalToken token) override;
+        bool expressionSadnessIfCurrent(OperationalToken token) override;
+        bool expressionYawnIfCurrent(OperationalToken token) override;
 
         void expressionHappy()override;
         void expressionSurprised()override;
@@ -43,11 +63,23 @@ namespace cmvr::device {
     private:
         // 内部方法
         void parseConfig(const config::BioHeadRobotConfig &config);
-        void sendServoCommands( const std::vector<double>& targets, uint16_t duration_ms);
+        bool sendServoCommands(
+            const std::vector<double>& targets,
+            uint16_t duration_ms,
+            bool force = false);
+        bool sendRawIfCurrent(
+            OperationalToken token,
+            const std::vector<uint8_t>& raw_data);
         uint16_t angleToRaw(double angle);
         double normalizeToAngle(double normalized, size_t index);
 
-        void sendExpression(const std::vector<double>& device_64_angles, const std::vector<double>& device_65_angles, int step_ms);
+        bool sendExpression(
+            OperationalToken token,
+            const std::vector<double>& device_64_angles,
+            const std::vector<double>& device_65_angles,
+            int step_ms);
+        bool startSpeaking(OperationalToken token);
+        void speakthread(OperationalToken token);
 
 
 
@@ -69,8 +101,9 @@ namespace cmvr::device {
 
         std::shared_ptr<std::thread> speak_thread_;
         std::atomic<bool> speak_running_{false};
-
-
+        std::mutex speak_mutex_;
+        std::mutex expression_wait_mutex_;
+        std::condition_variable expression_wait_cv_;
 
     };
 

cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp

@@ -21,6 +21,11 @@ BioHeadRobot::BioHeadRobot(const config::BioHeadRobotConfig &config) {
 
 }
 
+BioHeadRobot::~BioHeadRobot()
+{
+    speakstop();
+}
+
 
 
 bool BioHeadRobot::init() {
@@ -112,13 +117,36 @@ double BioHeadRobot::normalizeToAngle(double normalized, size_t index) {
 }
 
 void BioHeadRobot::getState(RobotState &state) {
+    std::lock_guard lock(stateMutex_);
     state.error = false;
     state.joint_positions = current_joints_;
 }
 
 void BioHeadRobot::eStop() {
     CMVR_LOG(WARNING) << "[BioHeadRobot] Emergency stop: hold current joint positions.";
-    sendServoCommands(current_joints_, 100);  // 快速下发当前角度
+    (void)sendServoCommands(last_joints_, 0, true);
+}
+
+bool BioHeadRobot::setExpressionPoseIfCurrent(
+    const OperationalToken token,
+    FacialExpressionState& expression_state,
+    const double vel,
+    const double acc)
+{
+    return runIfOperationalActivityCurrent_(token, [&] {
+        setExpressionPose(expression_state, vel, acc);
+    });
+}
+
+bool BioHeadRobot::streamFacialPoseIfCurrent(
+    const OperationalToken token,
+    FacialExpressionState& expression_state,
+    const double vel,
+    const double acc)
+{
+    return runIfOperationalActivityCurrent_(token, [&] {
+        streamFacialPose(expression_state, vel, acc);
+    });
 }
 
 void BioHeadRobot::setExpressionPose(FacialExpressionState& expression_state, double vel, double acc) {
@@ -160,8 +188,10 @@ void BioHeadRobot::setExpressionPose(FacialExpressionState& expression_state, do
     for (size_t i = 0; i < joints.size(); ++i) {
         CMVR_LOG(INFO) << "Joint[" << i << "] = " << joints[i];  // 打印每个关节的角度
     }
-    uint16_t duration = static_cast<uint16_t>(1000.0 / vel);
-    sendServoCommands(joints, duration);
+    const uint16_t duration = vel > 0.0
+        ? static_cast<uint16_t>(1000.0 / vel)
+        : 0U;
+    (void)sendServoCommands(joints, duration);
 
 }
 
@@ -208,17 +238,30 @@ void BioHeadRobot::streamFacialPose(FacialExpressionState& expression_state, dou
     CMVR_LOG(INFO) << "嘴角3=： "  << ": " << joints[15];
     CMVR_LOG(INFO) << "嘴角4=： "  << ": " << joints[16];
 
-    uint16_t duration = static_cast<uint16_t>(1000.0 / vel);
-    sendServoCommands(joints, duration);
+    const uint16_t duration = vel > 0.0
+        ? static_cast<uint16_t>(1000.0 / vel)
+        : 0U;
+    (void)sendServoCommands(joints, duration);
 }
 
 
 
 void BioHeadRobot::speakstart() {
+    (void)speakStartIfCurrent(beginOperationalActivity());
+}
+
+bool BioHeadRobot::speakStartIfCurrent(const OperationalToken token)
+{
+    return startSpeaking(token);
+}
+
+bool BioHeadRobot::startSpeaking(const OperationalToken token)
+{
+    std::lock_guard lock(speak_mutex_);
 
     if (speak_running_.load()) {
         CMVR_LOG(INFO) << "[BioHeadRobot] speak thread already running.";
-        return;
+        return operationalActivityCurrent_(token);
     }
 
     // 检查 channels 中是否有 65:8 和 65:9
@@ -229,12 +272,9 @@ void BioHeadRobot::speakstart() {
     }
     if (!found8 || !found9) {
         CMVR_LOG(ERROR) << "[BioHeadRobot] Required servo channels not found (addr 65 ch 8/9). speakstart aborted.";
-        return;
+        return false;
     }
 
-    // 启动线程
-    speak_running_.store(true);
-
     // 清理旧线程（若有）
     if (speak_thread_ && speak_thread_->joinable()) {
         try {
@@ -245,19 +285,24 @@ void BioHeadRobot::speakstart() {
         speak_thread_.reset();
     }
 
-    speak_thread_ = std::make_shared<std::thread>(&BioHeadRobot::speakthread, this);
+    bool started = false;
+    const bool current = runIfOperationalActivityCurrent_(token, [&] {
+        speak_running_.store(true, std::memory_order_release);
+        speak_thread_ = std::make_shared<std::thread>(
+            &BioHeadRobot::speakthread, this, token);
+        started = true;
+    });
+    if (!current || !started) {
+        speak_running_.store(false, std::memory_order_release);
+        return false;
+    }
     CMVR_LOG(INFO) << "[BioHeadRobot] speak thread started.";
+    return true;
 }
 
 void BioHeadRobot::speakstop() {
-    {
-        if (!speak_running_.load()) {
-            CMVR_LOG(INFO) << "[BioHeadRobot] speak thread not running.";
-            return;
-        }
-        speak_running_.store(false);
-    }
-    // 唤醒线程（如果在 wait 中）
+    std::lock_guard lock(speak_mutex_);
+    speak_running_.store(false, std::memory_order_release);
 
     // join 并清理线程对象
     if (speak_thread_) {
@@ -275,7 +320,20 @@ void BioHeadRobot::speakstop() {
     CMVR_LOG(INFO) << "[BioHeadRobot] speak thread stopped.";
 }
 
-void BioHeadRobot::speakthread() {
+bool BioHeadRobot::stopOperationalActivity()
+{
+    invalidateOperationalActivities_();
+    expression_wait_cv_.notify_all();
+    speakstop();
+
+    bool hold_confirmed = false;
+    (void)runOperationalStop_([&] {
+        hold_confirmed = sendServoCommands(last_joints_, 0, true);
+    });
+    return hold_confirmed;
+}
+
+void BioHeadRobot::speakthread(const OperationalToken token) {
     CMVR_LOG(INFO) << "[BioHeadRobot] speakthread running.";
 
     // 固定参数
@@ -313,7 +371,11 @@ void BioHeadRobot::speakthread() {
     }
 
     // 以当前角度为基准
-    std::vector<double> base = current_joints_;
+    std::vector<double> base;
+    {
+        std::lock_guard lock(stateMutex_);
+        base = current_joints_;
+    }
     if (base.size() != channels_.size()) {
         base.resize(channels_.size(), 90.0);
     }
@@ -346,7 +408,8 @@ void BioHeadRobot::speakthread() {
     double current_random_factor = 0.0;
     const double random_update_interval = 0.2;  // 每0.2秒更新一次随机扰动
 
-    while (speak_running_.load()) {
+    while (speak_running_.load(std::memory_order_acquire) &&
+           operationalActivityCurrent_(token)) {
         auto now = std::chrono::steady_clock::now();
         double t = std::chrono::duration_cast<std::chrono::duration<double>>(now - start).count();
 
@@ -452,7 +515,9 @@ void BioHeadRobot::speakthread() {
         }
 
         // 下发
-        serial_->sendRawServoData(raw_data);
+        if (!sendRawIfCurrent(token, raw_data)) {
+            break;
+        }
 
         // 控制循环频率
         std::this_thread::sleep_for(std::chrono::milliseconds(step_ms));
@@ -483,12 +548,23 @@ void BioHeadRobot::speakthread() {
         }
     }
 
-    serial_->sendRawServoData(restore_data);
-    CMVR_LOG(INFO) << "[BioHeadRobot] speakthread exiting and restored base pose.";
+    if (sendRawIfCurrent(token, restore_data)) {
+        CMVR_LOG(INFO)
+            << "[BioHeadRobot] speakthread exiting and restored base pose.";
+    } else {
+        CMVR_LOG(INFO)
+            << "[BioHeadRobot] speakthread stopped without a stale restore.";
+    }
+    speak_running_.store(false, std::memory_order_release);
 }
 
 
-void BioHeadRobot::sendExpression(const std::vector<double>& device_64_angles, const std::vector<double>& device_65_angles, int step_ms) {
+bool BioHeadRobot::sendExpression(
+    const OperationalToken token,
+    const std::vector<double>& device_64_angles,
+    const std::vector<double>& device_65_angles,
+    const int step_ms)
+{
     std::vector<uint8_t> raw_data;
 
     // 处理设备64角度
@@ -511,8 +587,20 @@ void BioHeadRobot::sendExpression(const std::vector<double>& device_64_angles, c
         raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节
     }
 
-    serial_->sendRawServoData(raw_data);
-    std::this_thread::sleep_for(std::chrono::seconds(5));
+    if (!sendRawIfCurrent(token, raw_data)) {
+        return false;
+    }
+    {
+        std::unique_lock lock(expression_wait_mutex_);
+        if (expression_wait_cv_.wait_for(
+                lock,
+                std::chrono::seconds(5),
+                [this, token] {
+                    return !operationalActivityCurrent_(token);
+                })) {
+            return false;
+        }
+    }
 
     // 恢复到原始角度
     // 设备64角度（10通道）
@@ -542,50 +630,78 @@ void BioHeadRobot::sendExpression(const std::vector<double>& device_64_angles, c
         raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节
     }
 
-    serial_->sendRawServoData(raw_data_neutral);
+    return sendRawIfCurrent(token, raw_data_neutral);
 }
 
 //高兴
 void BioHeadRobot::expressionHappy() {
+    (void)expressionHappyIfCurrent(beginOperationalActivity());
+}
+bool BioHeadRobot::expressionHappyIfCurrent(const OperationalToken token) {
     const std::vector<double> device_64_angles =  {90, 90, 90, 90, 80, 125, 100, 60, 90, 90};
     const std::vector<double> device_65_angles = {100, 80, 125, 135, 100, 105, 110, 90, 90, 90};
-    sendExpression(device_64_angles, device_65_angles, 0);
+    return sendExpression(token, device_64_angles, device_65_angles, 0);
 }
 //惊讶
 void BioHeadRobot::expressionSurprised() {
+    (void)expressionSurprisedIfCurrent(beginOperationalActivity());
+}
+bool BioHeadRobot::expressionSurprisedIfCurrent(const OperationalToken token) {
     const std::vector<double> device_64_angles = {90, 100, 100, 70, 20, 140, 130, 50, 90, 90};
     const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 70, 110};
-    sendExpression(device_64_angles, device_65_angles, 0);
+    return sendExpression(token, device_64_angles, device_65_angles, 0);
 }
 //睡觉
 void BioHeadRobot::expressionTired() {
+    (void)expressionTiredIfCurrent(beginOperationalActivity());
+}
+bool BioHeadRobot::expressionTiredIfCurrent(const OperationalToken token) {
     const std::vector<double> device_64_angles = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
     const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 105, 110, 90, 85, 95};
-    sendExpression(device_64_angles, device_65_angles, 0);
+    return sendExpression(token, device_64_angles, device_65_angles, 0);
 }
 
 //愤怒
 void BioHeadRobot::expressionAngry() {
+    (void)expressionAngryIfCurrent(beginOperationalActivity());
+}
+bool BioHeadRobot::expressionAngryIfCurrent(const OperationalToken token) {
     const std::vector<double> device_64_angles = {90, 70, 90, 110, 70, 125, 110, 80, 70, 90};
     const std::vector<double> device_65_angles = {100, 80, 130, 130, 70, 55, 50, 125, 90, 90};
-    sendExpression(device_64_angles, device_65_angles, 0);
+    return sendExpression(token, device_64_angles, device_65_angles, 0);
 }
 //悲伤
 void BioHeadRobot::expressionSadness() {
+    (void)expressionSadnessIfCurrent(beginOperationalActivity());
+}
+bool BioHeadRobot::expressionSadnessIfCurrent(const OperationalToken token) {
     const std::vector<double> device_64_angles = {90, 70, 90, 110, 70, 125, 110, 80, 90, 90};
     const std::vector<double> device_65_angles = {100, 80, 130, 130, 70, 55, 50, 125, 90, 90};
-    sendExpression(device_64_angles, device_65_angles, 0);
+    return sendExpression(token, device_64_angles, device_65_angles, 0);
 }
 //打哈欠
 void BioHeadRobot::expressionYawn() {
+    (void)expressionYawnIfCurrent(beginOperationalActivity());
+}
+bool BioHeadRobot::expressionYawnIfCurrent(const OperationalToken token) {
     const std::vector<double> device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90};
     const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90};
-    sendExpression(device_64_angles, device_65_angles, 0);
+    return sendExpression(token, device_64_angles, device_65_angles, 0);
 }
 
 
 
-void BioHeadRobot::sendServoCommands(const std::vector<double>& targets, uint16_t duration_ms) {
+bool BioHeadRobot::sendServoCommands(
+    const std::vector<double>& targets,
+    const uint16_t duration_ms,
+    const bool force)
+{
+    if (!serial_ || targets.size() != channels_.size() ||
+        targets.size() != min_angles_.size() ||
+        targets.size() != max_angles_.size() ||
+        targets.size() != last_joints_.size()) {
+        return false;
+    }
     std::vector<uint8_t> addrs, chs;
     std::vector<uint16_t> raws;
 
@@ -598,17 +714,14 @@ void BioHeadRobot::sendServoCommands(const std::vector<double>& targets, uint16_
             continue;
         }
 
-        // 更新 last_joints_，只有当角度变化较大时才更新
-        last_joints_[i] = tgt;
-
         // 准备打包数据
         addrs.push_back(channels_[i].addr);
         chs.push_back(channels_[i].channel);
         raws.push_back(angleToRaw(tgt));
     }
     // 2. 如果没有任何通道需要更新，就直接返回
-    if (raws.empty()) {
-        return;
+    if (raws.empty() && !force) {
+        return true;
     }
      std::vector<uint8_t> raw_data;
      // 原始格式处理
@@ -640,7 +753,41 @@ void BioHeadRobot::sendServoCommands(const std::vector<double>& targets, uint16_
 
 
 
-    serial_->sendRawServoData(raw_data);
+    const bool sent = serial_->sendRawServoData(raw_data);
+    if (sent) {
+        for (std::size_t i = 0; i < targets.size(); ++i) {
+            last_joints_[i] =
+                std::clamp(targets[i], min_angles_[i], max_angles_[i]);
+        }
+        std::lock_guard lock(stateMutex_);
+        current_joints_ = last_joints_;
+    }
+    return sent;
+}
+
+bool BioHeadRobot::sendRawIfCurrent(
+    const OperationalToken token,
+    const std::vector<uint8_t>& raw_data)
+{
+    bool sent = false;
+    const bool current = runIfOperationalActivityCurrent_(token, [&] {
+        sent = serial_ && serial_->sendRawServoData(raw_data);
+        if (!sent || raw_data.size() % 5U != 0U) {
+            return;
+        }
+        for (std::size_t offset = 0; offset < raw_data.size(); offset += 5U) {
+            for (std::size_t index = 0; index < channels_.size(); ++index) {
+                if (channels_[index].addr == raw_data[offset] &&
+                    channels_[index].channel == raw_data[offset + 1U]) {
+                    last_joints_[index] = raw_data[offset + 2U];
+                    break;
+                }
+            }
+        }
+        std::lock_guard lock(stateMutex_);
+        current_joints_ = last_joints_;
+    });
+    return current && sent;
 }
 
 
@@ -651,4 +798,3 @@ uint16_t BioHeadRobot::angleToRaw(double angle) {
 
 
 }  // namespace cmvr::device
-

cmvr-es/devices/canbus/CMakeLists.txt

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

cmvr-es/devices/canbus/abstract_canbus.h

@@ -3,6 +3,14 @@
 //
 
 #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"
@@ -14,20 +22,26 @@ namespace cmvr::device {
     */
     struct CanFrame {
         /// Message id
-        uint32_t id;
+        uint32_t id{0};
         /// Message length
-        uint8_t len;
-        /// Message content
-        uint8_t data[8];
-        /// Time stamp
-        struct timeval timestamp;
+        uint8_t len{0};
+        /// Message content. Classic CAN uses at most the first 8 bytes.
+        uint8_t data[64]{};
+        bool is_extended_id{false};
+        bool is_remote_frame{false};
+        bool is_error_frame{false};
+        bool is_fd{false};
+        bool bitrate_switch{false};
+        bool error_state_indicator{false};
+        /// Local host receive time used for freshness and watchdog checks.
+        int64_t rx_monotonic_ns{0};
+        /// Legacy wall-clock field retained for source compatibility.
+        struct timeval timestamp{0, 0};
 
         /**
          * @brief Constructor
          */
-        CanFrame() : id(0), len(0), timestamp{0} {
-            std::memset(data, 0, sizeof(data));
-        }
+        CanFrame() = default;
 
         /**
          * @brief CanFrame string including essential information about the message.
@@ -37,10 +51,15 @@ namespace cmvr::device {
             std::stringstream output_stream("");
             output_stream << "id:0x" << Byte::byte_to_hex(id)
                     << ",len:" << static_cast<int>(len) << ",data:";
-            for (uint8_t i = 0; i < len; ++i) {
+            const auto printable_len =
+                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 << ",";
+            output_stream << ",fd:" << is_fd
+                          << ",brs:" << bitrate_switch
+                          << ",extended:" << is_extended_id
+                          << ",error:" << is_error_frame << ",";
             return output_stream.str();
         }
     };
@@ -67,6 +86,28 @@ namespace cmvr::device {
         virtual cmvr::msgs::ErrorCode send(const std::vector<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.
@@ -75,7 +116,9 @@ namespace cmvr::device {
         virtual cmvr::msgs::ErrorCode sendSingleFrame(
             const std::vector<CanFrame> &frames) {
             if (frames.size() != 1U) {
-                CMVR_LOG(FATAL) << "frames size not equal to 1, actual frame size: " << frames.size();
+                CMVR_LOG(ERROR) << "frames size not equal to 1, actual frame size: "
+                                << frames.size();
+                return cmvr::msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
             }
             int32_t n = 1;
             return send(frames, &n);
@@ -91,6 +134,17 @@ namespace cmvr::device {
         virtual cmvr::msgs::ErrorCode receive(std::vector<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,9 +12,13 @@
 #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;
@@ -24,8 +28,25 @@ namespace cmvr {
             auto channel_id = cfg.channel_id();
             port_ = static_cast<CANCardParameter::CANChannelId>(channel_id);
             interface_ = CANCardParameter::NATIVE;
-
-            enable_can_err_check_ = false;
+            interface_name_ =
+                cfg.has_interface_name() && !cfg.interface_name().empty()
+                    ? cfg.interface_name()
+                    : cfg.dev_id();
+            enable_fd_ = cfg.has_enable_fd() && cfg.enable_fd();
+            default_bitrate_switch_ =
+                cfg.has_bitrate_switch() && cfg.bitrate_switch();
+            receive_own_messages_ =
+                cfg.has_receive_own_messages() && cfg.receive_own_messages();
+            receive_timeout_us_ =
+                cfg.has_receive_timeout_us() && cfg.receive_timeout_us() > 0
+                    ? cfg.receive_timeout_us()
+                    : 100000U;
+            send_timeout_us_ =
+                cfg.has_send_timeout_us() && cfg.send_timeout_us() > 0
+                    ? cfg.send_timeout_us()
+                    : 100000U;
+            enable_can_err_check_ =
+                cfg.has_enable_error_frames() && cfg.enable_error_frames();
         }
 
 
@@ -49,7 +70,7 @@ namespace cmvr {
         }
 
         SocketCanClientRaw::~SocketCanClientRaw() {
-            if (dev_handler_) {
+            if (dev_handler_ >= 0) {
                 stop();
             }
         }
@@ -59,8 +80,8 @@ namespace cmvr {
                 status_ = ErrorCode::OK;
                 return true;
             }
-            struct sockaddr_can addr;
-            struct ifreq ifr;
+            struct sockaddr_can addr {};
+            struct ifreq ifr {};
 
             // open device
             // guss net is the device minor number, if one card is 0,1
@@ -91,17 +112,71 @@ namespace cmvr {
                 if (ret < 0) {
                     CMVR_LOG(ERROR) << "add receive msg id filter error code: " << ret;
                     status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
+                    stop();
                     return false;
                 }
             }
 
-            // 2. enable reception of can frames.
+            // 2. Explicitly opt into CAN-FD only when configured. This socket
+            // option does not configure the physical link bitrate or state.
+            if (enable_fd_) {
                 int enable = 1;
-            ret = ::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable,
-                               sizeof(enable));
+                ret = ::setsockopt(dev_handler_, SOL_CAN_RAW,
+                                   CAN_RAW_FD_FRAMES, &enable, sizeof(enable));
                 if (ret < 0) {
-                CMVR_LOG(ERROR) << "enable reception of can frame error code: " << ret;
+                    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;
             }
 
@@ -115,13 +190,39 @@ namespace cmvr {
                 interface_prefix = "can";
             }
 
-            const std::string can_name = absl::StrCat(interface_prefix, port_);
-            std::strncpy(ifr.ifr_name, can_name.c_str(), IFNAMSIZ);
-            if (ioctl(dev_handler_, SIOCGIFINDEX, &ifr) < 0) {
-                CMVR_LOG(ERROR) << "ioctl error";
+            const std::string can_name =
+                interface_name_.empty()
+                    ? absl::StrCat(interface_prefix, port_)
+                    : interface_name_;
+            if (can_name.size() >= IFNAMSIZ) {
+                CMVR_LOG(ERROR) << "CAN interface name is too long: " << can_name;
                 status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
+                stop();
                 return false;
             }
+            std::strncpy(ifr.ifr_name, can_name.c_str(), IFNAMSIZ);
+            ifr.ifr_name[IFNAMSIZ - 1] = '\0';
+            if (ioctl(dev_handler_, SIOCGIFINDEX, &ifr) < 0) {
+                CMVR_LOG(ERROR) << "CAN interface not found: " << can_name
+                                << ", error=" << std::strerror(errno);
+                status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
+                stop();
+                return false;
+            }
+
+            if (enable_fd_) {
+                struct ifreq mtu_request {};
+                std::strncpy(mtu_request.ifr_name, can_name.c_str(), IFNAMSIZ);
+                mtu_request.ifr_name[IFNAMSIZ - 1] = '\0';
+                if (::ioctl(dev_handler_, SIOCGIFMTU, &mtu_request) < 0 ||
+                    mtu_request.ifr_mtu != CANFD_MTU) {
+                    CMVR_LOG(ERROR) << "CAN-FD requested but interface MTU is not CANFD_MTU: "
+                                    << can_name;
+                    status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
+                    stop();
+                    return false;
+                }
+            }
 
             // bind socket to network interface
 
@@ -131,8 +232,10 @@ namespace cmvr {
                          sizeof(addr));
 
             if (ret < 0) {
-                CMVR_LOG(ERROR) << "bind socket to network interface error code: " << ret;
+                CMVR_LOG(ERROR) << "bind socket to CAN interface failed: "
+                                << std::strerror(errno);
                 status_ = ErrorCode::CAN_CLIENT_ERROR_BASE;
+                stop();
                 return false;
             }
 
@@ -142,10 +245,11 @@ namespace cmvr {
         }
 
         bool SocketCanClientRaw::stop() {
-            if (is_started_) {
             is_started_ = false;
-
-                int ret = close(dev_handler_);
+            if (dev_handler_ >= 0) {
+                const int fd = dev_handler_;
+                dev_handler_ = -1;
+                int ret = close(fd);
                 if (ret < 0) {
                     CMVR_LOG(ERROR) << "close error code:" << ret << ", " << getErrorString(ret);
                     return false;
@@ -159,48 +263,190 @@ namespace cmvr {
         // Synchronous transmission of CAN messages
         ErrorCode SocketCanClientRaw::send(const std::vector<CanFrame> &frames,
                                            int32_t *const frame_num) {
-            if (frame_num == nullptr) {
-                CMVR_LOG(FATAL) << "frame_num is null";
+            return sendWithDeadline_(
+                frames, frame_num,
+                std::chrono::steady_clock::now() +
+                    std::chrono::microseconds(send_timeout_us_));
         }
-            if (frames.size() != static_cast<size_t>(*frame_num)) {
-                CMVR_LOG(FATAL) << "frames size does not match frame_num";
+
+        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";
+                return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
+            }
+            if (*frame_num < 0 ||
+                frames.size() != static_cast<size_t>(*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;
             }
-            for (size_t i = 0; i < frames.size() && i < MAX_CAN_SEND_FRAME_LEN; ++i) {
-                if (frames[i].len > CANBUS_MESSAGE_LENGTH || frames[i].len < 0) {
-                    CMVR_LOG(ERROR) << "frames[" << i << "].len = " << frames[i].len
-                            << ", which is not equal to can message data length ("
-                            << CANBUS_MESSAGE_LENGTH << ").";
+            if (std::chrono::steady_clock::now() >= send_deadline) {
+                *frame_num = 0;
                 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;
+
+            // 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;
+                }
+            }
+
+            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 {
-                    send_frames_[i].can_id = (frames[i].id & CAN_SFF_MASK);
+                    classic_frame.can_id = can_id;
+                    classic_frame.can_dlc = source.len;
+                    std::memcpy(
+                        classic_frame.data, source.data, source.len);
+                    expected = CAN_MTU;
+                    payload = &classic_frame;
                 }
-                // 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);
 
-                // Synchronous transmission of CAN messages
-                int ret = static_cast<int>(
-                    write(dev_handler_, &send_frames_[i], sizeof(send_frames_[i])));
-                if (ret <= 0) {
-                    CMVR_LOG(ERROR) << "can " << port_ << " send message failed, error code: " << ret;
-                    return ErrorCode::CAN_CLIENT_ERROR_BASE;
+                while (true) {
+                    const auto written = ::send(
+                        dev_handler_, payload, expected,
+                        MSG_DONTWAIT | MSG_NOSIGNAL);
+                    if (written == static_cast<ssize_t>(expected)) {
+                        ++sent_count;
+                        break;
+                    }
+                    if (written >= 0) {
+                        *frame_num = sent_count;
+                        CMVR_LOG(ERROR)
+                            << "can " << port_
+                            << " sent a partial frame";
+                        return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
+                    }
+                    if (errno == EINTR) {
+                        continue;
+                    }
+                    if (errno != EAGAIN && errno != EWOULDBLOCK) {
+                        *frame_num = sent_count;
+                        CMVR_LOG(ERROR) << "can " << port_
+                                        << " send message failed: "
+                                        << std::strerror(errno);
+                        return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
+                    }
+
+                    const auto now = std::chrono::steady_clock::now();
+                    if (now >= send_deadline) {
+                        *frame_num = sent_count;
+                        CMVR_LOG(ERROR)
+                            << "can " << port_
+                            << " send batch timed out";
+                        return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED;
+                    }
+                    const auto remaining =
+                        std::chrono::duration_cast<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;
@@ -213,39 +459,109 @@ namespace cmvr {
                 return ErrorCode::CAN_CLIENT_ERROR_FRAME_NUM;
             }
 
-            for (int32_t i = 0; i < *frame_num && i < MAX_CAN_RECV_FRAME_LEN; ++i) {
+            frames->clear();
+            const int32_t requested = *frame_num;
+            *frame_num = 0;
+            for (int32_t i = 0; i < requested && i < MAX_CAN_RECV_FRAME_LEN; ++i) {
                 CanFrame cf;
-                auto ret = read(dev_handler_, &recv_frames_[i], sizeof(recv_frames_[i]));
+                struct canfd_frame raw {};
+                const auto ret = ::read(dev_handler_, &raw, CANFD_MTU);
 
                 if (ret < 0) {
-                    CMVR_LOG(ERROR) << "receive message failed, error code: " << ret;
-                    return ErrorCode::CAN_CLIENT_ERROR_BASE;
-                }
-                if (recv_frames_[i].can_dlc > CANBUS_MESSAGE_LENGTH ||
-                    recv_frames_[i].can_dlc < 0) {
-                    CMVR_LOG(ERROR) << "recv_frames_[" << i
-                            << "].can_dlc = " << recv_frames_[i].can_dlc
-                            << ", which is not equal to can message data length ("
-                            << CANBUS_MESSAGE_LENGTH << ").";
+                    if (errno == EAGAIN || errno == EWOULDBLOCK ||
+                        errno == EINTR) {
                         return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
                     }
-                if (recv_frames_[i].can_id > CAN_STANDARD_MAX_ID) {
-                    cf.id = enable_can_err_check_
-                                ? recv_frames_[i].can_id & CAN_EFF_MASK | CAN_ERR_FLAG
-                                : recv_frames_[i].can_id & CAN_EFF_MASK;
-                } else {
-                    cf.id = (recv_frames_[i].can_id & CAN_SFF_MASK);
+                    CMVR_LOG(ERROR) << "receive CAN message failed: "
+                                    << std::strerror(errno);
+                    return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
                 }
-                // CMVR_LOG(INFO) << "Socket can receive can id is " << recv_frames_[i].can_id;
-                cf.len = recv_frames_[i].can_dlc;
-                std::memcpy(cf.data, recv_frames_[i].data, recv_frames_[i].can_dlc);
+                if (ret != CAN_MTU && ret != CANFD_MTU) {
+                    CMVR_LOG(ERROR) << "unexpected SocketCAN MTU: " << ret;
+                    return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED;
+                }
+
+                const canid_t raw_id = raw.can_id;
+                cf.is_extended_id = (raw_id & CAN_EFF_FLAG) != 0;
+                cf.is_remote_frame = (raw_id & CAN_RTR_FLAG) != 0;
+                cf.is_error_frame = (raw_id & CAN_ERR_FLAG) != 0;
+                if (cf.is_error_frame) {
+                    cf.id = raw_id & CAN_ERR_MASK;
+                } else if (cf.is_extended_id) {
+                    cf.id = raw_id & CAN_EFF_MASK;
+                } else {
+                    cf.id = raw_id & CAN_SFF_MASK;
+                }
+
+                cf.is_fd = ret == CANFD_MTU;
+                if (cf.is_fd) {
+                    cf.len = raw.len;
+                    cf.bitrate_switch = (raw.flags & CANFD_BRS) != 0;
+                    cf.error_state_indicator = (raw.flags & CANFD_ESI) != 0;
+                } else {
+                    const auto* classic =
+                        reinterpret_cast<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;
         }
 
-        std::string SocketCanClientRaw::getErrorString(const int32_t /*status*/) {
-            return "";
+        bool SocketCanClientRaw::discardPendingFrames() {
+            if (!is_started_ || dev_handler_ < 0) {
+                return false;
+            }
+
+            constexpr std::size_t kMaximumDrainFrames = 4096;
+            const auto deadline =
+                std::chrono::steady_clock::now() +
+                std::chrono::microseconds(send_timeout_us_);
+            std::size_t count = 0;
+            while (count < kMaximumDrainFrames &&
+                   std::chrono::steady_clock::now() < deadline) {
+                struct canfd_frame raw {};
+                const auto received = ::recv(
+                    dev_handler_, &raw, CANFD_MTU, MSG_DONTWAIT);
+                if (received == CAN_MTU || received == CANFD_MTU) {
+                    ++count;
+                    continue;
+                }
+                if (received < 0 &&
+                    (errno == EAGAIN || errno == EWOULDBLOCK)) {
+                    return true;
+                }
+                if (received < 0 && errno == EINTR) {
+                    continue;
+                }
+                CMVR_LOG(ERROR)
+                    << "failed while draining pending CAN frames: "
+                    << (received < 0 ? std::strerror(errno)
+                                     : "unexpected MTU");
+                return false;
+            }
+            CMVR_LOG(ERROR)
+                << "CAN receive queue did not drain within its bound";
+            return false;
+        }
+
+        std::string SocketCanClientRaw::getErrorString(const int32_t status) {
+            return std::strerror(status < 0 ? -status : status);
         }
     }
 }

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

@@ -12,11 +12,13 @@
 #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>
 
@@ -51,6 +53,10 @@ 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
@@ -60,6 +66,7 @@ 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.
@@ -67,14 +74,23 @@ namespace cmvr {
              */
             std::string getErrorString(const int32_t status) override;
         private:
-            int dev_handler_ = 0;
+            int dev_handler_{-1};
             cmvr::msgs::CANCardParameter::CANChannelId port_;
             cmvr::msgs::CANCardParameter::CANInterface interface_;
-            can_frame send_frames_[MAX_CAN_SEND_FRAME_LEN];
-            can_frame recv_frames_[MAX_CAN_RECV_FRAME_LEN];
+            std::string interface_name_;
+            bool enable_fd_{false};
+            bool default_bitrate_switch_{false};
+            bool receive_own_messages_{false};
+            uint32_t receive_timeout_us_{100000};
+            uint32_t send_timeout_us_{100000};
 
             //
             bool enable_can_err_check_{false};
+
+            cmvr::msgs::ErrorCode sendWithDeadline_(
+                const std::vector<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,44 +1,226 @@
-#include "common/base/logging/logger.h"
-//
-// Created by lgv on 2025/7/16.
-//
-#include "cmvr/msgs/error_code.pb.h"
-#include "cmvr/msgs/can_card_parameter.pb.h"
 #include "canbus/can_client/socket/socket_can_client_raw.h"
-#include "gtest/gtest.h"
-namespace cmvr {
-namespace device {
-    using cmvr::msgs::ErrorCode;
-    using cmvr::msgs::CANCardParameter;
 
-    TEST(SocketCanClientRawTest, simple_test) {
-        CANCardParameter param;
-        param.set_brand(CANCardParameter::SOCKET_CAN_RAW);
-        param.set_channel_id(CANCardParameter::CHANNEL_ID_ZERO);
+#include <algorithm>
+#include <chrono>
+#include <filesystem>
+#include <iterator>
+#include <string>
+#include <thread>
+#include <vector>
 
-        cmvr::config::SocketCanConfig cfg;
-        cfg.set_channel_id(0);
-        SocketCanClientRaw socket_can_client(cfg);
+#include <gtest/gtest.h>
 
-        // EXPECT_EQ(socket_can_client.start(), ErrorCode::CAN_CLIENT_ERROR_BASE);
-        socket_can_client.start();
-        std::vector<CanFrame> frames;
-        int32_t num = 0;
-        EXPECT_EQ(socket_can_client.send(frames, &num),
-                  ErrorCode::OK);
-        ++num;
-        EXPECT_EQ(socket_can_client.receive(&frames, &num),
-                  ErrorCode::OK);
-        CMVR_LOG(INFO) << frames.at(0).CanFrameString();
-        CanFrame can_frame;
-        can_frame.id = 0x123;
-        can_frame.len = 8;
-        memset(can_frame.data, 0xA3, sizeof(can_frame.data));
-        frames.clear();
-        frames.push_back(can_frame);
-        EXPECT_EQ(socket_can_client.sendSingleFrame(frames),
-                  ErrorCode::OK);
-        socket_can_client.stop();
+namespace cmvr::device {
+namespace {
+
+std::size_t openFileDescriptorCount()
+{
+    std::error_code error;
+    std::size_t count = 0;
+    for (std::filesystem::directory_iterator iterator(
+             "/proc/self/fd", error);
+         !error && iterator != std::filesystem::directory_iterator();
+         iterator.increment(error)) {
+        ++count;
+    }
+    return error ? 0U : count;
+}
+
+config::SocketCanConfig vcanConfig(const bool enable_fd)
+{
+    config::SocketCanConfig config;
+    config.set_interface_name("vcan0");
+    config.set_enable_fd(enable_fd);
+    config.set_bitrate_switch(enable_fd);
+    config.set_receive_own_messages(false);
+    config.set_receive_timeout_us(2000U);
+    config.set_send_timeout_us(2000U);
+    return config;
+}
+
+bool vcanAvailable()
+{
+    return ::if_nametoindex("vcan0") != 0U;
+}
+
+TEST(SocketCanClientRawTest, MissingClassicInterfaceFailsWithoutLeakingFd)
+{
+    config::SocketCanConfig config;
+    config.set_interface_name("cmvr_no_such_can");
+    config.set_enable_fd(false);
+    config.set_receive_timeout_us(100U);
+    config.set_send_timeout_us(100U);
+    SocketCanClientRaw client(config);
+
+    const auto before = openFileDescriptorCount();
+    ASSERT_GT(before, 0U);
+    for (int attempt = 0; attempt < 32; ++attempt) {
+        EXPECT_FALSE(client.start());
+        EXPECT_TRUE(client.stop());
+    }
+    const auto after = openFileDescriptorCount();
+    EXPECT_LE(after, before + 1U);
+}
+
+TEST(SocketCanClientRawTest, ClosedClientRejectsClassicSendAndReceive)
+{
+    config::SocketCanConfig config;
+    config.set_interface_name("cmvr_no_such_can");
+    config.set_enable_fd(false);
+    SocketCanClientRaw client(config);
+
+    CanFrame frame;
+    frame.id = 0x123U;
+    frame.len = 8U;
+    frame.is_fd = false;
+    std::fill(std::begin(frame.data), std::end(frame.data), 0xA3U);
+    std::vector<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,9 +26,14 @@
 namespace cmvr {
     namespace device {
         const int32_t CAN_FRAME_SIZE = 8;
-        const int32_t MAX_CAN_SEND_FRAME_LEN = 1;
+        const int32_t CAN_FD_FRAME_SIZE = 64;
+        // One UME cycle may submit a complete arm worth of frames. The receive
+        // API intentionally remains one-frame-at-a-time so a caller never
+        // blocks waiting to fill an artificial batch.
+        const int32_t MAX_CAN_SEND_FRAME_LEN = 64;
         const int32_t MAX_CAN_RECV_FRAME_LEN = 1;     // 这个暂时改为 1 ,大量数据的时候改为 10
 
         const int32_t CANBUS_MESSAGE_LENGTH = 8; // according to ISO-11891-1
+        const int32_t CANFD_MESSAGE_LENGTH = 64;
     }
 }

cmvr-es/devices/dexhand/abstract_dexhand.h

@@ -156,6 +156,17 @@ namespace cmvr::device {
                 lifecycle == Status::STREAMING;
         }
 
+        // Stops command-driven activity without changing the device lifecycle
+        // or closing its transport. Implementations must return true only after
+        // no pre-stop activity can continue. Motion-capable hands without a
+        // reliable hold/idle command deliberately fail closed.
+        virtual bool stopOperationalActivity() { return false; }
+
+        // Restores an activity paused by stopOperationalActivity(). This is
+        // called only after a new command has crossed the system admission
+        // boundary. Most motion-capable hands need no separate resume command.
+        virtual bool resumeOperationalActivity() { return true; }
+
         virtual void setAngles(const std::vector<int>& finger_joint_angles) = 0;
         virtual void setTactilePollingRegion(FingerType finger, TactileRegion region) {
             setTactilePollingRegions({TactileRegionKey{finger, region}});

cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h

@@ -49,6 +49,8 @@ namespace cmvr::device {
         Status state() const override;
         std::string lastError() const override;
         void getState(DexHandState& state) override;
+        bool stopOperationalActivity() override;
+        bool resumeOperationalActivity() override;
 
         void setAngles(const std::vector<int>& finger_joint_angles) override;
         void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) override;
@@ -122,6 +124,7 @@ namespace cmvr::device {
         mutable std::mutex polling_mutex_;
         std::condition_variable polling_cv_;
         bool requested_polling_{true};
+        bool polling_paused_for_stop_all_{false};
         std::thread polling_thread_;
         std::atomic<bool> polling_thread_running_{false};
         std::chrono::milliseconds poll_interval_{10};

cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp

@@ -450,6 +450,28 @@ void PX6AXGen3::getState(DexHandState& state_out) {
     state_out = std::move(next_state);
 }
 
+bool PX6AXGen3::stopOperationalActivity() {
+    {
+        std::lock_guard<std::mutex> lock(polling_mutex_);
+        polling_paused_for_stop_all_ = true;
+    }
+    polling_cv_.notify_all();
+
+    // The refresh mutex is the bounded device-I/O dispatch boundary. Once it is
+    // acquired, a pre-stop sensor transaction cannot still be using the wire.
+    std::lock_guard<std::mutex> refresh_lock(refresh_mutex_);
+    return true;
+}
+
+bool PX6AXGen3::resumeOperationalActivity() {
+    {
+        std::lock_guard<std::mutex> lock(polling_mutex_);
+        polling_paused_for_stop_all_ = false;
+    }
+    polling_cv_.notify_all();
+    return true;
+}
+
 void PX6AXGen3::setAngles(const std::vector<int>&) {
     CMVR_LOG(ERROR) << "PX6AXGen3 is a tactile sensor only and does not support setAngles.";
 }
@@ -589,6 +611,12 @@ void PX6AXGen3::refreshSensorData(const bool read_distributed, const bool read_r
     }
 
     std::lock_guard<std::mutex> refresh_lock(refresh_mutex_);
+    {
+        std::lock_guard<std::mutex> polling_lock(polling_mutex_);
+        if (polling_paused_for_stop_all_) {
+            return;
+        }
+    }
     const bool had_valid_snapshot = isSnapshotReady(read_distributed, read_resultant);
 
     try {
@@ -722,9 +750,10 @@ void PX6AXGen3::pollingLoop() {
     auto next_poll_deadline = std::chrono::steady_clock::now();
     std::unique_lock<std::mutex> lock(polling_mutex_);
     while (polling_thread_running_.load(std::memory_order_acquire)) {
-        if (!requested_polling_) {
+        if (!requested_polling_ || polling_paused_for_stop_all_) {
             polling_cv_.wait(lock, [this]() {
-                return !polling_thread_running_.load(std::memory_order_acquire) || requested_polling_;
+                return !polling_thread_running_.load(std::memory_order_acquire) ||
+                    (requested_polling_ && !polling_paused_for_stop_all_);
             });
             next_poll_deadline = std::chrono::steady_clock::now();
             continue;
@@ -746,7 +775,8 @@ void PX6AXGen3::pollingLoop() {
         }
 
         polling_cv_.wait_until(lock, next_poll_deadline, [this]() {
-            return !polling_thread_running_.load(std::memory_order_acquire);
+            return !polling_thread_running_.load(std::memory_order_acquire) ||
+                polling_paused_for_stop_all_;
         });
     }
 }
@@ -758,6 +788,15 @@ void PX6AXGen3::ensureSensorReady(const bool allow_background,
     const bool background_covers_request =
         (!require_tactile || polls_tactile) &&
         (!require_resultant || polls_resultant);
+    bool polling_paused = false;
+    {
+        std::lock_guard<std::mutex> lock(polling_mutex_);
+        polling_paused = polling_paused_for_stop_all_;
+    }
+    if (polling_paused) {
+        return;
+    }
+
     const bool background_ready = allow_background &&
         background_covers_request &&
         polling_thread_running_.load(std::memory_order_acquire) &&

cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h

@@ -28,14 +28,17 @@ namespace cmvr::device {
     class ModbusController {
     public:
         ModbusController() = default;
-        ~ModbusController();
+        virtual ~ModbusController();
 
-        bool open(const std::string& ip, int port);
-        void close();
-        bool isOpen() const;
+        virtual bool open(const std::string& ip, int port);
+        virtual void close();
+        virtual bool isOpen() const;
 
-        bool writeRegisters(int address, const uint16_t* values, int count);
-        bool readRegisterBlock(int start_address, int count, std::vector<uint16_t>& values);
+        virtual bool writeRegisters(int address, const uint16_t* values, int count);
+        virtual bool readRegisterBlock(
+            int start_address,
+            int count,
+            std::vector<uint16_t>& values);
 
     private:
         void closeUnlocked();
@@ -58,6 +61,9 @@ namespace cmvr::device {
         using RegionMask = std::bitset<TACTILE_REGION_SLOT_COUNT>;
 
         explicit RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& cfg);
+        RH56DFTPDexhand(
+            const config::RH56DFTPDexHandConfig& cfg,
+            std::unique_ptr<ModbusController> controller);
         ~RH56DFTPDexhand() override;
 
         std::string typeName() const override { return "RH56DFTPDexhand"; }
@@ -68,6 +74,8 @@ namespace cmvr::device {
         Status state() const override;
         std::string lastError() const override;
         void getState(DexHandState& state) override;
+        bool stopOperationalActivity() override;
+        bool resumeOperationalActivity() override;
 
         void setAngles(const std::vector<int>& finger_joint_angles) override;
         void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) override;
@@ -110,6 +118,18 @@ namespace cmvr::device {
 
         mutable std::mutex command_mutex_;
         std::array<int, ANGLE_COMMAND_COUNT> last_commanded_angles_{};
+        // Kept separately from last_commanded_angles_: a failed Modbus block
+        // write may still have changed a prefix of the device registers. Such
+        // an attempt must remain visible to StopAll without being reported as
+        // a successfully accepted command.
+        std::array<int, ANGLE_COMMAND_COUNT> pending_angle_target_{};
+        bool angle_target_unconfirmed_{false};
+
+        // Normal command and tactile I/O take this gate in shared mode.
+        // StopAll first closes admission and then takes it exclusively, which
+        // drains every operation that crossed the boundary before the stop.
+        mutable std::shared_mutex operational_gate_;
+        std::atomic<bool> operational_paused_{false};
 
         std::array<RH56TactileBuffer, 2> tactile_buffers_;
         std::array<RegionMask, 2> tactile_buffer_masks_{};

cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp

@@ -21,6 +21,11 @@ namespace {
     using Status = DexHand::Status;
 
     constexpr int kAngleSetByteAddress = 1486;
+    constexpr int kAngleActualByteAddress = 1546;
+    constexpr int kAngleStoppedTolerance = 5;
+    constexpr int kAngleStableTolerance = 1;
+    constexpr int kAngleStopConfirmationSamples = 3;
+    constexpr auto kAngleStopSampleInterval = std::chrono::milliseconds(10);
     constexpr int kDefaultPort = 6000;
     constexpr int kMaxRegistersPerRead = 125;
 
@@ -325,8 +330,16 @@ void ModbusController::closeUnlocked() {
 }
 
 RH56DFTPDexhand::RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& cfg)
-    : controller_(std::make_unique<ModbusController>()),
-      dexhandCfg_(cfg) {
+    : RH56DFTPDexhand(cfg, std::make_unique<ModbusController>()) {
+}
+
+RH56DFTPDexhand::RH56DFTPDexhand(
+        const config::RH56DFTPDexHandConfig& cfg,
+        std::unique_ptr<ModbusController> controller)
+    : controller_(std::move(controller)), dexhandCfg_(cfg) {
+    if (!controller_) {
+        throw std::invalid_argument("RH56 Modbus controller is required");
+    }
     id_ = dexhandCfg_.id();
     ip_address_ = dexhandCfg_.ip();
     if (dexhandCfg_.port() > 0) {
@@ -352,6 +365,9 @@ bool RH56DFTPDexhand::init() {
 }
 
 bool RH56DFTPDexhand::start() {
+    if (!resumeOperationalActivity()) {
+        return false;
+    }
     if (tactile_thread_running_.exchange(true, std::memory_order_acq_rel)) {
         transitionTo(Status::STREAMING);
         return true;
@@ -387,6 +403,7 @@ bool RH56DFTPDexhand::start() {
 }
 
 bool RH56DFTPDexhand::stop() {
+    operational_paused_.store(true, std::memory_order_release);
     tactile_thread_running_.store(false, std::memory_order_release);
     polling_cv_.notify_all();
 
@@ -394,9 +411,15 @@ bool RH56DFTPDexhand::stop() {
         tactile_thread_.join();
     }
 
+    {
+        // Drain command and tactile dispatches before closing their transport.
+        std::unique_lock<std::shared_mutex> operational_lock(
+            operational_gate_);
+        operational_paused_.store(true, std::memory_order_release);
         if (controller_) {
             controller_->close();
         }
+    }
 
     if (state() != Status::FAULT) {
         transitionTo(Status::STOPPED);
@@ -433,13 +456,124 @@ void RH56DFTPDexhand::getState(DexHandState& state_out) {
     state_out = std::move(next_state);
 }
 
+bool RH56DFTPDexhand::stopOperationalActivity() {
+    operational_paused_.store(true, std::memory_order_release);
+    polling_cv_.notify_all();
+
+    // Taking the gate exclusively confirms that every command write and
+    // tactile read admitted before StopAll has left the Modbus boundary.
+    std::unique_lock<std::shared_mutex> operational_lock(operational_gate_);
+    operational_paused_.store(true, std::memory_order_release);
+
+    std::array<int, ANGLE_COMMAND_COUNT> target{};
+    {
+        std::lock_guard<std::mutex> command_lock(command_mutex_);
+        if (!angle_target_unconfirmed_) {
+            return true;
+        }
+        target = pending_angle_target_;
+    }
+
+    // RH56 exposes no hold/quick-stop command. The actual-angle registers are
+    // therefore the only physical confirmation available. Require several
+    // samples both at the requested target and stable over time; a single
+    // sample can coincide with a joint crossing the target while still moving.
+    // Otherwise StopAll stays fail-closed and a later round can retry.
+    if (!controller_ || !controller_->isOpen()) {
+        CMVR_LOG(ERROR)
+            << "[RH56DFTPDexhand] cannot confirm the last angle target: "
+               "Modbus is not connected";
+        return false;
+    }
+    std::vector<uint16_t> previous_actual;
+    for (int sample = 0; sample < kAngleStopConfirmationSamples; ++sample) {
+        if (sample != 0) {
+            std::this_thread::sleep_for(kAngleStopSampleInterval);
+        }
+
+        std::vector<uint16_t> actual;
+        if (!controller_->readRegisterBlock(
+                kAngleActualByteAddress,
+                static_cast<int>(ANGLE_COMMAND_COUNT),
+                actual) ||
+            actual.size() != ANGLE_COMMAND_COUNT) {
+            CMVR_LOG(ERROR)
+                << "[RH56DFTPDexhand] failed to read actual joint angles "
+                   "while confirming operational stop";
+            return false;
+        }
+        for (std::size_t index = 0; index < target.size(); ++index) {
+            if (std::abs(static_cast<int>(actual[index]) - target[index]) >
+                kAngleStoppedTolerance) {
+                CMVR_LOG(WARNING)
+                    << "[RH56DFTPDexhand] joint " << index
+                    << " has not reached its pending target; target="
+                    << target[index] << ", actual=" << actual[index];
+                return false;
+            }
+            if (!previous_actual.empty() &&
+                std::abs(static_cast<int>(actual[index]) -
+                         static_cast<int>(previous_actual[index])) >
+                    kAngleStableTolerance) {
+                CMVR_LOG(WARNING)
+                    << "[RH56DFTPDexhand] joint " << index
+                    << " is not stable while confirming operational stop; "
+                       "previous="
+                    << previous_actual[index] << ", actual=" << actual[index];
+                return false;
+            }
+        }
+        previous_actual = std::move(actual);
+    }
+
+    {
+        std::lock_guard<std::mutex> command_lock(command_mutex_);
+        angle_target_unconfirmed_ = false;
+    }
+    return true;
+}
+
+bool RH56DFTPDexhand::resumeOperationalActivity() {
+    std::unique_lock<std::shared_mutex> operational_lock(operational_gate_);
+    operational_paused_.store(false, std::memory_order_release);
+    operational_lock.unlock();
+    polling_cv_.notify_all();
+    return true;
+}
+
 void RH56DFTPDexhand::setAngles(const std::vector<int>& finger_joint_angles) {
+    if (finger_joint_angles.size() != ANGLE_COMMAND_COUNT) {
+        CMVR_LOG(ERROR) << "RH56DFTPDexhand expects exactly 6 joint angles.";
+        return;
+    }
+    if (operational_paused_.load(std::memory_order_acquire)) {
+        CMVR_LOG(WARNING)
+            << "[RH56DFTPDexhand] angle command rejected while operational "
+               "activity is paused";
+        return;
+    }
+    std::shared_lock<std::shared_mutex> operational_lock(operational_gate_);
+    if (operational_paused_.load(std::memory_order_acquire)) {
+        return;
+    }
     const auto registers = encodeAngleCommand(finger_joint_angles);
 
     try {
         if (!ensureConnected()) {
             return;
         }
+
+        // Mark the write attempt before crossing the Modbus boundary. A false
+        // return can represent a partial register write, so only StopAll's
+        // physical confirmation may clear this state.
+        {
+            std::lock_guard<std::mutex> lock(command_mutex_);
+            std::copy(
+                finger_joint_angles.begin(),
+                finger_joint_angles.end(),
+                pending_angle_target_.begin());
+            angle_target_unconfirmed_ = true;
+        }
         if (!controller_->writeRegisters(
                 kAngleSetByteAddress,
                 registers.data(),
@@ -538,6 +672,14 @@ void RH56DFTPDexhand::refreshTactileData(const RegionMask& mask) {
     if (mask.none()) {
         return;
     }
+    if (operational_paused_.load(std::memory_order_acquire)) {
+        return;
+    }
+
+    std::shared_lock<std::shared_mutex> operational_lock(operational_gate_);
+    if (operational_paused_.load(std::memory_order_acquire)) {
+        return;
+    }
 
     try {
         if (!ensureConnected()) {
@@ -586,9 +728,12 @@ void RH56DFTPDexhand::tactilePollingLoop() {
     auto next_poll_deadline = std::chrono::steady_clock::now();
     std::unique_lock<std::mutex> lock(polling_mutex_);
     while (tactile_thread_running_.load(std::memory_order_acquire)) {
-        if (requested_polling_mask_.none()) {
+        if (requested_polling_mask_.none() ||
+            operational_paused_.load(std::memory_order_acquire)) {
             polling_cv_.wait(lock, [this]() {
-                return !tactile_thread_running_.load(std::memory_order_acquire) || requested_polling_mask_.any();
+                return !tactile_thread_running_.load(std::memory_order_acquire) ||
+                    (!operational_paused_.load(std::memory_order_acquire) &&
+                     requested_polling_mask_.any());
             });
             next_poll_deadline = std::chrono::steady_clock::now();
             continue;
@@ -611,7 +756,9 @@ void RH56DFTPDexhand::tactilePollingLoop() {
         }
 
         polling_cv_.wait_until(lock, next_poll_deadline, [this, mask]() {
-            return !tactile_thread_running_.load(std::memory_order_acquire) || requested_polling_mask_ != mask;
+            return !tactile_thread_running_.load(std::memory_order_acquire) ||
+                operational_paused_.load(std::memory_order_acquire) ||
+                requested_polling_mask_ != mask;
         });
     }
 }
@@ -667,6 +814,9 @@ void RH56DFTPDexhand::ensureTactileMaskReady(const RegionMask& mask, const bool
     if (mask.none()) {
         return;
     }
+    if (operational_paused_.load(std::memory_order_acquire)) {
+        return;
+    }
 
     const bool background_ready = allow_background &&
         tactile_thread_running_.load(std::memory_order_acquire) &&

cmvr-es/devices/gripper/abstract_gripper.h

@@ -23,6 +23,11 @@ namespace cmvr::device{
         virtual void setPosition(float position, float vel) {}
         virtual void setForce(float value) {}
 
+        // Stops command-driven gripper activity while preserving the device
+        // lifecycle. Backends must explicitly confirm this contract before
+        // SystemService::StopAll can report success.
+        virtual bool stopOperationalActivity() { return false; }
+
     protected:
         GripperState state_;
     };

cmvr-es/devices/speaker/abstract_speaker.h

@@ -21,6 +21,11 @@ namespace cmvr::device{
         virtual int getVolume() const {return 0;}
         virtual void pause() {}
         virtual void resume() {}
+        // Stops the current file or streamed playback without changing the
+        // device lifecycle. SystemService StopAll and SpeakerService use this
+        // typed operation; implementations should return only after their
+        // playback workers can no longer emit audio.
+        virtual bool stopPlayback() { return false; }
         virtual bool pushAudioFrame(const AudioStreamFrameData& frame_data) { return false; }
         virtual void stopStreaming() {}
 

cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_speaker.h

@@ -32,6 +32,7 @@ namespace cmvr::device {
         bool init() override;
         bool start() override;
         bool stop() override;
+        bool stopPlayback() override;
         void play(const std::string& audio_path) override;
         void setVolume(int volume) override;
         int getVolume() const override;
@@ -45,6 +46,7 @@ namespace cmvr::device {
         bool initPulseDevice_();
         bool initAudioParams_(const std::string& audio_path);
     private:
+        bool stopPlayback_(bool deinitialize);
         void decode_audio_();
         void play_audio_();
         bool startStreamingPlayback_(const AudioStreamFrameData& frame_data);

cmvr-es/devices/speaker/ffmpeg_speaker/src/ffmpeg_speaker.cpp

@@ -34,6 +34,7 @@ ffmpegSpeaker::~ffmpegSpeaker() {
     is_stopping_ = true;
     {
         std::lock_guard<std::mutex> lock(mtx_);
+        state_.is_initialized = false;
         state_.is_running = false;
         state_.is_decoding = false;
         state_.is_paused = false;
@@ -94,7 +95,6 @@ void ffmpegSpeaker::resetPlayState()
         // 清空所有帧
     }
 
-    state_.is_initialized = false;
     is_streaming_input_ = false;
 
     audio_path_.clear();
@@ -102,11 +102,22 @@ void ffmpegSpeaker::resetPlayState()
 }
 
 bool ffmpegSpeaker::stop() {
+    return stopPlayback_(true);
+}
+
+bool ffmpegSpeaker::stopPlayback() {
+    return stopPlayback_(false);
+}
+
+bool ffmpegSpeaker::stopPlayback_(const bool deinitialize) {
     std::lock_guard<std::mutex> stop_lock(stop_mtx_);
     is_stopping_ = true;
 
     {
         lock_guard lock(mtx_);
+        if (deinitialize) {
+            state_.is_initialized = false;
+        }
         state_.is_running = false;
         state_.is_decoding = false;
         state_.is_paused = false;
@@ -304,7 +315,7 @@ void ffmpegSpeaker::decode_audio_() {
         return;
     }
 
-    const AVCodec* decoder = nullptr;
+    AVCodec* decoder = nullptr;
     const int stream_index = av_find_best_stream(fmt_ctx.get(), AVMEDIA_TYPE_AUDIO, -1, -1, &decoder, 0);
     if (stream_index < 0) {
         {

cmvr-es/main.cpp

@@ -1,10 +1,13 @@
 #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"
 
-
 namespace {
 
 bool blockShutdownSignals(sigset_t& shutdown_signals)
@@ -15,57 +18,76 @@ bool blockShutdownSignals(sigset_t& shutdown_signals)
     return pthread_sigmask(SIG_BLOCK, &shutdown_signals, nullptr) == 0;
 }
 
-} // namespace
+struct CommandLineOptions {
+    std::string config_path;
+    double control_period_s{0.001};
+    bool show_help{false};
+};
 
-namespace fs = std::filesystem;
-static bool setupIntelMediaEnvironment()
+void printUsage(const char* program)
 {
-    char exe_path_buffer[PATH_MAX] = {};
-
-    const ssize_t length = readlink(
-        "/proc/self/exe",
-        exe_path_buffer,
-        sizeof(exe_path_buffer) - 1);
-
-    if (length <= 0) {
-        std::cerr << "Failed to resolve /proc/self/exe\n";
-        return false;
+    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";
 }
 
-    exe_path_buffer[length] = '\0';
-
-    const fs::path executable_path(exe_path_buffer);
-    const fs::path bin_directory = executable_path.parent_path();
-    const fs::path install_directory = bin_directory.parent_path();
-    const fs::path library_directory = install_directory / "lib";
-    const fs::path va_driver_directory = library_directory / "dri";
-    const fs::path ihd_driver = va_driver_directory / "iHD_drv_video.so";
-
-    if (!fs::exists(ihd_driver)) {
-        std::cerr << "Intel media driver not found: "
-                  << ihd_driver << '\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;
     }
-
-    // overwrite=1：强制 cmvr-es 使用随项目发布的媒体运行时。
-    setenv("LIBVA_DRIVER_NAME", "iHD", 1);
-    setenv(
-        "LIBVA_DRIVERS_PATH",
-        va_driver_directory.c_str(),
-        1);
-    setenv(
-        "ONEVPL_SEARCH_PATH",
-        library_directory.c_str(),
-        1);
-
     return true;
 }
 
+} // namespace
 
-int main()
+int main(int argc, char* argv[])
 {
-    if (!setupIntelMediaEnvironment()) {
-        return EXIT_FAILURE;
+    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;
@@ -74,10 +96,13 @@ int main()
     }
 
     cmvr::Runtime runtime;
-    if (!runtime.init()) {
+    const bool initialized = options.config_path.empty()
+                                 ? runtime.init()
+                                 : runtime.init(options.config_path);
+    if (!initialized) {
         return 1;
     }
-    if (!runtime.startTasks()) {
+    if (!runtime.startTasks(options.control_period_s)) {
         return 1;
     }
 

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

@@ -15,9 +15,16 @@
 
 #include "device_factory.h"
 #include "cmvr/config/device_manager_config/device_manager_config.pb.h"
+#include "manager/safety_manager/include/safety_manager.h"
 
 namespace cmvr::device {
 
+    struct DeviceInventoryEntry {
+        std::string id;
+        DeviceKind kind = DeviceKind::Unknown;
+        std::shared_ptr<AbstractDevice> device;
+    };
+
     class DeviceManager {
     public:
         DeviceManager(const DeviceManager&) = delete;
@@ -27,16 +34,29 @@ namespace cmvr::device {
         static DeviceManager& getInstance();
         static void destroyInstance();
 
-        void start();
-        void restart();
+        bool start();
+        bool restart();
         void stop();
+        bool initialized() const noexcept { return initialized_; }
 
         void getDeviceList(std::list<std::pair<std::string, std::string>> &device_list);
         void registerDevice(const std::shared_ptr<AbstractDevice>& device);
         void registerDevice(const std::string& device_id, const std::shared_ptr<AbstractDevice>& device);
         std::shared_ptr<AbstractDevice> getDeviceBase(const std::string& device_id);
+        // Copies only manager-owned metadata and shared ownership. No device
+        // methods are called, so a blocked driver cannot delay this snapshot.
+        std::vector<DeviceInventoryEntry> inventorySnapshot() const;
         DeviceManagerSnapshot snapshot() const;
 
+        safety::SafetyManager& safetyManager() noexcept
+        {
+            return *safety_manager_;
+        }
+        const safety::SafetyManager& safetyManager() const noexcept
+        {
+            return *safety_manager_;
+        }
+
         std::string version() const;
         std::string name() const;
         std::string description() const;
@@ -54,14 +74,27 @@ namespace cmvr::device {
         std::unordered_map<std::string, DeviceRecord> devices_;
         std::unordered_map<std::string, ManagedDeviceSnapshot> device_statuses_;
         std::unique_ptr<DeviceFactory> dev_factory_;
+        std::unique_ptr<safety::SafetyManager> safety_manager_;
+        bool initialized_{false};
 
         explicit DeviceManager(const config::DeviceManagerConfig &cfg);
         void log_device_plan_() const;
-        void pre_scan_robot_arm_dependencies_() const;
-        void init_devices_();
+        bool pre_scan_robot_arm_dependencies_() const;
+        bool init_devices_();
         void configure_mujoco_viewer_pip_();
-        void start_devices_();
-        void stop_devices_();
+        void initialize_device_statuses_();
+        void mark_initializing_statuses_error_(const std::string& error_message);
+        void update_device_status_(const std::string& device_id,
+                                   ManagedDeviceState state,
+                                   const std::string& error_message = {});
+        DeviceHealthSnapshot sample_device_health_(
+            const std::shared_ptr<AbstractDevice>& device) const;
+        void update_device_health_(const std::string& device_id,
+                                   DeviceHealthSnapshot health);
+        bool register_device_safety_(
+            const std::shared_ptr<AbstractDevice>& device,
+            const config::DeviceConfigEntry* config_entry = nullptr);
+        void stop_devices_(bool update_status = true);
     };
 } // cmvr
 

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

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <chrono>
+#include <memory>
+
+#include "devices/abstract_device.h"
+#include "manager/safety_manager/include/safety_participant.h"
+
+namespace cmvr::device {
+
+safety::DeviceSafetyRegistration makeDeviceSafetyRegistration(
+    const std::shared_ptr<AbstractDevice>& device,
+    std::chrono::milliseconds configured_maximum_age =
+        std::chrono::milliseconds::zero(),
+    std::chrono::milliseconds configured_stop_timeout =
+        std::chrono::milliseconds::zero());
+
+} // namespace cmvr::device

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

@@ -4,9 +4,13 @@
 //
 
 #include "../include/device_manager.h"
+#include "../include/device_safety_adapters.h"
 
 #include <algorithm>
+#include <chrono>
 #include <exception>
+#include <utility>
+#include <vector>
 
 #include "devices/agv/abstract_agv.h"
 #include "devices/arm/robot_arm.h"
@@ -31,6 +35,113 @@ 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;
+constexpr auto kSafetyStartupValidationTimeout = std::chrono::seconds(2);
+
+cmvr::safety::SafetyManagerConfig safetyConfigFrom(
+    const cmvr::config::DeviceManagerConfig& config)
+{
+    cmvr::safety::SafetyManagerConfig result;
+    if (!config.has_safety()) {
+        result.enforcement_mode = cmvr::safety::EnforcementMode::Shadow;
+        return result;
+    }
+
+    const auto& source = config.safety();
+    switch (source.mode()) {
+    case cmvr::config::SafetyManagerConfig::LEGACY:
+        result.enforcement_mode = cmvr::safety::EnforcementMode::Legacy;
+        break;
+    case cmvr::config::SafetyManagerConfig::ENFORCE_SELECTED:
+        result.enforcement_mode =
+            cmvr::safety::EnforcementMode::EnforceSelected;
+        break;
+    case cmvr::config::SafetyManagerConfig::ENFORCE_ALL:
+        result.enforcement_mode = cmvr::safety::EnforcementMode::EnforceAll;
+        break;
+    case cmvr::config::SafetyManagerConfig::SHADOW:
+    case cmvr::config::SafetyManagerConfig::ENFORCEMENT_MODE_UNSPECIFIED:
+    default:
+        result.enforcement_mode = cmvr::safety::EnforcementMode::Shadow;
+        break;
+    }
+    for (const auto& id : source.enforced_device_ids()) {
+        if (!id.empty()) {
+            result.enforced_device_ids.insert(id);
+        }
+    }
+    for (const auto& entry : config.devices()) {
+        if (entry.safety_enforce() && !entry.id().empty()) {
+            result.enforced_device_ids.insert(entry.id());
+        }
+    }
+    if (source.stop_all_timeout_ms() != 0) {
+        result.stop_all_timeout =
+            std::chrono::milliseconds(source.stop_all_timeout_ms());
+    }
+    if (source.recovery_timeout_ms() != 0) {
+        result.recovery_timeout =
+            std::chrono::milliseconds(source.recovery_timeout_ms());
+    }
+    if (source.command_ledger_result_capacity() != 0) {
+        result.command_ledger.result_capacity =
+            source.command_ledger_result_capacity();
+    }
+    if (source.command_ledger_total_id_capacity() != 0) {
+        result.command_ledger.total_id_capacity =
+            source.command_ledger_total_id_capacity();
+    }
+    if (source.event_history_capacity() != 0) {
+        result.event_history_capacity = source.event_history_capacity();
+    }
+    result.fail_startup_on_missing_control_capability =
+        source.fail_startup_on_missing_control_capability();
+    return result;
+}
+
+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)
 {
@@ -122,16 +233,30 @@ std::shared_ptr<DeviceManager> DeviceManager::instance_ = nullptr;
 std::mutex DeviceManager::init_mutex_;
 
 
-DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) {
-    cfg_ = cfg;
-
-    dev_factory_ = std::make_unique<DeviceFactory>();
+DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg)
+    : cfg_(cfg),
+      dev_factory_(std::make_unique<DeviceFactory>()),
+      safety_manager_(std::make_unique<safety::SafetyManager>(
+          safetyConfigFrom(cfg)))
+{
+    initialize_device_statuses_();
     logSection("Device Plan");
     log_device_plan_();
-    pre_scan_robot_arm_dependencies_();
+    const bool dependencies_valid = pre_scan_robot_arm_dependencies_();
     logSection("Initialize Devices");
-    init_devices_();
+    if (!dependencies_valid) {
+        mark_initializing_statuses_error_(
+            "device dependency validation failed");
+    }
+    const bool devices_initialized =
+        dependencies_valid ? init_devices_() : false;
+    initialized_ = dependencies_valid && devices_initialized;
+    if (initialized_) {
         configure_mujoco_viewer_pip_();
+    } else {
+        CMVR_LOG(ERROR) << "[DeviceManager]: Initialization failed for at "
+                           "least one enabled device";
+    }
 }
 
 DeviceManager& DeviceManager::getInstance(const config::DeviceManagerConfig& cfg) {
@@ -156,35 +281,159 @@ void DeviceManager::destroyInstance() {
     MotorManager::clearActiveJoints();
 }
 
-void DeviceManager::start(){
-    for (auto& [id, record] : devices_) {
-        if (!record.device) {
-            CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id;
-            continue;
-        }
-        if (record.device->start()) {
-            CMVR_LOG(INFO) << "[DeviceManager]: Start device " << id << " Success";
-        } else {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id << " Failed";
+bool DeviceManager::start(){
+    std::lock_guard lifecycle_lock(lifecycle_mutex_);
+    if (!initialized_) {
+        CMVR_LOG(ERROR) << "[DeviceManager]: Refusing to start because "
+                           "initialization did not complete";
+        stop_devices_(false);
+        return false;
     }
+
+    std::vector<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);
         }
     }
 
-void DeviceManager::restart() {
+    bool all_started = true;
+    for (const auto& [id, device] : devices) {
+        if (!device) {
+            CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id;
+            update_device_status_(
+                id, ManagedDeviceState::Error,
+                "cannot start null device: " + id);
+            all_started = false;
+            continue;
+        }
+        bool started = false;
+        std::string error_message;
+        try {
+            (void)safety_manager_->advanceDeviceGeneration(id);
+            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) {
+            const auto health = sample_device_health_(device);
+            update_device_health_(id, health);
+            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) {
+        const auto coverage = safety_manager_->validateStartupCoverage(
+            safety::SafetyClock::now() + kSafetyStartupValidationTimeout);
+        if (!coverage.ready) {
+            all_started = false;
+            for (const auto& issue : coverage.issues) {
+                CMVR_LOG(ERROR)
+                    << "[DeviceManager]: Safety startup coverage failed"
+                    << ", target=" << issue.target_id
+                    << ", reason=" << safety::toString(issue.reason)
+                    << ", detail=" << issue.detail;
+            }
+        }
+    }
+    if (!all_started) {
+        CMVR_LOG(ERROR) << "[DeviceManager]: Device or safety startup failed; "
+                           "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);
+    } else {
+        safety_manager_->markStartupComplete();
+    }
+    return all_started;
+}
+
+bool DeviceManager::restart() {
     stop();
-    start();
+    return start();
 }
 
 void DeviceManager::stop() {
-    for (auto& [id, record] : devices_) {
-        if (!record.device) {
+    std::lock_guard lifecycle_lock(lifecycle_mutex_);
+    stop_devices_();
+}
+
+void DeviceManager::stop_devices_(const bool update_status) {
+    std::vector<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;
         }
-        if (record.device->stop()) {
+        bool stopped = false;
+        std::string error_message;
+        try {
+            stopped = device->stop();
+            if (!stopped) {
+                error_message = "device stop returned false: " + id;
+            }
+        } catch (const std::exception& error) {
+            error_message =
+                "device stop threw for " + id + ": " + error.what();
+            CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id
+                            << " threw: " << error.what();
+        } catch (...) {
+            error_message =
+                "device stop threw an unknown exception: " + id;
+            CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id
+                            << " threw an unknown exception";
+        }
+        if (stopped) {
+            if (update_status) {
+                update_device_status_(id, ManagedDeviceState::Stopped);
+            }
             CMVR_LOG(INFO) << "[DeviceManager]: Stop device " << id << " Success";
+            safety_manager_->updateDeviceRuntimeState(
+                id, ManagedDeviceState::Stopped,
+                sample_device_health_(device));
         } else {
+            if (update_status) {
+                update_device_status_(
+                    id, ManagedDeviceState::Error, error_message);
+            }
             CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id << " Failed";
+            safety_manager_->updateDeviceRuntimeState(
+                id, ManagedDeviceState::Error,
+                {DeviceHealthState::Fault, error_message});
         }
     }
 }
@@ -192,6 +441,7 @@ void DeviceManager::stop() {
 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.";
@@ -217,8 +467,27 @@ std::shared_ptr<AbstractDevice> DeviceManager::getDeviceBase(const std::string&
     return it->second.device;
 }
 
+std::vector<DeviceInventoryEntry> DeviceManager::inventorySnapshot() const
+{
+    std::vector<DeviceInventoryEntry> result;
+    {
+        std::shared_lock lock(devices_mutex_);
+        result.reserve(devices_.size());
+        for (const auto& [id, record] : devices_) {
+            result.push_back({id, record.kind, record.device});
+        }
+    }
+
+    std::sort(result.begin(), result.end(),
+              [](const auto& lhs, const auto& rhs) {
+                  return lhs.id < rhs.id;
+              });
+    return result;
+}
+
 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);
     }
@@ -244,42 +513,139 @@ 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;
+    {
+        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);
+    }
+    if (!register_device_safety_(device)) {
+        update_device_status_(
+            device_id, ManagedDeviceState::Error,
+            "failed to register device safety capability: " + device_id);
+        CMVR_LOG(ERROR) << "[DeviceManager]: Failed to register device safety "
+                           "capability, id=" << device_id;
+    } else {
+        update_device_health_(
+            device_id, sample_device_health_(device));
+    }
     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)
+{
+    DeviceHealthSnapshot health;
+    {
+        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();
+        health = status.health;
+    }
+    safety_manager_->updateDeviceRuntimeState(device_id, state, health);
+}
+
 DeviceManagerSnapshot DeviceManager::snapshot() const
 {
-    struct SnapshotSource {
-        ManagedDeviceSnapshot status;
-        std::shared_ptr<AbstractDevice> device;
-    };
-
-    std::vector<SnapshotSource> sources;
+    std::vector<ManagedDeviceSnapshot> sources;
     {
         std::shared_lock lock(devices_mutex_);
-        sources.reserve(devices_.size());
-        for (const auto& [id, record] : devices_) {
-            SnapshotSource source;
-            source.status.id = id;
-            source.status.kind = record.kind;
-            source.status.type_name = record.type_name;
-            source.status.enabled = true;
-            source.status.state = ManagedDeviceState::Ready;
-            source.device = record.device;
-            sources.push_back(std::move(source));
+        sources.reserve(device_statuses_.size());
+        for (const auto& [id, stored_status] : device_statuses_) {
+            (void)id;
+            sources.push_back(stored_status);
         }
     }
 
@@ -290,23 +656,22 @@ DeviceManagerSnapshot DeviceManager::snapshot() const
     result.devices.reserve(sources.size());
 
     for (auto& source : sources) {
-        if (source.device) {
-            try {
-                source.status.health = source.device->healthSnapshot();
-            } catch (const std::exception& error) {
-                source.status.health.state = DeviceHealthState::Fault;
-                source.status.health.error_message = error.what();
-            } catch (...) {
-                source.status.health.state = DeviceHealthState::Fault;
-                source.status.health.error_message =
-                    "device health snapshot threw an unknown exception";
+        source.health.error_message =
+            truncateDeviceError(source.health.error_message);
+        const bool lifecycle_error =
+            source.state == ManagedDeviceState::Error;
+        const bool health_error =
+            source.health.state == DeviceHealthState::Degraded ||
+            source.health.state == DeviceHealthState::Fault;
+        source.abnormal = lifecycle_error || health_error;
+        if (source.error_message.empty()) {
+            source.error_message = source.health.error_message;
         }
+        source.error_message = truncateDeviceError(source.error_message);
+        if (source.status_updated_at_unix_ms == 0) {
+            source.status_updated_at_unix_ms = unixTimeMs();
         }
-        source.status.abnormal =
-            source.status.health.state == DeviceHealthState::Degraded ||
-            source.status.health.state == DeviceHealthState::Fault;
-        source.status.error_message = source.status.health.error_message;
-        result.devices.push_back(std::move(source.status));
+        result.devices.push_back(std::move(source));
     }
 
     std::sort(result.devices.begin(), result.devices.end(),
@@ -317,6 +682,78 @@ DeviceManagerSnapshot DeviceManager::snapshot() const
     return result;
 }
 
+DeviceHealthSnapshot DeviceManager::sample_device_health_(
+    const std::shared_ptr<AbstractDevice>& device) const
+{
+    if (!device) {
+        return {
+            DeviceHealthState::Fault,
+            "device health target is null"};
+    }
+    try {
+        auto health = device->healthSnapshot();
+        health.error_message = truncateDeviceError(health.error_message);
+        return health;
+    } catch (const std::exception& error) {
+        return {DeviceHealthState::Fault, truncateDeviceError(error.what())};
+    } catch (...) {
+        return {
+            DeviceHealthState::Fault,
+            "device health snapshot threw an unknown exception"};
+    }
+}
+
+void DeviceManager::update_device_health_(
+    const std::string& device_id,
+    DeviceHealthSnapshot health)
+{
+    ManagedDeviceState lifecycle = ManagedDeviceState::Unknown;
+    {
+        std::unique_lock lock(devices_mutex_);
+        auto& status = device_statuses_[device_id];
+        status.health = std::move(health);
+        lifecycle = status.state;
+        const bool health_error =
+            status.health.state == DeviceHealthState::Degraded ||
+            status.health.state == DeviceHealthState::Fault;
+        status.abnormal =
+            status.state == ManagedDeviceState::Error || health_error;
+        if (status.state != ManagedDeviceState::Error) {
+            status.error_message = status.health.error_message;
+        }
+        status.status_updated_at_unix_ms = unixTimeMs();
+        health = status.health;
+    }
+    safety_manager_->updateDeviceRuntimeState(
+        device_id, lifecycle, std::move(health));
+}
+
+bool DeviceManager::register_device_safety_(
+    const std::shared_ptr<AbstractDevice>& device,
+    const config::DeviceConfigEntry* config_entry)
+{
+    auto maximum_age = std::chrono::milliseconds::zero();
+    auto stop_timeout = std::chrono::milliseconds::zero();
+    if (config_entry) {
+        if (config_entry->maximum_safety_snapshot_age_ms() != 0) {
+            maximum_age = std::chrono::milliseconds(
+                config_entry->maximum_safety_snapshot_age_ms());
+        }
+        if (config_entry->safety_stop_timeout_ms() != 0) {
+            stop_timeout = std::chrono::milliseconds(
+                config_entry->safety_stop_timeout_ms());
+        }
+    }
+    auto registration = makeDeviceSafetyRegistration(
+        device, maximum_age, stop_timeout);
+    if (registration.descriptor.device_id.empty()) {
+        return false;
+    }
+    const bool registered =
+        safety_manager_->registerDevice(std::move(registration));
+    return registered;
+}
+
 std::string DeviceManager::version() const {
     return cfg_.version().empty() ? "1.0" : cfg_.version();
 }
@@ -354,10 +791,11 @@ void DeviceManager::log_device_plan_() const
     CMVR_LOG(INFO) << "[DeviceManager]: Device plan end";
 }
 
-void DeviceManager::pre_scan_robot_arm_dependencies_() const
+bool DeviceManager::pre_scan_robot_arm_dependencies_() const
 {
     MotorJointSelections selections;
     std::unordered_map<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) {
@@ -365,22 +803,22 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
         }
         if (entry.id().empty()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager device id is empty";
-            return;
+            return false;
         }
         if (entry.config_file().empty()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager config_file is empty: " << entry.id();
-            return;
+            return false;
         }
 
         config::MotorRootConfig root_cfg;
         if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load motor config: " << entry.config_file();
-            return;
+            return false;
         }
         if (!root_cfg.motor().id().empty() && root_cfg.motor().id() != entry.id()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: MotorManager entry id '" << entry.id()
                             << "' does not match config id '" << root_cfg.motor().id() << "'";
-            return;
+            return false;
         }
         motor_roots.emplace(entry.id(), std::move(root_cfg));
     }
@@ -391,17 +829,17 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
         }
         if (entry.id().empty()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm device id is empty";
-            return;
+            return false;
         }
         if (entry.config_file().empty()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm config_file is empty: " << entry.id();
-            return;
+            return false;
         }
 
         config::ArmRootConfig root_cfg;
         if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
             CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load arm config: " << entry.config_file();
-            return;
+            return false;
         }
 
         const config::RobotArmConfig* arm_cfg = nullptr;
@@ -414,29 +852,30 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
         if (!arm_cfg) {
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm ID '" << entry.id()
                             << "' not found in config: " << entry.config_file();
-            return;
+            return false;
         }
 
-        if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor) {
+        if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor ||
+            arm_cfg->backend_case() == config::RobotArmConfig::kUme) {
             continue;
         }
         if (arm_cfg->backend_case() != config::RobotArmConfig::kMotor) {
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm backend is not configured: " << entry.id();
-            return;
+            return false;
         }
 
         const auto& motor_config = arm_cfg->motor();
         if (motor_config.motor_system_id().empty()) {
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_system_id: " << entry.id();
-            return;
+            return false;
         }
         if (motor_config.motor_group_ids_size() == 0) {
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_group_ids: " << entry.id();
-            return;
+            return false;
         }
         if (motor_config.joint_names_size() == 0) {
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing joint_names: " << entry.id();
-            return;
+            return false;
         }
 
         const auto motor_root_it = motor_roots.find(motor_config.motor_system_id());
@@ -444,7 +883,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
             CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id()
                             << "' depends on disabled or missing MotorManager: "
                             << motor_config.motor_system_id();
-            return;
+            return false;
         }
 
         std::unordered_set<std::string> allowed_groups;
@@ -452,7 +891,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
         for (const auto& group_id : motor_config.motor_group_ids()) {
             if (group_id.empty()) {
                 CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty motor_group_id: " << entry.id();
-                return;
+                return false;
             }
             allowed_groups.insert(group_id);
         }
@@ -461,7 +900,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
         for (const auto& joint_name : motor_config.joint_names()) {
             if (joint_name.empty()) {
                 CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty joint_name: " << entry.id();
-                return;
+                return false;
             }
 
             std::string matched_group;
@@ -480,7 +919,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
                 CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id()
                                 << "' joint '" << joint_name
                                 << "' not found in configured motor_group_ids";
-                return;
+                return false;
             }
             group_selection[matched_group].insert(joint_name);
         }
@@ -495,46 +934,132 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
         }
     }
 
-    MotorManager::clearActiveJoints();
     for (auto& [motor_system_id, group_selection] : selections) {
         MotorManager::setActiveJoints(motor_system_id, std::move(group_selection));
     }
+    return true;
 }
 
-void DeviceManager::init_devices_() {
+bool DeviceManager::init_devices_() {
+    bool all_initialized = true;
     for (const auto& entry : cfg_.devices()) {
         if (!entry.enable()) {
             continue;
         }
 
+        {
+            std::shared_lock lock(devices_mutex_);
+            const auto status_it = device_statuses_.find(entry.id());
+            if (status_it != device_statuses_.end() &&
+                status_it->second.state == ManagedDeviceState::Error) {
+                all_initialized = false;
+                continue;
+            }
+        }
+
         CMVR_LOG(INFO) << "[DeviceManager]: Initialize device begin"
                        << ", id=" << entry.id()
                        << ", type=" << deviceTypeToString(entry.type())
                        << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file());
 
-        DeviceRecord record = dev_factory_->create(entry);
+        DeviceRecord record;
+        try {
+            record = dev_factory_->create(entry);
+        } catch (const std::exception& error) {
+            update_device_status_(
+                entry.id(), ManagedDeviceState::Error,
+                "device creation threw for " + entry.id() + ": " +
+                    error.what());
+            CMVR_LOG(ERROR) << "[DeviceManager]: Device creation threw for "
+                            << entry.id() << ": " << error.what();
+            all_initialized = false;
+            continue;
+        } catch (...) {
+            update_device_status_(
+                entry.id(), ManagedDeviceState::Error,
+                "device creation threw an unknown exception: " +
+                    entry.id());
+            CMVR_LOG(ERROR) << "[DeviceManager]: Device creation threw an "
+                               "unknown exception for " << entry.id();
+            all_initialized = false;
+            continue;
+        }
         if (!record.device || record.id.empty()) {
+            update_device_status_(
+                entry.id(), ManagedDeviceState::Error,
+                "failed to create configured device: " + entry.id());
             CMVR_LOG(ERROR) << "[DeviceManager]: Failed to create device for entry id=" << entry.id();
+            all_initialized = false;
             continue;
         }
         CMVR_LOG(INFO) << "[DeviceManager]: Create device object success"
                        << ", id=" << record.id
                        << ", type=" << record.type_name
                        << ", kind=" << toString(record.kind);
-        if (devices_.count(record.id)) {
+        bool duplicate_device = false;
+        {
+            std::shared_lock lock(devices_mutex_);
+            duplicate_device = devices_.count(record.id) != 0;
+        }
+        if (duplicate_device) {
+            update_device_status_(
+                entry.id(), ManagedDeviceState::Error,
+                "duplicate configured device id: " + record.id);
             CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate " << record.type_name << " Device ID " << record.id;
+            all_initialized = false;
             continue;
         }
         CMVR_LOG(INFO) << "[DeviceManager]: Init device object begin"
                        << ", id=" << record.id
                        << ", type=" << record.type_name
                        << ", kind=" << toString(record.kind);
-        if (!record.device->init()) {
+        bool device_initialized = false;
+        std::string init_error_message;
+        try {
+            device_initialized = record.device->init();
+            if (!device_initialized) {
+                init_error_message =
+                    "device init returned false: " + record.id;
+            }
+        } catch (const std::exception& error) {
+            init_error_message =
+                "device init threw for " + record.id + ": " +
+                error.what();
+            CMVR_LOG(ERROR) << "[DeviceManager]: Init device object threw"
+                            << ", id=" << record.id
+                            << ", error=" << error.what();
+        } catch (...) {
+            init_error_message =
+                "device init threw an unknown exception: " + record.id;
+            CMVR_LOG(ERROR) << "[DeviceManager]: Init device object threw an "
+                               "unknown exception, id=" << record.id;
+        }
+        if (!device_initialized) {
+            update_device_status_(
+                entry.id(), ManagedDeviceState::Error,
+                init_error_message);
             CMVR_LOG(ERROR) << "[DeviceManager]: Init device object failed"
                             << ", id=" << record.id
                             << ", type=" << record.type_name
                             << ", kind=" << toString(record.kind)
                             << ", config_file=" << entry.config_file();
+            try {
+                if (!record.device->stop()) {
+                    CMVR_LOG(ERROR)
+                        << "[DeviceManager]: Cleanup after failed init "
+                           "returned false, id=" << record.id;
+                }
+            } catch (const std::exception& error) {
+                CMVR_LOG(ERROR)
+                    << "[DeviceManager]: Cleanup after failed init threw"
+                    << ", id=" << record.id
+                    << ", error=" << error.what();
+            } catch (...) {
+                CMVR_LOG(ERROR)
+                    << "[DeviceManager]: Cleanup after failed init threw an "
+                       "unknown exception, id=" << record.id;
+            }
+            all_initialized = false;
             continue;
         }
         CMVR_LOG(INFO) << "[DeviceManager]: Init device object success"
@@ -542,8 +1067,55 @@ void DeviceManager::init_devices_() {
                        << ", type=" << record.type_name
                        << ", kind=" << toString(record.kind)
                        << ", config_file=" << entry.config_file();
-        devices_.emplace(record.id, std::move(record));
+        const auto registered_device = record.device;
+        std::string registered_id;
+        {
+            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();
+            registered_id = id;
+        }
+
+        if (!register_device_safety_(registered_device, &entry)) {
+            update_device_status_(
+                registered_id, ManagedDeviceState::Error,
+                "failed to register device safety capability: " +
+                    registered_id);
+            CMVR_LOG(ERROR)
+                << "[DeviceManager]: Failed to register device safety "
+                   "capability"
+                << ", id=" << registered_id
+                << ", kind=" << toString(registered_device->kind());
+            all_initialized = false;
+        } else {
+            update_device_health_(
+                registered_id, sample_device_health_(registered_device));
+        }
+    }
+    return all_initialized;
 }
 
 void DeviceManager::configure_mujoco_viewer_pip_()

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

@@ -5,8 +5,12 @@
 #include "devices/microphone/abstract_microphone.h"
 
 #include <atomic>
+#include <chrono>
+#include <condition_variable>
 #include <cstddef>
+#include <future>
 #include <memory>
+#include <mutex>
 #include <stdexcept>
 #include <string>
 #include <thread>
@@ -23,6 +27,7 @@ namespace {
 using cmvr::device::AbstractDevice;
 using cmvr::device::DeviceHealthSnapshot;
 using cmvr::device::DeviceHealthState;
+using cmvr::device::DeviceInventoryEntry;
 using cmvr::device::DeviceKind;
 using cmvr::device::DeviceManager;
 using cmvr::device::DeviceManagerSnapshot;
@@ -123,6 +128,51 @@ public:
     std::atomic<int> health_calls{0};
 };
 
+class BlockingHealthDevice final : public AbstractDevice {
+public:
+    explicit BlockingHealthDevice(std::string id)
+        : AbstractDevice(std::move(id))
+    {
+    }
+
+    DeviceKind kind() const noexcept override { return DeviceKind::Arm; }
+    std::string typeName() const override { return "BlockingHealthDevice"; }
+
+    DeviceHealthSnapshot healthSnapshot() override
+    {
+        std::unique_lock lock(mutex_);
+        ++health_calls;
+        health_entered_ = true;
+        condition_.notify_all();
+        condition_.wait(lock, [this] { return release_health_; });
+        return {DeviceHealthState::Healthy, {}};
+    }
+
+    bool waitForHealthCall(const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(mutex_);
+        return condition_.wait_for(
+            lock, timeout, [this] { return health_entered_; });
+    }
+
+    void releaseHealthCall()
+    {
+        {
+            std::lock_guard lock(mutex_);
+            release_health_ = true;
+        }
+        condition_.notify_all();
+    }
+
+    std::atomic<int> health_calls{0};
+
+private:
+    std::mutex mutex_;
+    std::condition_variable condition_;
+    bool health_entered_{false};
+    bool release_health_{false};
+};
+
 const ManagedDeviceSnapshot* findDevice(const DeviceManagerSnapshot& snapshot,
                                         const std::string& id)
 {
@@ -144,6 +194,16 @@ bool isSorted(const DeviceManagerSnapshot& snapshot)
     return true;
 }
 
+bool isSorted(const std::vector<DeviceInventoryEntry>& inventory)
+{
+    for (std::size_t i = 1; i < inventory.size(); ++i) {
+        if (inventory[i].id < inventory[i - 1].id) {
+            return false;
+        }
+    }
+    return true;
+}
+
 bool testCategoryHealthAdapters()
 {
     MemoryCamera camera;
@@ -253,6 +313,13 @@ bool testConfiguredAndDynamicSnapshots()
     CHECK_TRUE(duplicate_status->error_message ==
                "duplicate configured device id: duplicate_device");
 
+    // Configuration failures deliberately make this manager ineligible for
+    // start(). Use a fresh, valid manager for dynamic registration and
+    // lifecycle transitions so the test does not weaken fail-closed startup.
+    DeviceManager::destroyInstance();
+    cmvr::config::DeviceManagerConfig dynamic_config;
+    auto& dynamic_manager = DeviceManager::getInstance(dynamic_config);
+
     auto healthy = std::make_shared<FakeDevice>("z_healthy");
     auto degraded = std::make_shared<FakeDevice>("a_degraded");
     degraded->health = {
@@ -264,18 +331,18 @@ bool testConfiguredAndDynamicSnapshots()
     auto health_throw = std::make_shared<FakeDevice>("b_health_throw");
     health_throw->throw_on_health = true;
 
-    manager.registerDevice(healthy);
-    manager.registerDevice(degraded);
-    manager.registerDevice(start_fail);
-    manager.registerDevice(stop_fail);
-    manager.registerDevice(health_throw);
+    dynamic_manager.registerDevice(healthy);
+    dynamic_manager.registerDevice(degraded);
+    dynamic_manager.registerDevice(start_fail);
+    dynamic_manager.registerDevice(stop_fail);
+    dynamic_manager.registerDevice(health_throw);
 
     // Duplicate registration must retain the original object and status.
-    manager.registerDevice(
+    dynamic_manager.registerDevice(
         std::make_shared<FakeDevice>("z_healthy", DeviceKind::Speaker));
-    CHECK_TRUE(manager.getDeviceBase("z_healthy") == healthy);
+    CHECK_TRUE(dynamic_manager.getDeviceBase("z_healthy") == healthy);
 
-    const auto registered = manager.snapshot();
+    const auto registered = dynamic_manager.snapshot();
     CHECK_TRUE(isSorted(registered));
     const auto* healthy_registered =
         findDevice(registered, "z_healthy");
@@ -304,8 +371,8 @@ bool testConfiguredAndDynamicSnapshots()
     CHECK_TRUE(thrown_health->health.error_message.size() <= 512);
     CHECK_TRUE(thrown_health->error_message.size() <= 512);
 
-    manager.start();
-    const auto running = manager.snapshot();
+    CHECK_TRUE(!dynamic_manager.start());
+    const auto running = dynamic_manager.snapshot();
     CHECK_TRUE(findDevice(running, "z_healthy")->state ==
                ManagedDeviceState::Running);
     CHECK_TRUE(findDevice(running, "m_start_fail")->state ==
@@ -319,14 +386,16 @@ bool testConfiguredAndDynamicSnapshots()
     CHECK_TRUE(healthy_registered->state ==
                ManagedDeviceState::Registered);
 
-    manager.stop();
-    const auto stopped = manager.snapshot();
+    dynamic_manager.stop();
+    const auto stopped = dynamic_manager.snapshot();
     CHECK_TRUE(findDevice(stopped, "z_healthy")->state ==
                ManagedDeviceState::Stopped);
     CHECK_TRUE(findDevice(stopped, "n_stop_fail")->state ==
                ManagedDeviceState::Error);
     CHECK_TRUE(findDevice(stopped, "n_stop_fail")->abnormal);
-    CHECK_TRUE(healthy->stop_calls.load() == 1);
+    // Failed start rolls back every device once; explicit stop performs the
+    // second best-effort stop.
+    CHECK_TRUE(healthy->stop_calls.load() == 2);
     return true;
 }
 
@@ -365,6 +434,71 @@ bool testConcurrentSnapshotAndRegistration()
     return true;
 }
 
+bool testManagerSnapshotsDoNotWaitForDeviceHealth()
+{
+    DeviceManager::destroyInstance();
+    cmvr::config::DeviceManagerConfig config;
+    auto& manager = DeviceManager::getInstance(config);
+    auto blocking_device =
+        std::make_shared<BlockingHealthDevice>("blocked_health_arm");
+    auto other_device =
+        std::make_shared<FakeDevice>("a_camera", DeviceKind::Camera);
+    manager.registerDevice(other_device);
+
+    auto registration_future = std::async(
+        std::launch::async, [&manager, blocking_device] {
+            manager.registerDevice(blocking_device);
+        });
+    if (!blocking_device->waitForHealthCall(std::chrono::seconds(2))) {
+        blocking_device->releaseHealthCall();
+        registration_future.wait();
+        return false;
+    }
+
+    auto snapshot_future = std::async(std::launch::async, [&manager] {
+        return manager.snapshot();
+    });
+    if (snapshot_future.wait_for(std::chrono::milliseconds(250)) !=
+        std::future_status::ready) {
+        blocking_device->releaseHealthCall();
+        snapshot_future.wait();
+        registration_future.wait();
+        return false;
+    }
+
+    auto inventory_future = std::async(std::launch::async, [&manager] {
+        return manager.inventorySnapshot();
+    });
+    if (inventory_future.wait_for(std::chrono::milliseconds(250)) !=
+        std::future_status::ready) {
+        blocking_device->releaseHealthCall();
+        inventory_future.wait();
+        registration_future.wait();
+        return false;
+    }
+
+    const auto snapshot = snapshot_future.get();
+    const auto inventory = inventory_future.get();
+    const auto* blocked_status =
+        findDevice(snapshot, "blocked_health_arm");
+    const bool snapshots_valid =
+        blocked_status != nullptr &&
+        blocked_status->state == ManagedDeviceState::Registered &&
+        blocked_status->health.state == DeviceHealthState::Unknown &&
+        inventory.size() == 2 && isSorted(inventory) &&
+        inventory[0].id == "a_camera" &&
+        inventory[0].kind == DeviceKind::Camera &&
+        inventory[0].device == other_device &&
+        inventory[1].id == "blocked_health_arm" &&
+        inventory[1].kind == DeviceKind::Arm &&
+        inventory[1].device == blocking_device &&
+        blocking_device->health_calls.load() == 1;
+
+    blocking_device->releaseHealthCall();
+    registration_future.get();
+    return snapshots_valid && blocking_device->health_calls.load() == 1;
+}
+
 } // namespace
 
 int main()
@@ -373,7 +507,8 @@ int main()
     const bool success =
         testCategoryHealthAdapters() &&
         testConfiguredAndDynamicSnapshots() &&
-        testConcurrentSnapshotAndRegistration();
+        testConcurrentSnapshotAndRegistration() &&
+        testManagerSnapshotsDoNotWaitForDeviceHealth();
     DeviceManager::destroyInstance();
     return success ? 0 : 1;
 }

cmvr-es/manager/media_source_hub/CMakeLists.txt

@@ -1,61 +0,0 @@
-if(CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR)
-    cmake_minimum_required(VERSION 3.22)
-    project(cmvr_media_source_hub LANGUAGES CXX)
-    enable_testing()
-endif()
-
-add_library(media_source_hub STATIC
-        src/media_source_hub.cpp
-)
-
-target_compile_features(media_source_hub PUBLIC cxx_std_17)
-target_include_directories(media_source_hub
-        PUBLIC
-        ${CMAKE_CURRENT_SOURCE_DIR}/../..
-)
-
-add_library(cmvr_es::media_source_hub ALIAS media_source_hub)
-
-if(NOT CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR)
-    add_library(media_source_hub_device_adapter STATIC
-            src/device_media_source_adapter.cpp
-    )
-    target_compile_features(media_source_hub_device_adapter PUBLIC cxx_std_17)
-    target_include_directories(media_source_hub_device_adapter
-            PUBLIC
-            ${CMAKE_CURRENT_SOURCE_DIR}/../..
-    )
-    target_link_libraries(media_source_hub_device_adapter
-            PUBLIC
-            cmvr_es::media_source_hub
-            cmvr_es::common
-            cmvr_es::proto
-            cmvr_es::logging
-    )
-    add_library(cmvr_es::media_source_hub_device_adapter ALIAS media_source_hub_device_adapter)
-endif()
-
-option(CMVR_MEDIA_SOURCE_HUB_BUILD_TESTS
-       "Build the standalone MediaSourceHub self-test"
-       ${PROJECT_IS_TOP_LEVEL})
-
-if(CMVR_MEDIA_SOURCE_HUB_BUILD_TESTS)
-    find_package(Threads REQUIRED)
-    add_executable(media_source_hub_test
-            tests/media_source_hub_test.cpp
-    )
-    target_compile_features(media_source_hub_test PRIVATE cxx_std_17)
-    target_link_libraries(media_source_hub_test
-            PRIVATE
-            cmvr_es::media_source_hub
-            Threads::Threads
-    )
-    # This self-test only links the static Hub and pthreads. In the root build,
-    # the project-wide third-party RUNPATH can otherwise make the loader pick up
-    # a vendor libstdc++.so (for example from the AUBO SDK), even though the test
-    # has no dependency on that SDK.
-    set_target_properties(media_source_hub_test PROPERTIES
-            SKIP_BUILD_RPATH TRUE
-    )
-    add_test(NAME media_source_hub_test COMMAND media_source_hub_test)
-endif()

cmvr-es/manager/media_source_hub/include/device_media_source_adapter.h

@@ -1,35 +0,0 @@
-#ifndef CMVR_ES_DEVICE_MEDIA_SOURCE_ADAPTER_H
-#define CMVR_ES_DEVICE_MEDIA_SOURCE_ADAPTER_H
-
-#pragma once
-
-#include <cstddef>
-#include <memory>
-#include <string>
-
-#include "devices/camera/abstract_camera.h"
-#include "devices/microphone/abstract_microphone.h"
-#include "manager/media_source_hub/include/media_source_hub.h"
-
-namespace cmvr::media {
-
-// Process-wide protocol-neutral media hub shared by gRPC and QUIC services.
-MediaSourceHub& globalMediaSourceHub();
-
-// Registration is idempotent for an already registered track. The adapter owns a
-// short-lived pump thread and one startStreaming()/stopStreaming() lease only while
-// at least one Hub subscription is active. It ensures start() succeeds but deliberately
-// does not call stop(), because the base device lifecycle can also be owned by control RPCs.
-bool ensureCameraMediaSource(
-    MediaSourceHub& hub,
-    const std::shared_ptr<device::AbstractCamera>& camera,
-    size_t ring_capacity = 64);
-
-bool ensureMicrophoneMediaSource(
-    MediaSourceHub& hub,
-    const std::shared_ptr<device::AbstractMicrophone>& microphone,
-    size_t ring_capacity = 256);
-
-} // namespace cmvr::media
-
-#endif // CMVR_ES_DEVICE_MEDIA_SOURCE_ADAPTER_H

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

@@ -1,132 +0,0 @@
-#ifndef CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H
-#define CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H
-
-#pragma once
-
-#include <chrono>
-#include <cstddef>
-#include <cstdint>
-#include <functional>
-#include <memory>
-#include <optional>
-#include <string>
-#include <vector>
-
-#include "common/base/ring_buffer.h"
-#include "common/media/media_frame.h"
-
-namespace cmvr::media {
-
-// MediaSourceHub owns no protocol-specific state. A device or capture adapter registers
-// start/stop callbacks and receives a sink callback when the first consumer subscribes.
-class MediaSourceHub final {
-public:
-    using FrameRing = BroadcastFrameRing<MediaFrame>;
-    using FrameReadResult = FrameRing::ReadResult;
-    using StartPosition = FrameRing::StartPosition;
-    using FrameSink = std::function<void(MediaFramePtr)>;
-    // Cancellation checks run while MediaSourceHub protects source lifecycle
-    // state. Predicates must therefore be fast, non-blocking and must not call
-    // back into the same hub.
-    using CancelPredicate = std::function<bool()>;
-
-    struct SourceCallbacks {
-        // start() may run asynchronously. It must observe cancelled during any
-        // potentially blocking startup work and return false promptly once set.
-        // MediaSourceHub retains the callback state until a non-cooperative start
-        // eventually returns, so late completion cannot access destroyed state.
-        std::function<bool(
-            const FrameSink& sink,
-            const CancelPredicate& cancelled)> start;
-        // stop() is the synchronous publication barrier for the last lease and
-        // must unblock and join the source producer before returning.
-        std::function<void()> stop;
-        std::function<bool()> request_key_frame;
-    };
-
-private:
-    struct SourceState;
-
-public:
-    class Subscription final {
-    public:
-        Subscription() = default;
-        ~Subscription();
-
-        Subscription(const Subscription&) = delete;
-        Subscription& operator=(const Subscription&) = delete;
-        Subscription(Subscription&& other) noexcept;
-        Subscription& operator=(Subscription&& other) noexcept;
-
-        // A Subscription owns one reader cursor and is single-consumer. Moving,
-        // resetting, or reading the same object concurrently is unsupported; use
-        // one independent subscription per consumer thread.
-        bool valid() const;
-        explicit operator bool() const { return valid(); }
-
-        // Returns the most recently observed immutable descriptor. A callback source may
-        // replace the initially registered UNKNOWN codec/config descriptor with the first
-        // real frame descriptor without invalidating existing subscriptions.
-        TrackDescriptorPtr descriptor() const;
-
-        std::optional<FrameReadResult> tryRead();
-        std::optional<FrameReadResult> waitRead(std::chrono::milliseconds timeout);
-        uint64_t discardPendingIfExceeds(size_t maximum_pending_frames);
-        uint64_t droppedCount() const noexcept;
-        void reset();
-
-    private:
-        friend class MediaSourceHub;
-        Subscription(std::shared_ptr<SourceState> source, FrameRing::Cursor cursor);
-
-        std::shared_ptr<SourceState> source_;
-        FrameRing::Cursor cursor_;
-        bool active_{false};
-    };
-
-    MediaSourceHub();
-    ~MediaSourceHub();
-
-    MediaSourceHub(const MediaSourceHub&) = delete;
-    MediaSourceHub& operator=(const MediaSourceHub&) = delete;
-
-    bool registerSource(
-        TrackDescriptorPtr initial_descriptor,
-        SourceCallbacks callbacks,
-        size_t ring_capacity = 64);
-
-    // Active sources cannot be unregistered. Destroy/reset their subscriptions first.
-    bool unregisterSource(const std::string& track_id);
-
-    bool hasSource(const std::string& track_id) const;
-    std::vector<TrackDescriptorPtr> listTracks() const;
-    size_t subscriberCount(const std::string& track_id) const;
-
-    // Protocol adapters can request an IDR after a discontinuity without knowing the
-    // concrete camera implementation. Returns false when unsupported or not running.
-    bool requestKeyFrame(const std::string& track_id) const;
-
-    Subscription subscribe(
-        const std::string& track_id,
-        StartPosition start_position = StartPosition::NEXT_PUBLISHED,
-        CancelPredicate cancelled = {});
-
-    // Stops all registered sources and invalidates outstanding subscriptions. The
-    // subscriptions remain destructible and their waitRead calls are awakened.
-    // A cooperative in-progress start is cancelled; a callback that violates the
-    // cancellation contract is quarantined with retained state rather than blocking
-    // shutdown or risking a use-after-free.
-    void shutdown();
-
-private:
-    struct Impl;
-    std::shared_ptr<Impl> impl_;
-};
-
-// Compatibility alias for older protocol tests and integrations. New code should
-// use MediaSourceHub directly.
-using MediaSourceManager = MediaSourceHub;
-
-} // namespace cmvr::media
-
-#endif // CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H

cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp

@@ -1,687 +0,0 @@
-#include "manager/media_source_hub/include/device_media_source_adapter.h"
-
-#include <algorithm>
-#include <atomic>
-#include <chrono>
-#include <cctype>
-#include <cstdint>
-#include <exception>
-#include <iterator>
-#include <limits>
-#include <mutex>
-#include <thread>
-#include <utility>
-#include <vector>
-
-#include "common/base/logging/logger.h"
-
-namespace cmvr::media {
-namespace {
-
-std::string normalizedCodec(std::string codec) {
-    codec.erase(
-        std::remove_if(codec.begin(), codec.end(), [](const unsigned char c) {
-            return !std::isalnum(c);
-        }),
-        codec.end());
-    std::transform(codec.begin(), codec.end(), codec.begin(), [](const unsigned char c) {
-        return static_cast<char>(std::tolower(c));
-    });
-    return codec;
-}
-
-Codec videoCodec(const std::string& value) {
-    const std::string codec = normalizedCodec(value);
-    if (codec == "h264" || codec == "avc" || codec == "avc1" ||
-        codec == "libx264" || codec == "h264qsv") {
-        return Codec::H264;
-    }
-    if (codec == "h265" || codec == "hevc" || codec == "hvc1" ||
-        codec == "libx265" || codec == "h265qsv" || codec == "hevcqsv") {
-        return Codec::H265;
-    }
-    return Codec::UNKNOWN;
-}
-
-PayloadFormat videoPayloadFormat(
-    const Codec codec,
-    const std::vector<uint8_t>& payload) noexcept {
-    if (codec != Codec::H264 && codec != Codec::H265) {
-        return PayloadFormat::UNKNOWN;
-    }
-    const bool three_byte_start_code = payload.size() >= 3 &&
-        payload[0] == 0U && payload[1] == 0U && payload[2] == 1U;
-    const bool four_byte_start_code = payload.size() >= 4 &&
-        payload[0] == 0U && payload[1] == 0U && payload[2] == 0U && payload[3] == 1U;
-    return three_byte_start_code || four_byte_start_code
-        ? PayloadFormat::ANNEX_B
-        : PayloadFormat::UNKNOWN;
-}
-
-Codec audioCodec(const std::string& value) {
-    const std::string codec = normalizedCodec(value);
-    if (codec == "opus") return Codec::OPUS;
-    if (codec == "aac") return Codec::AAC;
-    if (codec == "pcms16le") return Codec::PCM_S16LE;
-    return Codec::UNKNOWN;
-}
-
-Codec audioCodec(const device::AudioStreamFrameData& source) {
-    const Codec codec = audioCodec(source.codec);
-    if (codec != Codec::UNKNOWN || !normalizedCodec(source.codec).empty()) {
-        return codec;
-    }
-    switch (source.format) {
-        case device::AudioStreamFormat::PCM:
-            return Codec::PCM_S16LE;
-        case device::AudioStreamFormat::AAC:
-            return Codec::AAC;
-        case device::AudioStreamFormat::OPUS:
-            return Codec::OPUS;
-        default:
-            return Codec::UNKNOWN;
-    }
-}
-
-PayloadFormat audioPayloadFormat(
-    const Codec codec,
-    const std::vector<uint8_t>& payload) noexcept {
-    switch (codec) {
-        case Codec::OPUS:
-            return PayloadFormat::OPUS_PACKET;
-        case Codec::PCM_S16LE:
-            return PayloadFormat::RAW;
-        case Codec::AAC: {
-            // FFmpeg encoders commonly expose raw AAC access units plus AudioSpecificConfig;
-            // only advertise ADTS when the sync word and layer bits are actually present.
-            const bool has_adts_header = payload.size() >= 2 && payload[0] == 0xFFU &&
-                (payload[1] & 0xF6U) == 0xF0U;
-            return has_adts_header ? PayloadFormat::AAC_ADTS : PayloadFormat::RAW;
-        }
-        default:
-            return PayloadFormat::UNKNOWN;
-    }
-}
-
-Rational sanitizedTimeBase(
-    const int32_t numerator,
-    const int32_t denominator,
-    const int32_t fallback_denominator) noexcept {
-    return Rational{
-        numerator > 0 ? numerator : 1,
-        denominator > 0 ? denominator : std::max(1, fallback_denominator)};
-}
-
-uint64_t descriptorGeneration(
-    const uint64_t stream_epoch,
-    const uint32_t codec_generation) {
-    const uint64_t generation = (stream_epoch << 32U) | codec_generation;
-    return generation == 0 ? 1 : generation;
-}
-
-template<typename DeviceT>
-struct PumpState : public std::enable_shared_from_this<PumpState<DeviceT>> {
-    explicit PumpState(std::shared_ptr<DeviceT> device_ptr)
-        : device(std::move(device_ptr)) {}
-
-    virtual ~PumpState() {
-        stop();
-    }
-
-    bool begin(
-        const MediaSourceHub::FrameSink& frame_sink,
-        const MediaSourceHub::CancelPredicate& cancelled) {
-        if (!frame_sink || !device) {
-            return false;
-        }
-        const auto cancellation_requested = [&cancelled] {
-            if (!cancelled) return false;
-            try {
-                return cancelled();
-            } catch (...) {
-                return true;
-            }
-        };
-        if (cancellation_requested()) {
-            return false;
-        }
-
-        std::unique_lock<std::mutex> lock(mutex);
-        if (running.load(std::memory_order_acquire)) {
-            return true;
-        }
-        if (worker.joinable()) {
-            // A previous worker must always be collected before a new capture lease starts.
-            std::thread stale_worker = std::move(worker);
-            lock.unlock();
-            collectThread(std::move(stale_worker));
-            lock.lock();
-        }
-
-        sink = frame_sink;
-        bool streaming_attempted = false;
-        try {
-            if (!device->start()) {
-                sink = {};
-                return false;
-            }
-            if (cancellation_requested()) {
-                sink = {};
-                return false;
-            }
-            streaming_attempted = true;
-            if (!device->startStreaming()) {
-                sink = {};
-                lock.unlock();
-                stopDeviceStreaming();
-                return false;
-            }
-            streaming_started = true;
-            if (cancellation_requested()) {
-                streaming_started = false;
-                sink = {};
-                lock.unlock();
-                stopDeviceStreaming();
-                return false;
-            }
-            running.store(true, std::memory_order_release);
-            try {
-                // The worker owns the pump while run() is active. This also
-                // makes the defensive self-stop/detach path lifetime-safe.
-                const auto self = this->shared_from_this();
-                worker = std::thread([self] { self->run(); });
-            } catch (...) {
-                running.store(false, std::memory_order_release);
-                streaming_started = false;
-                sink = {};
-                lock.unlock();
-                stopDeviceStreaming();
-                return false;
-            }
-        } catch (const std::exception& error) {
-            CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to start media source: "
-                            << error.what();
-            sink = {};
-            lock.unlock();
-            if (streaming_attempted) stopDeviceStreaming();
-            return false;
-        } catch (...) {
-            CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to start media source";
-            sink = {};
-            lock.unlock();
-            if (streaming_attempted) stopDeviceStreaming();
-            return false;
-        }
-        return true;
-    }
-
-    void stop() noexcept {
-        std::thread thread;
-        bool stop_streaming = false;
-        {
-            std::lock_guard<std::mutex> lock(mutex);
-            running.store(false, std::memory_order_release);
-            stop_streaming = streaming_started;
-            streaming_started = false;
-            sink = {};
-            thread = std::move(worker);
-        }
-        if (stop_streaming) {
-            stopDeviceStreaming();
-        }
-        if (thread.joinable()) {
-            collectThread(std::move(thread));
-        }
-    }
-
-    static void collectThread(std::thread thread) noexcept {
-        if (!thread.joinable()) {
-            return;
-        }
-        try {
-            if (thread.get_id() == std::this_thread::get_id()) {
-                thread.detach();
-            } else {
-                thread.join();
-            }
-        } catch (const std::exception& error) {
-            CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to collect media pump: "
-                            << error.what();
-            if (thread.joinable()) {
-                try {
-                    thread.detach();
-                } catch (...) {
-                    // std::thread's destructor would terminate if this extremely rare
-                    // platform error occurred; there is no recoverable ownership path.
-                }
-            }
-        }
-    }
-
-    virtual void run() = 0;
-
-    void stopDeviceStreaming() noexcept {
-        try {
-            if (device) {
-                device->stopStreaming();
-            }
-        } catch (const std::exception& error) {
-            CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to stop media source: "
-                            << error.what();
-        } catch (...) {
-            CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to stop media source";
-        }
-    }
-
-    std::shared_ptr<DeviceT> device;
-    std::atomic<bool> running{false};
-    std::mutex mutex;
-    std::thread worker;
-    MediaSourceHub::FrameSink sink;
-    bool streaming_started{false};
-};
-
-struct CameraPump final : PumpState<device::AbstractCamera> {
-    CameraPump(std::shared_ptr<device::AbstractCamera> camera, std::string id)
-        : PumpState(std::move(camera)), track_id(std::move(id)) {}
-    ~CameraPump() override { stop(); }
-
-    void run() override {
-        size_t cursor = 0;
-        uint64_t last_epoch = 0;
-        uint64_t last_sequence = 0;
-        uint64_t cached_config_generation = 0;
-        std::vector<uint8_t> cached_codec_config;
-        TrackDescriptorPtr last_descriptor;
-        bool have_previous = false;
-        bool have_cached_config_generation = false;
-        bool waiting_for_key_frame = true;
-        bool pending_discontinuity = true;
-        bool requested_key_frame = false;
-        std::chrono::steady_clock::time_point last_key_frame_request;
-
-        const auto request_key_frame = [&] {
-            const auto now = std::chrono::steady_clock::now();
-            if (requested_key_frame &&
-                now - last_key_frame_request < std::chrono::milliseconds(250)) {
-                return;
-            }
-            requested_key_frame = true;
-            last_key_frame_request = now;
-            try {
-                device->requestKeyFrame();
-            } catch (...) {
-                // Unsupported/failed key-frame requests fall back to the encoder's GOP.
-            }
-        };
-        request_key_frame();
-
-        while (running.load(std::memory_order_acquire)) {
-            device::StreamFrameData source;
-            try {
-                if (!device->waitEncodedFrame(source, cursor, std::chrono::milliseconds(50))) {
-                    std::this_thread::sleep_for(std::chrono::milliseconds(1));
-                    continue;
-                }
-            } catch (const std::exception& error) {
-                CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Camera frame read failed for "
-                                << track_id << ": " << error.what();
-                std::this_thread::sleep_for(std::chrono::milliseconds(10));
-                continue;
-            } catch (...) {
-                CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Camera frame read failed for "
-                                << track_id;
-                std::this_thread::sleep_for(std::chrono::milliseconds(10));
-                continue;
-            }
-            if (source.rgbFrame.empty()) {
-                continue;
-            }
-
-            const uint64_t descriptor_generation = descriptorGeneration(
-                source.stream_epoch, source.codec_config_generation);
-            if (!have_cached_config_generation ||
-                cached_config_generation != descriptor_generation) {
-                cached_codec_config.clear();
-                cached_config_generation = descriptor_generation;
-                have_cached_config_generation = true;
-            }
-            if (!source.codec_config.empty()) {
-                cached_codec_config = source.codec_config;
-            }
-
-            TrackDescriptor::Config track;
-            track.id = track_id;
-            track.source_id = device->id();
-            track.kind = MediaKind::VIDEO;
-            track.codec = videoCodec(source.codec);
-            track.payload_format = videoPayloadFormat(track.codec, source.rgbFrame);
-            track.time_base = sanitizedTimeBase(
-                source.time_base_num,
-                source.time_base_den,
-                source.fps);
-            track.width = static_cast<uint32_t>(std::max(0, source.width));
-            track.height = static_cast<uint32_t>(std::max(0, source.height));
-            track.nominal_rate = static_cast<uint32_t>(std::max(0, source.fps));
-            track.fx = source.intrinsics.fx;
-            track.fy = source.intrinsics.fy;
-            track.cx = source.intrinsics.cx;
-            track.cy = source.intrinsics.cy;
-            track.distortion.assign(
-                std::begin(source.intrinsics.coeffs),
-                std::end(source.intrinsics.coeffs));
-            track.generation = descriptor_generation;
-            track.codec_config = cached_codec_config;
-
-            const bool epoch_changed = have_previous && source.stream_epoch != last_epoch;
-            const bool sequence_wrapped = have_previous &&
-                last_sequence == std::numeric_limits<uint64_t>::max() && source.sequence != 0;
-            const bool sequence_gap = have_previous && !epoch_changed &&
-                (sequence_wrapped ||
-                 (last_sequence != std::numeric_limits<uint64_t>::max() &&
-                  source.sequence != last_sequence + 1));
-
-            TrackDescriptorPtr descriptor;
-            try {
-                descriptor = makeTrackDescriptor(std::move(track));
-            } catch (const std::exception& error) {
-                CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Invalid camera descriptor for "
-                                << track_id << ": " << error.what();
-                pending_discontinuity = true;
-                last_epoch = source.stream_epoch;
-                last_sequence = source.sequence;
-                have_previous = true;
-                continue;
-            }
-            const bool descriptor_changed = last_descriptor &&
-                !equivalentTrackDescriptor(*last_descriptor, *descriptor);
-            const bool discontinuity = !have_previous || source.discontinuity || epoch_changed ||
-                sequence_gap || descriptor_changed;
-            const bool inter_frame_codec = descriptor->codec == Codec::H264 ||
-                descriptor->codec == Codec::H265;
-
-            if (discontinuity) {
-                pending_discontinuity = true;
-                if (inter_frame_codec) {
-                    waiting_for_key_frame = true;
-                    request_key_frame();
-                } else {
-                    waiting_for_key_frame = false;
-                }
-            }
-
-            last_epoch = source.stream_epoch;
-            last_sequence = source.sequence;
-            last_descriptor = descriptor;
-            have_previous = true;
-
-            if (waiting_for_key_frame && !source.bKey) {
-                request_key_frame();
-                continue;
-            }
-            waiting_for_key_frame = false;
-
-            MediaFrame::Config frame;
-            frame.descriptor = std::move(descriptor);
-            frame.payload = std::move(source.rgbFrame);
-            frame.sequence = source.sequence;
-            frame.source_timestamp = source.source_timestamp;
-            frame.source_frame_number = source.source_frame_number;
-            frame.pts = source.pts;
-            frame.dts = source.dts;
-            frame.duration = source.duration;
-            frame.capture_time_ns = source.capture_monotonic_ns > 0
-                ? static_cast<uint64_t>(source.capture_monotonic_ns)
-                : 0;
-            frame.capture_utc_ns = source.capture_utc_ns;
-            frame.key_frame = source.bKey;
-            frame.discontinuity = pending_discontinuity;
-
-            MediaSourceHub::FrameSink current_sink;
-            {
-                std::lock_guard<std::mutex> lock(mutex);
-                current_sink = sink;
-            }
-            if (current_sink) {
-                try {
-                    current_sink(makeMediaFrame(std::move(frame)));
-                    pending_discontinuity = false;
-                } catch (const std::exception& error) {
-                    CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Invalid camera frame for "
-                                    << track_id << ": " << error.what();
-                    pending_discontinuity = true;
-                }
-            }
-        }
-    }
-
-    std::string track_id;
-};
-
-struct MicrophonePump final : PumpState<device::AbstractMicrophone> {
-    MicrophonePump(std::shared_ptr<device::AbstractMicrophone> microphone, std::string id)
-        : PumpState(std::move(microphone)), track_id(std::move(id)) {}
-    ~MicrophonePump() override { stop(); }
-
-    void run() override {
-        size_t cursor = 0;
-        uint64_t last_epoch = 0;
-        uint64_t last_sequence = 0;
-        uint64_t cached_config_generation = 0;
-        std::vector<uint8_t> cached_codec_config;
-        TrackDescriptorPtr last_descriptor;
-        bool have_previous = false;
-        bool have_cached_config_generation = false;
-        bool pending_discontinuity = true;
-        while (running.load(std::memory_order_acquire)) {
-            device::AudioStreamFrameData source;
-            try {
-                if (!device->waitEncodedFrame(source, cursor, std::chrono::milliseconds(50))) {
-                    std::this_thread::sleep_for(std::chrono::milliseconds(1));
-                    continue;
-                }
-            } catch (const std::exception& error) {
-                CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Microphone frame read failed for "
-                                << track_id << ": " << error.what();
-                std::this_thread::sleep_for(std::chrono::milliseconds(10));
-                continue;
-            } catch (...) {
-                CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Microphone frame read failed for "
-                                << track_id;
-                std::this_thread::sleep_for(std::chrono::milliseconds(10));
-                continue;
-            }
-            if (source.data.empty()) {
-                continue;
-            }
-
-            const uint64_t descriptor_generation = descriptorGeneration(
-                source.stream_epoch, source.codec_config_generation);
-            if (!have_cached_config_generation ||
-                cached_config_generation != descriptor_generation) {
-                cached_codec_config.clear();
-                cached_config_generation = descriptor_generation;
-                have_cached_config_generation = true;
-            }
-            if (!source.codec_config.empty()) {
-                cached_codec_config = source.codec_config;
-            }
-
-            TrackDescriptor::Config track;
-            track.id = track_id;
-            track.source_id = device->id();
-            track.kind = MediaKind::AUDIO;
-            track.codec = audioCodec(source);
-            track.payload_format = audioPayloadFormat(track.codec, source.data);
-            track.time_base = sanitizedTimeBase(
-                source.time_base_num,
-                source.time_base_den,
-                source.sample_rate);
-            track.sample_rate = static_cast<uint32_t>(std::max(0, source.sample_rate));
-            track.channels = static_cast<uint32_t>(std::max(0, source.channels));
-            // Packet sample counts belong to MediaFrame::duration, not immutable track metadata.
-            track.nominal_rate = 0;
-            track.generation = descriptor_generation;
-            track.codec_config = cached_codec_config;
-
-            const bool epoch_changed = have_previous && source.stream_epoch != last_epoch;
-            const bool sequence_wrapped = have_previous &&
-                last_sequence == std::numeric_limits<uint64_t>::max() && source.sequence != 0;
-            const bool sequence_gap = have_previous && !epoch_changed &&
-                (sequence_wrapped ||
-                 (last_sequence != std::numeric_limits<uint64_t>::max() &&
-                  source.sequence != last_sequence + 1));
-
-            TrackDescriptorPtr descriptor;
-            try {
-                descriptor = makeTrackDescriptor(std::move(track));
-            } catch (const std::exception& error) {
-                CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Invalid microphone descriptor for "
-                                << track_id << ": " << error.what();
-                pending_discontinuity = true;
-                last_epoch = source.stream_epoch;
-                last_sequence = source.sequence;
-                have_previous = true;
-                continue;
-            }
-            const bool descriptor_changed = last_descriptor &&
-                !equivalentTrackDescriptor(*last_descriptor, *descriptor);
-            if (!have_previous || source.discontinuity || epoch_changed || sequence_gap ||
-                descriptor_changed) {
-                pending_discontinuity = true;
-            }
-
-            last_epoch = source.stream_epoch;
-            last_sequence = source.sequence;
-            last_descriptor = descriptor;
-            have_previous = true;
-
-            MediaFrame::Config frame;
-            frame.descriptor = std::move(descriptor);
-            frame.payload = std::move(source.data);
-            frame.sequence = source.sequence;
-            frame.pts = source.pts;
-            frame.dts = source.dts;
-            frame.duration = source.duration > 0 ? source.duration : source.nb_samples;
-            frame.capture_time_ns = source.capture_monotonic_ns > 0
-                ? static_cast<uint64_t>(source.capture_monotonic_ns)
-                : 0;
-            frame.capture_utc_ns = source.capture_utc_ns;
-            frame.key_frame = true;
-            frame.discontinuity = pending_discontinuity;
-
-            MediaSourceHub::FrameSink current_sink;
-            {
-                std::lock_guard<std::mutex> lock(mutex);
-                current_sink = sink;
-            }
-            if (current_sink) {
-                try {
-                    current_sink(makeMediaFrame(std::move(frame)));
-                    pending_discontinuity = false;
-                } catch (const std::exception& error) {
-                    CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Invalid microphone frame for "
-                                    << track_id << ": " << error.what();
-                    pending_discontinuity = true;
-                }
-            }
-        }
-    }
-
-    std::string track_id;
-};
-
-TrackDescriptorPtr initialTrack(
-    std::string track_id,
-    std::string source_id,
-    const MediaKind kind) {
-    TrackDescriptor::Config config;
-    config.id = std::move(track_id);
-    config.source_id = std::move(source_id);
-    config.kind = kind;
-    // A placeholder descriptor is never emitted as a media sample, but it
-    // still carries a mathematically valid neutral time base.
-    config.time_base = Rational{1, 1};
-    config.generation = 1;
-    return makeTrackDescriptor(std::move(config));
-}
-
-} // namespace
-
-MediaSourceHub& globalMediaSourceHub() {
-    static MediaSourceHub hub;
-    return hub;
-}
-
-static std::string cameraColorTrackId(const std::string& device_id) {
-    return device_id + "/video/color";
-}
-
-static std::string microphoneTrackId(const std::string& device_id) {
-    return device_id + "/audio/main";
-}
-
-bool ensureCameraMediaSource(
-    MediaSourceHub& hub,
-    const std::shared_ptr<device::AbstractCamera>& camera,
-    const size_t ring_capacity) {
-    if (!camera || camera->id().empty()) {
-        return false;
-    }
-    const std::string track_id = cameraColorTrackId(camera->id());
-    if (hub.hasSource(track_id)) {
-        return true;
-    }
-
-    const auto pump = std::make_shared<CameraPump>(camera, track_id);
-    MediaSourceHub::SourceCallbacks callbacks;
-    callbacks.start = [pump](
-                          const MediaSourceHub::FrameSink& sink,
-                          const MediaSourceHub::CancelPredicate& cancelled) {
-        return pump->begin(sink, cancelled);
-    };
-    callbacks.stop = [pump] { pump->stop(); };
-    callbacks.request_key_frame = [camera] { return camera->requestKeyFrame(); };
-    const bool registered = hub.registerSource(
-        initialTrack(track_id, camera->id(), MediaKind::VIDEO),
-        std::move(callbacks),
-        ring_capacity);
-    if (!registered && !hub.hasSource(track_id)) {
-        CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to register camera track: " << track_id;
-        return false;
-    }
-    return true;
-}
-
-bool ensureMicrophoneMediaSource(
-    MediaSourceHub& hub,
-    const std::shared_ptr<device::AbstractMicrophone>& microphone,
-    const size_t ring_capacity) {
-    if (!microphone || microphone->id().empty()) {
-        return false;
-    }
-    const std::string track_id = microphoneTrackId(microphone->id());
-    if (hub.hasSource(track_id)) {
-        return true;
-    }
-
-    const auto pump = std::make_shared<MicrophonePump>(microphone, track_id);
-    MediaSourceHub::SourceCallbacks callbacks;
-    callbacks.start = [pump](
-                          const MediaSourceHub::FrameSink& sink,
-                          const MediaSourceHub::CancelPredicate& cancelled) {
-        return pump->begin(sink, cancelled);
-    };
-    callbacks.stop = [pump] { pump->stop(); };
-    const bool registered = hub.registerSource(
-        initialTrack(track_id, microphone->id(), MediaKind::AUDIO),
-        std::move(callbacks),
-        ring_capacity);
-    if (!registered && !hub.hasSource(track_id)) {
-        CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to register microphone track: " << track_id;
-        return false;
-    }
-    return true;
-}
-
-} // namespace cmvr::media

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

@@ -1,596 +0,0 @@
-#include "manager/media_source_hub/include/media_source_hub.h"
-
-#include <algorithm>
-#include <atomic>
-#include <condition_variable>
-#include <mutex>
-#include <thread>
-#include <unordered_map>
-#include <utility>
-
-namespace cmvr::media {
-
-struct MediaSourceHub::SourceState final : public std::enable_shared_from_this<SourceState> {
-    enum class Lifecycle {
-        STOPPED,
-        STARTING,
-        RUNNING,
-        STOPPING
-    };
-
-    struct StartAttempt {
-        size_t waiters{0};
-        bool completed{false};
-        bool succeeded{false};
-        std::atomic<bool> cancel_requested{false};
-    };
-
-    SourceState(
-        TrackDescriptorPtr initial_descriptor,
-        SourceCallbacks source_callbacks,
-        const size_t ring_capacity)
-        : track_id(initial_descriptor->id),
-          descriptor(std::move(initial_descriptor)),
-          callbacks(std::move(source_callbacks)),
-          ring(ring_capacity) {}
-
-    FrameSink makeSink() {
-        const std::weak_ptr<SourceState> weak_source = shared_from_this();
-        return [weak_source](MediaFramePtr frame) {
-            if (const auto source = weak_source.lock()) {
-                source->acceptFrame(std::move(frame));
-            }
-        };
-    }
-
-    void acceptFrame(MediaFramePtr frame) {
-        if (!frame || !frame->descriptor || frame->descriptor->id != track_id) {
-            return;
-        }
-
-        {
-            std::lock_guard<std::mutex> lock(lifecycle_mutex);
-            if (!registered ||
-                (lifecycle != Lifecycle::STARTING && lifecycle != Lifecycle::RUNNING)) {
-                return;
-            }
-        }
-
-        const TrackDescriptorPtr current = std::atomic_load(&descriptor);
-        if (!current || !equivalentTrackDescriptor(*current, *frame->descriptor)) {
-            // C++17 atomic shared_ptr free functions provide an atomic descriptor snapshot to
-            // all subscriptions while frames remain immutable.
-            std::atomic_store(&descriptor, frame->descriptor);
-        }
-        ring.publish(std::move(frame));
-    }
-
-    static bool isCancelled(const CancelPredicate& cancelled) noexcept {
-        if (!cancelled) return false;
-        try {
-            return cancelled();
-        } catch (...) {
-            return true;
-        }
-    }
-
-    bool invokeStart(
-        const FrameSink& sink,
-        const CancelPredicate& cancelled) noexcept {
-        std::lock_guard<std::mutex> callback_lock(callback_mutex);
-        try {
-            return callbacks.start && callbacks.start(sink, cancelled);
-        } catch (...) {
-            return false;
-        }
-    }
-
-    void invokeStop() noexcept {
-        std::lock_guard<std::mutex> callback_lock(callback_mutex);
-        try {
-            if (callbacks.stop) callbacks.stop();
-        } catch (...) {
-        }
-    }
-
-    void completeStart(
-        const std::shared_ptr<StartAttempt>& attempt,
-        const bool started) {
-        bool stop_abandoned_start = false;
-        {
-            std::lock_guard<std::mutex> lock(lifecycle_mutex);
-            if (start_attempt != attempt || lifecycle != Lifecycle::STARTING) {
-                return;
-            }
-            attempt->completed = true;
-            attempt->succeeded = started;
-            if (started && registered && attempt->waiters != 0U) {
-                lifecycle = Lifecycle::RUNNING;
-            } else if (started) {
-                lifecycle = Lifecycle::STOPPING;
-                ring.close();
-                stop_abandoned_start = true;
-            } else {
-                lifecycle = Lifecycle::STOPPED;
-            }
-            lifecycle_condition.notify_all();
-        }
-
-        if (stop_abandoned_start) {
-            invokeStop();
-            std::lock_guard<std::mutex> lock(lifecycle_mutex);
-            if (lifecycle == Lifecycle::STOPPING) {
-                lifecycle = Lifecycle::STOPPED;
-            }
-            lifecycle_condition.notify_all();
-        }
-    }
-
-    bool acquire(
-        const StartPosition start_position,
-        FrameRing::Cursor& cursor,
-        const CancelPredicate& cancelled) {
-        std::unique_lock<std::mutex> lock(lifecycle_mutex);
-        while (lifecycle == Lifecycle::STOPPING) {
-            if (!registered || isCancelled(cancelled)) return false;
-            lifecycle_condition.wait_for(lock, std::chrono::milliseconds(10));
-        }
-        if (!registered || isCancelled(cancelled)) return false;
-
-        if (lifecycle == Lifecycle::RUNNING) {
-            ++subscriber_count;
-            lock.unlock();
-            cursor = ring.makeCursor(start_position);
-            return true;
-        }
-
-        std::shared_ptr<StartAttempt> attempt;
-        if (lifecycle == Lifecycle::STOPPED) {
-            lifecycle = Lifecycle::STARTING;
-            ring.reset();
-            const FrameSink sink = makeSink();
-            attempt = std::make_shared<StartAttempt>();
-            attempt->waiters = 1U;
-            start_attempt = attempt;
-            const auto self = shared_from_this();
-            try {
-                std::thread([self, attempt, sink]() {
-                    const CancelPredicate cancelled = [attempt] {
-                        return attempt->cancel_requested.load(
-                            std::memory_order_acquire);
-                    };
-                    const bool started = self->invokeStart(sink, cancelled);
-                    self->completeStart(attempt, started);
-                }).detach();
-            } catch (...) {
-                start_attempt.reset();
-                lifecycle = Lifecycle::STOPPED;
-                lifecycle_condition.notify_all();
-                return false;
-            }
-        } else if (lifecycle == Lifecycle::STARTING) {
-            attempt = start_attempt;
-            if (!attempt || attempt->cancel_requested.load(std::memory_order_acquire)) {
-                return false;
-            }
-            ++attempt->waiters;
-        } else {
-            return false;
-        }
-
-        while (registered && !attempt->completed) {
-            if (isCancelled(cancelled)) {
-                if (attempt->waiters != 0U) --attempt->waiters;
-                if (attempt->waiters == 0U) {
-                    attempt->cancel_requested.store(true, std::memory_order_release);
-                }
-                lifecycle_condition.notify_all();
-                return false;
-            }
-            lifecycle_condition.wait_for(lock, std::chrono::milliseconds(10));
-        }
-
-        const bool caller_cancelled = isCancelled(cancelled);
-        const bool acquired = !caller_cancelled && registered && attempt->completed &&
-                              attempt->succeeded &&
-                              lifecycle == Lifecycle::RUNNING;
-        if (attempt->waiters != 0U) --attempt->waiters;
-        if (!acquired) {
-            if (attempt->waiters == 0U) {
-                attempt->cancel_requested.store(true, std::memory_order_release);
-            }
-            const bool stop_unclaimed_source =
-                start_attempt == attempt && attempt->waiters == 0U &&
-                subscriber_count == 0U &&
-                lifecycle == Lifecycle::RUNNING;
-            if (stop_unclaimed_source) {
-                lifecycle = Lifecycle::STOPPING;
-                ring.close();
-                lock.unlock();
-                invokeStop();
-                lock.lock();
-                if (lifecycle == Lifecycle::STOPPING) {
-                    lifecycle = Lifecycle::STOPPED;
-                }
-                lifecycle_condition.notify_all();
-            }
-            return false;
-        }
-        ++subscriber_count;
-        lock.unlock();
-        cursor = ring.makeCursor(start_position);
-        return true;
-    }
-
-    void release() {
-        std::unique_lock<std::mutex> lock(lifecycle_mutex);
-        if (subscriber_count == 0) {
-            return;
-        }
-        --subscriber_count;
-        if (subscriber_count != 0 || lifecycle != Lifecycle::RUNNING) {
-            return;
-        }
-
-        lifecycle = Lifecycle::STOPPING;
-        ring.close();
-        lock.unlock();
-        invokeStop();
-        lock.lock();
-        lifecycle = Lifecycle::STOPPED;
-        lifecycle_condition.notify_all();
-    }
-
-    bool deactivateIfUnused() {
-        std::unique_lock<std::mutex> lock(lifecycle_mutex);
-        if (subscriber_count != 0) {
-            return false;
-        }
-        registered = false;
-        ring.close();
-        if (lifecycle == Lifecycle::STARTING) {
-            if (start_attempt) {
-                start_attempt->cancel_requested.store(true, std::memory_order_release);
-            }
-            lifecycle_condition.notify_all();
-            return true;
-        }
-        if (lifecycle == Lifecycle::STOPPING || lifecycle == Lifecycle::STOPPED) {
-            lifecycle_condition.notify_all();
-            return true;
-        }
-
-        lifecycle = Lifecycle::STOPPING;
-        lock.unlock();
-        invokeStop();
-        lock.lock();
-        if (lifecycle == Lifecycle::STOPPING) {
-            lifecycle = Lifecycle::STOPPED;
-        }
-        lifecycle_condition.notify_all();
-        return true;
-    }
-
-    void shutdown() {
-        std::unique_lock<std::mutex> lock(lifecycle_mutex);
-        registered = false;
-        ring.close();
-        if (lifecycle == Lifecycle::STARTING) {
-            if (start_attempt) {
-                start_attempt->cancel_requested.store(true, std::memory_order_release);
-            }
-            lifecycle_condition.notify_all();
-            return;
-        }
-        if (lifecycle == Lifecycle::STOPPING) {
-            lifecycle_condition.notify_all();
-            return;
-        }
-        if (lifecycle == Lifecycle::STOPPED) {
-            lifecycle_condition.notify_all();
-            return;
-        }
-
-        lifecycle = Lifecycle::STOPPING;
-        lock.unlock();
-        invokeStop();
-        lock.lock();
-        if (lifecycle == Lifecycle::STOPPING) {
-            lifecycle = Lifecycle::STOPPED;
-        }
-        lifecycle_condition.notify_all();
-    }
-
-    bool validForSubscription() const {
-        std::lock_guard<std::mutex> lock(lifecycle_mutex);
-        return registered && lifecycle == Lifecycle::RUNNING;
-    }
-
-    size_t subscriberCount() const {
-        std::lock_guard<std::mutex> lock(lifecycle_mutex);
-        return subscriber_count;
-    }
-
-    bool requestKeyFrame() const {
-        // Serialize with stop first, then re-check lifecycle. A stop that has
-        // already begun rejects the request; a stop that begins afterwards
-        // waits for this callback before invoking the device stop barrier.
-        std::lock_guard<std::mutex> callback_lock(callback_mutex);
-        {
-            std::lock_guard<std::mutex> lock(lifecycle_mutex);
-            if (!registered || lifecycle != Lifecycle::RUNNING || !callbacks.request_key_frame) {
-                return false;
-            }
-        }
-        try {
-            return callbacks.request_key_frame();
-        } catch (...) {
-            return false;
-        }
-    }
-
-    TrackDescriptorPtr currentDescriptor() const {
-        return std::atomic_load(&descriptor);
-    }
-
-    const std::string track_id;
-    mutable TrackDescriptorPtr descriptor;
-    const SourceCallbacks callbacks;
-    FrameRing ring;
-
-    mutable std::mutex callback_mutex;
-    mutable std::mutex lifecycle_mutex;
-    std::condition_variable lifecycle_condition;
-    Lifecycle lifecycle{Lifecycle::STOPPED};
-    size_t subscriber_count{0};
-    bool registered{true};
-    std::shared_ptr<StartAttempt> start_attempt;
-};
-
-struct MediaSourceHub::Impl final {
-    mutable std::mutex mutex;
-    std::unordered_map<std::string, std::shared_ptr<SourceState>> sources;
-};
-
-MediaSourceHub::Subscription::Subscription(
-    std::shared_ptr<SourceState> source,
-    FrameRing::Cursor cursor)
-    : source_(std::move(source)),
-      cursor_(std::move(cursor)),
-      active_(static_cast<bool>(source_)) {}
-
-MediaSourceHub::Subscription::~Subscription() {
-    reset();
-}
-
-MediaSourceHub::Subscription::Subscription(Subscription&& other) noexcept
-    : source_(std::move(other.source_)),
-      cursor_(other.cursor_),
-      active_(other.active_) {
-    other.active_ = false;
-}
-
-MediaSourceHub::Subscription& MediaSourceHub::Subscription::operator=(Subscription&& other) noexcept {
-    if (this == &other) {
-        return *this;
-    }
-    reset();
-    source_ = std::move(other.source_);
-    cursor_ = other.cursor_;
-    active_ = other.active_;
-    other.active_ = false;
-    return *this;
-}
-
-bool MediaSourceHub::Subscription::valid() const {
-    return active_ && source_ && source_->validForSubscription();
-}
-
-TrackDescriptorPtr MediaSourceHub::Subscription::descriptor() const {
-    return source_ ? source_->currentDescriptor() : nullptr;
-}
-
-std::optional<MediaSourceHub::FrameReadResult> MediaSourceHub::Subscription::tryRead() {
-    if (!active_ || !source_) {
-        return std::nullopt;
-    }
-    return source_->ring.tryRead(cursor_);
-}
-
-std::optional<MediaSourceHub::FrameReadResult> MediaSourceHub::Subscription::waitRead(
-    const std::chrono::milliseconds timeout) {
-    if (!active_ || !source_) {
-        return std::nullopt;
-    }
-    return source_->ring.waitRead(cursor_, timeout);
-}
-
-uint64_t MediaSourceHub::Subscription::discardPendingIfExceeds(
-    const size_t maximum_pending_frames) {
-    if (!active_ || !source_) {
-        return 0;
-    }
-    return source_->ring.discardPendingIfExceeds(cursor_, maximum_pending_frames);
-}
-
-uint64_t MediaSourceHub::Subscription::droppedCount() const noexcept {
-    return cursor_.dropped_count;
-}
-
-void MediaSourceHub::Subscription::reset() {
-    if (active_ && source_) {
-        source_->release();
-    }
-    active_ = false;
-    source_.reset();
-}
-
-MediaSourceHub::MediaSourceHub()
-    : impl_(std::make_shared<Impl>()) {}
-
-MediaSourceHub::~MediaSourceHub() {
-    shutdown();
-}
-
-bool MediaSourceHub::registerSource(
-    TrackDescriptorPtr initial_descriptor,
-    SourceCallbacks callbacks,
-    const size_t ring_capacity) {
-    if (!impl_ || !initial_descriptor || initial_descriptor->id.empty() ||
-        !callbacks.start || ring_capacity == 0) {
-        return false;
-    }
-
-    std::shared_ptr<SourceState> source;
-    try {
-        source = std::make_shared<SourceState>(
-            std::move(initial_descriptor), std::move(callbacks), ring_capacity);
-    } catch (...) {
-        return false;
-    }
-
-    std::lock_guard<std::mutex> lock(impl_->mutex);
-    return impl_->sources.emplace(source->track_id, std::move(source)).second;
-}
-
-bool MediaSourceHub::unregisterSource(const std::string& track_id) {
-    if (!impl_ || track_id.empty()) {
-        return false;
-    }
-
-    std::shared_ptr<SourceState> source;
-    {
-        std::lock_guard<std::mutex> lock(impl_->mutex);
-        const auto it = impl_->sources.find(track_id);
-        if (it == impl_->sources.end()) {
-            return false;
-        }
-        source = it->second;
-    }
-
-    if (!source->deactivateIfUnused()) {
-        return false;
-    }
-
-    std::lock_guard<std::mutex> lock(impl_->mutex);
-    const auto it = impl_->sources.find(track_id);
-    if (it != impl_->sources.end() && it->second == source) {
-        impl_->sources.erase(it);
-        return true;
-    }
-    return false;
-}
-
-bool MediaSourceHub::hasSource(const std::string& track_id) const {
-    if (!impl_) {
-        return false;
-    }
-    std::lock_guard<std::mutex> lock(impl_->mutex);
-    return impl_->sources.find(track_id) != impl_->sources.end();
-}
-
-std::vector<TrackDescriptorPtr> MediaSourceHub::listTracks() const {
-    std::vector<std::shared_ptr<SourceState>> sources;
-    if (!impl_) {
-        return {};
-    }
-    {
-        std::lock_guard<std::mutex> lock(impl_->mutex);
-        sources.reserve(impl_->sources.size());
-        for (const auto& [track_id, source] : impl_->sources) {
-            (void)track_id;
-            sources.push_back(source);
-        }
-    }
-
-    std::vector<TrackDescriptorPtr> descriptors;
-    descriptors.reserve(sources.size());
-    for (const auto& source : sources) {
-        descriptors.push_back(source->currentDescriptor());
-    }
-    std::sort(descriptors.begin(), descriptors.end(), [](const auto& lhs, const auto& rhs) {
-        if (!lhs) return static_cast<bool>(rhs);
-        if (!rhs) return false;
-        return lhs->id < rhs->id;
-    });
-    return descriptors;
-}
-
-size_t MediaSourceHub::subscriberCount(const std::string& track_id) const {
-    if (!impl_) {
-        return 0;
-    }
-    std::shared_ptr<SourceState> source;
-    {
-        std::lock_guard<std::mutex> lock(impl_->mutex);
-        const auto it = impl_->sources.find(track_id);
-        if (it == impl_->sources.end()) {
-            return 0;
-        }
-        source = it->second;
-    }
-    return source->subscriberCount();
-}
-
-bool MediaSourceHub::requestKeyFrame(const std::string& track_id) const {
-    if (!impl_) {
-        return false;
-    }
-    std::shared_ptr<SourceState> source;
-    {
-        std::lock_guard<std::mutex> lock(impl_->mutex);
-        const auto it = impl_->sources.find(track_id);
-        if (it == impl_->sources.end()) {
-            return false;
-        }
-        source = it->second;
-    }
-    return source->requestKeyFrame();
-}
-
-MediaSourceHub::Subscription MediaSourceHub::subscribe(
-    const std::string& track_id,
-    const StartPosition start_position,
-    CancelPredicate cancelled) {
-    if (!impl_) {
-        return {};
-    }
-
-    std::shared_ptr<SourceState> source;
-    {
-        std::lock_guard<std::mutex> lock(impl_->mutex);
-        const auto it = impl_->sources.find(track_id);
-        if (it == impl_->sources.end()) {
-            return {};
-        }
-        source = it->second;
-    }
-
-    FrameRing::Cursor cursor;
-    if (!source->acquire(start_position, cursor, cancelled)) {
-        return {};
-    }
-    return Subscription(std::move(source), std::move(cursor));
-}
-
-void MediaSourceHub::shutdown() {
-    if (!impl_) {
-        return;
-    }
-
-    std::vector<std::shared_ptr<SourceState>> sources;
-    {
-        std::lock_guard<std::mutex> lock(impl_->mutex);
-        sources.reserve(impl_->sources.size());
-        for (auto& [track_id, source] : impl_->sources) {
-            (void)track_id;
-            sources.push_back(std::move(source));
-        }
-        impl_->sources.clear();
-    }
-    for (const auto& source : sources) {
-        source->shutdown();
-    }
-}
-
-} // namespace cmvr::media

cmvr-es/manager/media_source_hub/tests/media_source_hub_test.cpp

@@ -1,683 +0,0 @@
-#include "manager/media_source_hub/include/media_source_hub.h"
-
-#include <atomic>
-#include <chrono>
-#include <future>
-#include <iostream>
-#include <mutex>
-#include <stdexcept>
-#include <string>
-#include <thread>
-#include <type_traits>
-#include <utility>
-#include <vector>
-
-namespace {
-
-using namespace std::chrono_literals;
-using cmvr::media::Codec;
-using cmvr::media::MediaFrame;
-using cmvr::media::MediaFramePtr;
-using cmvr::media::MediaKind;
-using cmvr::media::MediaSourceHub;
-using cmvr::media::PayloadFormat;
-using cmvr::media::Rational;
-using cmvr::media::TrackDescriptor;
-using cmvr::media::TrackDescriptorPtr;
-
-static_assert(!std::is_copy_assignable<MediaFrame>::value, "MediaFrame must be immutable");
-static_assert(!std::is_copy_assignable<TrackDescriptor>::value, "TrackDescriptor must be immutable");
-static_assert(std::is_same<MediaFramePtr::element_type, const MediaFrame>::value,
-              "MediaFramePtr must share const frames");
-
-int failures = 0;
-
-#define CHECK_TRUE(expression)                                                                  \
-    do {                                                                                        \
-        if (!(expression)) {                                                                    \
-            std::cerr << __FILE__ << ':' << __LINE__ << " check failed: " #expression << '\n'; \
-            ++failures;                                                                         \
-        }                                                                                       \
-    } while (false)
-
-TrackDescriptorPtr makeVideoDescriptor(
-    const Codec codec,
-    const uint64_t generation,
-    std::vector<uint8_t> codec_config = {}) {
-    TrackDescriptor::Config config;
-    config.id = "camera.front.video";
-    config.source_id = "camera.front";
-    config.kind = MediaKind::VIDEO;
-    config.codec = codec;
-    config.payload_format = codec == Codec::UNKNOWN ? PayloadFormat::UNKNOWN : PayloadFormat::ANNEX_B;
-    config.time_base = Rational{1, 90000};
-    config.width = 640;
-    config.height = 360;
-    config.nominal_rate = 30;
-    config.generation = generation;
-    config.codec_config = std::move(codec_config);
-    return cmvr::media::makeTrackDescriptor(std::move(config));
-}
-
-MediaFramePtr makeFrame(
-    TrackDescriptorPtr descriptor,
-    const uint64_t sequence,
-    const uint8_t marker) {
-    MediaFrame::Config config;
-    config.descriptor = std::move(descriptor);
-    config.payload = {marker, static_cast<uint8_t>(marker + 1)};
-    config.sequence = sequence;
-    config.pts = static_cast<int64_t>(sequence * 3000);
-    config.dts = config.pts;
-    config.duration = 3000;
-    config.capture_time_ns = sequence * 1000000;
-    config.capture_utc_ns = 1700000000000000000LL + static_cast<int64_t>(sequence);
-    config.key_frame = sequence == 0;
-    return cmvr::media::makeMediaFrame(std::move(config));
-}
-
-void testMediaMetadataValidation() {
-    TrackDescriptor::Config invalid;
-    invalid.id = "invalid.video";
-    invalid.source_id = "invalid";
-    invalid.kind = MediaKind::VIDEO;
-    invalid.time_base = Rational{0, 1};
-    bool rejected = false;
-    try {
-        (void)cmvr::media::makeTrackDescriptor(std::move(invalid));
-    } catch (const std::invalid_argument&) {
-        rejected = true;
-    }
-    CHECK_TRUE(rejected);
-
-    const auto frame = makeFrame(makeVideoDescriptor(Codec::H264, 1), 7, 1);
-    CHECK_TRUE(frame->capture_time_ns == 7000000);
-    CHECK_TRUE(frame->capture_utc_ns == 1700000000000000007LL);
-    CHECK_TRUE(frame->duration == 3000);
-}
-
-void testLegacySpmcCompatibility() {
-    bool ring_zero_capacity_rejected = false;
-    try {
-        RingBuffer<int> invalid_ring(0);
-    } catch (const std::invalid_argument&) {
-        ring_zero_capacity_rejected = true;
-    }
-    CHECK_TRUE(ring_zero_capacity_rejected);
-
-    bool spmc_zero_capacity_rejected = false;
-    try {
-        SPMCRingBuffer<int> invalid_ring(0);
-    } catch (const std::invalid_argument&) {
-        spmc_zero_capacity_rejected = true;
-    }
-    CHECK_TRUE(spmc_zero_capacity_rejected);
-
-    SPMCRingBuffer<int> ring(2);
-    ring.push(10);
-    ring.push(20);
-    CHECK_TRUE(ring.size() == 2);
-    CHECK_TRUE(ring.getHead() == 2);
-    CHECK_TRUE(ring.getTail() == 0);
-    CHECK_TRUE(ring.getLast().has_value() && *ring.getLast() == 20);
-
-    size_t reader = 0;
-    CHECK_TRUE(ring.pop(reader).has_value());
-    ring.push(30);
-    ring.push(40);
-    CHECK_TRUE(!ring.pop(reader).has_value());
-    CHECK_TRUE(reader == ring.getTail());
-    CHECK_TRUE(ring.pop(reader).has_value());
-
-    ring.clear();
-    CHECK_TRUE(ring.empty());
-    CHECK_TRUE(ring.getHead() == 4);
-    ring.push(50);
-    CHECK_TRUE(!ring.pop(reader).has_value());
-    const auto after_clear = ring.pop(reader);
-    CHECK_TRUE(after_clear.has_value() && *after_clear == 50);
-}
-
-void testBroadcastFrameRing() {
-    using Ring = BroadcastFrameRing<MediaFrame>;
-    Ring ring(2);
-    const auto descriptor = makeVideoDescriptor(Codec::H264, 1, {1, 2, 3});
-    auto cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE);
-
-    CHECK_TRUE(ring.publish(makeFrame(descriptor, 0, 10)).value() == 0);
-    CHECK_TRUE(ring.publish(makeFrame(descriptor, 1, 20)).value() == 1);
-    CHECK_TRUE(ring.publish(makeFrame(descriptor, 2, 30)).value() == 2);
-
-    const auto first = ring.tryRead(cursor);
-    CHECK_TRUE(first.has_value());
-    CHECK_TRUE(first->sequence == 1);
-    CHECK_TRUE(first->value->sequence == 1);
-    CHECK_TRUE(first->dropped_count == 1);
-    CHECK_TRUE(first->dropped_since_last_read == 1);
-    CHECK_TRUE(ring.stats().dropped_count == 1);
-
-    const auto second = ring.tryRead(cursor);
-    CHECK_TRUE(second.has_value() && second->sequence == 2);
-    CHECK_TRUE(second->dropped_since_last_read == 0);
-
-    auto waiting_cursor = ring.makeCursor(Ring::StartPosition::NEXT_PUBLISHED);
-    auto waiting_read = std::async(std::launch::async, [&ring, &waiting_cursor] {
-        return ring.waitRead(waiting_cursor, 1s);
-    });
-    std::this_thread::sleep_for(10ms);
-    ring.publish(makeFrame(descriptor, 3, 40));
-    CHECK_TRUE(waiting_read.wait_for(500ms) == std::future_status::ready);
-    CHECK_TRUE(waiting_read.get().has_value());
-
-    const uint64_t next_generation = ring.reset();
-    CHECK_TRUE(next_generation == 2);
-    ring.publish(makeFrame(descriptor, 4, 50));
-    const auto after_reset = ring.tryRead(cursor);
-    CHECK_TRUE(after_reset.has_value());
-    CHECK_TRUE(after_reset->generation == 2);
-    CHECK_TRUE(after_reset->sequence == 0);
-    CHECK_TRUE(after_reset->generation_changed);
-
-    auto close_cursor = ring.makeCursor(Ring::StartPosition::NEXT_PUBLISHED);
-    auto close_wait = std::async(std::launch::async, [&ring, &close_cursor] {
-        return ring.waitRead(close_cursor, 2s);
-    });
-    ring.close();
-    CHECK_TRUE(close_wait.wait_for(500ms) == std::future_status::ready);
-    CHECK_TRUE(!close_wait.get().has_value());
-    CHECK_TRUE(!ring.publish(makeFrame(descriptor, 5, 60)).has_value());
-}
-
-void testBroadcastDiscardPending() {
-    using Ring = BroadcastFrameRing<MediaFrame>;
-    const auto descriptor = makeVideoDescriptor(Codec::H264, 1, {1, 2, 3});
-
-    {
-        Ring ring(8);
-        auto cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE);
-        ring.publish(makeFrame(descriptor, 0, 10));
-        ring.publish(makeFrame(descriptor, 1, 20));
-
-        CHECK_TRUE(ring.discardPendingIfExceeds(cursor, 2) == 0);
-        const auto first = ring.tryRead(cursor);
-        CHECK_TRUE(first.has_value());
-        CHECK_TRUE(first->sequence == 0);
-        CHECK_TRUE(first->dropped_since_last_read == 0);
-    }
-
-    {
-        Ring ring(8);
-        auto cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE);
-        ring.publish(makeFrame(descriptor, 0, 10));
-        ring.publish(makeFrame(descriptor, 1, 20));
-        ring.publish(makeFrame(descriptor, 2, 30));
-
-        CHECK_TRUE(ring.discardPendingIfExceeds(cursor, 2) == 3);
-        CHECK_TRUE(!ring.tryRead(cursor).has_value());
-
-        ring.publish(makeFrame(descriptor, 3, 40));
-        const auto after_discard = ring.tryRead(cursor);
-        CHECK_TRUE(after_discard.has_value());
-        CHECK_TRUE(after_discard->sequence == 3);
-        CHECK_TRUE(after_discard->dropped_count == 3);
-        CHECK_TRUE(after_discard->dropped_since_last_read == 3);
-    }
-
-    {
-        // Two frames are overwritten before the explicit three-frame discard.
-        // Both kinds of loss must be reported by the next successful read.
-        Ring ring(3);
-        auto cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE);
-        for (uint64_t sequence = 0; sequence < 5; ++sequence) {
-            ring.publish(makeFrame(
-                descriptor,
-                sequence,
-                static_cast<uint8_t>(sequence)));
-        }
-
-        CHECK_TRUE(ring.discardPendingIfExceeds(cursor, 2) == 3);
-        CHECK_TRUE(cursor.dropped_count == 5);
-        ring.publish(makeFrame(descriptor, 5, 50));
-        const auto after_overwrite_and_discard = ring.tryRead(cursor);
-        CHECK_TRUE(after_overwrite_and_discard.has_value());
-        CHECK_TRUE(after_overwrite_and_discard->sequence == 5);
-        CHECK_TRUE(after_overwrite_and_discard->dropped_count == 5);
-        CHECK_TRUE(after_overwrite_and_discard->dropped_since_last_read == 5);
-    }
-
-    {
-        // An old-generation OLDEST_AVAILABLE cursor adopts the reset generation
-        // before deciding whether that generation's pending frames are excessive.
-        Ring ring(4);
-        auto cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE);
-        ring.publish(makeFrame(descriptor, 0, 10));
-        ring.reset();
-        ring.publish(makeFrame(descriptor, 1, 20));
-        ring.publish(makeFrame(descriptor, 2, 30));
-
-        CHECK_TRUE(ring.discardPendingIfExceeds(cursor, 1) == 2);
-        ring.publish(makeFrame(descriptor, 3, 40));
-        const auto after_reset = ring.tryRead(cursor);
-        CHECK_TRUE(after_reset.has_value());
-        CHECK_TRUE(after_reset->generation == 2);
-        CHECK_TRUE(after_reset->sequence == 2);
-        CHECK_TRUE(after_reset->generation_changed);
-        CHECK_TRUE(after_reset->dropped_since_last_read == 2);
-    }
-}
-
-void testBroadcastDiscardConcurrentPublish() {
-    using Ring = BroadcastFrameRing<int>;
-    constexpr uint64_t frame_count = 4000;
-    Ring ring(64);
-    auto cursor = ring.makeCursor(Ring::StartPosition::NEXT_PUBLISHED);
-    std::atomic<bool> start{false};
-    std::atomic<bool> publisher_done{false};
-
-    std::thread publisher([&] {
-        while (!start.load(std::memory_order_acquire)) {
-            std::this_thread::yield();
-        }
-        for (uint64_t sequence = 0; sequence < frame_count; ++sequence) {
-            ring.publish(std::make_shared<const int>(static_cast<int>(sequence)));
-            if ((sequence & 7U) == 0U) {
-                std::this_thread::yield();
-            }
-        }
-        publisher_done.store(true, std::memory_order_release);
-    });
-
-    uint64_t read_count = 0;
-    uint64_t actively_discarded = 0;
-    start.store(true, std::memory_order_release);
-    while (true) {
-        actively_discarded += ring.discardPendingIfExceeds(cursor, 8);
-        if (ring.tryRead(cursor)) {
-            ++read_count;
-            continue;
-        }
-        if (publisher_done.load(std::memory_order_acquire)) {
-            actively_discarded += ring.discardPendingIfExceeds(cursor, 8);
-            if (ring.tryRead(cursor)) {
-                ++read_count;
-                continue;
-            }
-            break;
-        }
-        std::this_thread::yield();
-    }
-    publisher.join();
-
-    CHECK_TRUE(read_count + cursor.dropped_count == frame_count);
-    CHECK_TRUE(actively_discarded <= cursor.dropped_count);
-}
-
-void testBroadcastConcurrency() {
-    using Ring = BroadcastFrameRing<MediaFrame>;
-    constexpr uint64_t frame_count = 500;
-    Ring ring(frame_count);
-    const auto descriptor = makeVideoDescriptor(Codec::H264, 1, {1, 2, 3});
-    auto first_cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE);
-    auto second_cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE);
-
-    auto consume = [&ring](Ring::Cursor& cursor) {
-        uint64_t expected = 0;
-        while (expected < frame_count) {
-            const auto result = ring.waitRead(cursor, 1s);
-            if (!result || result->sequence != expected || result->value->sequence != expected) {
-                return false;
-            }
-            ++expected;
-        }
-        return cursor.dropped_count == 0;
-    };
-
-    auto first_consumer = std::async(std::launch::async, consume, std::ref(first_cursor));
-    auto second_consumer = std::async(std::launch::async, consume, std::ref(second_cursor));
-    std::thread producer([&ring, &descriptor] {
-        for (uint64_t sequence = 0; sequence < frame_count; ++sequence) {
-            ring.publish(makeFrame(descriptor, sequence, static_cast<uint8_t>(sequence)));
-        }
-    });
-
-    producer.join();
-    CHECK_TRUE(first_consumer.get());
-    CHECK_TRUE(second_consumer.get());
-}
-
-void testHubLifecycleAndDescriptorRefresh() {
-    MediaSourceHub hub;
-    const auto initial_descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1);
-    std::atomic<int> start_count{0};
-    std::atomic<int> stop_count{0};
-    std::atomic<int> key_frame_requests{0};
-    std::mutex sink_mutex;
-    MediaSourceHub::FrameSink sink;
-
-    MediaSourceHub::SourceCallbacks callbacks;
-    callbacks.start = [&](const MediaSourceHub::FrameSink& callback_sink,
-                          const MediaSourceHub::CancelPredicate&) {
-        {
-            std::lock_guard<std::mutex> lock(sink_mutex);
-            sink = callback_sink;
-        }
-        ++start_count;
-        return true;
-    };
-    callbacks.stop = [&] {
-        ++stop_count;
-        std::lock_guard<std::mutex> lock(sink_mutex);
-        sink = {};
-    };
-    callbacks.request_key_frame = [&] {
-        ++key_frame_requests;
-        return true;
-    };
-
-    CHECK_TRUE(hub.registerSource(initial_descriptor, callbacks, 4));
-    CHECK_TRUE(!hub.registerSource(initial_descriptor, callbacks, 4));
-    CHECK_TRUE(hub.hasSource(initial_descriptor->id));
-    CHECK_TRUE(hub.listTracks().size() == 1);
-
-    auto first = hub.subscribe(initial_descriptor->id);
-    auto second = hub.subscribe(initial_descriptor->id);
-    CHECK_TRUE(first.valid() && second.valid());
-    CHECK_TRUE(hub.requestKeyFrame(initial_descriptor->id));
-    CHECK_TRUE(key_frame_requests == 1);
-    CHECK_TRUE(start_count == 1);
-    CHECK_TRUE(hub.subscriberCount(initial_descriptor->id) == 2);
-    CHECK_TRUE(first.descriptor()->codec == Codec::UNKNOWN);
-    CHECK_TRUE(!hub.unregisterSource(initial_descriptor->id));
-
-    // Content changes at the same generation must atomically replace the initial descriptor.
-    const auto actual_descriptor = makeVideoDescriptor(Codec::H264, 1, {0, 0, 0, 1, 0x67});
-    MediaSourceHub::FrameSink producer;
-    {
-        std::lock_guard<std::mutex> lock(sink_mutex);
-        producer = sink;
-    }
-    CHECK_TRUE(static_cast<bool>(producer));
-    const auto shared_frame = makeFrame(actual_descriptor, 0, 70);
-    producer(shared_frame);
-
-    const auto first_read = first.waitRead(500ms);
-    const auto second_read = second.waitRead(500ms);
-    CHECK_TRUE(first_read.has_value() && first_read->value == shared_frame);
-    CHECK_TRUE(second_read.has_value() && second_read->value == shared_frame);
-    CHECK_TRUE(first.descriptor() == actual_descriptor);
-    CHECK_TRUE(second.descriptor()->codec_config == actual_descriptor->codec_config);
-
-    first.reset();
-    CHECK_TRUE(stop_count == 0);
-    CHECK_TRUE(hub.subscriberCount(initial_descriptor->id) == 1);
-    second.reset();
-    CHECK_TRUE(stop_count == 1);
-    CHECK_TRUE(!hub.requestKeyFrame(initial_descriptor->id));
-    CHECK_TRUE(hub.subscriberCount(initial_descriptor->id) == 0);
-
-    {
-        auto restarted = hub.subscribe(initial_descriptor->id);
-        CHECK_TRUE(restarted.valid());
-        CHECK_TRUE(start_count == 2);
-    }
-    CHECK_TRUE(stop_count == 2);
-    CHECK_TRUE(hub.unregisterSource(initial_descriptor->id));
-    CHECK_TRUE(!hub.hasSource(initial_descriptor->id));
-}
-
-void testSubscriptionDiscardPending() {
-    MediaSourceHub hub;
-    const auto descriptor = makeVideoDescriptor(Codec::H264, 1);
-    std::mutex sink_mutex;
-    MediaSourceHub::FrameSink sink;
-
-    MediaSourceHub::SourceCallbacks callbacks;
-    callbacks.start = [&](const MediaSourceHub::FrameSink& callback_sink,
-                          const MediaSourceHub::CancelPredicate&) {
-        std::lock_guard<std::mutex> lock(sink_mutex);
-        sink = callback_sink;
-        return true;
-    };
-    callbacks.stop = [&] {
-        std::lock_guard<std::mutex> lock(sink_mutex);
-        sink = {};
-    };
-
-    CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 8));
-    auto subscription = hub.subscribe(descriptor->id);
-    CHECK_TRUE(subscription.valid());
-
-    MediaSourceHub::FrameSink producer;
-    {
-        std::lock_guard<std::mutex> lock(sink_mutex);
-        producer = sink;
-    }
-    CHECK_TRUE(static_cast<bool>(producer));
-    producer(makeFrame(descriptor, 0, 10));
-    producer(makeFrame(descriptor, 1, 20));
-    producer(makeFrame(descriptor, 2, 30));
-
-    CHECK_TRUE(subscription.discardPendingIfExceeds(2) == 3);
-    CHECK_TRUE(!subscription.tryRead().has_value());
-    producer(makeFrame(descriptor, 3, 40));
-    const auto next = subscription.tryRead();
-    CHECK_TRUE(next.has_value());
-    CHECK_TRUE(next->value->sequence == 3);
-    CHECK_TRUE(next->dropped_since_last_read == 3);
-    CHECK_TRUE(subscription.droppedCount() == 3);
-}
-
-void testHubFailedStartAndShutdown() {
-    MediaSourceHub hub;
-    const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1);
-    std::atomic<int> start_attempts{0};
-    std::atomic<int> retry_stop_count{0};
-    MediaSourceHub::SourceCallbacks failed_callbacks;
-    failed_callbacks.start = [&](const MediaSourceHub::FrameSink&,
-                                 const MediaSourceHub::CancelPredicate&) {
-        return ++start_attempts >= 2;
-    };
-    failed_callbacks.stop = [&] { ++retry_stop_count; };
-    CHECK_TRUE(hub.registerSource(descriptor, std::move(failed_callbacks), 2));
-    auto failed = hub.subscribe(descriptor->id);
-    CHECK_TRUE(!failed.valid());
-    auto retry = hub.subscribe(descriptor->id);
-    CHECK_TRUE(retry.valid());
-    retry.reset();
-    CHECK_TRUE(start_attempts == 2);
-    CHECK_TRUE(retry_stop_count == 1);
-    CHECK_TRUE(hub.unregisterSource(descriptor->id));
-
-    std::atomic<int> stop_count{0};
-    MediaSourceHub::SourceCallbacks callbacks;
-    callbacks.start = [](const MediaSourceHub::FrameSink&,
-                         const MediaSourceHub::CancelPredicate&) {
-        return true;
-    };
-    callbacks.stop = [&] { ++stop_count; };
-    CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 2));
-    auto live = hub.subscribe(descriptor->id);
-    CHECK_TRUE(live.valid());
-    hub.shutdown();
-    CHECK_TRUE(stop_count == 1);
-    CHECK_TRUE(!live.valid());
-    CHECK_TRUE(!live.waitRead(50ms).has_value());
-}
-
-void testKeyFrameRequestIsOrderedBeforeStop() {
-    MediaSourceHub hub;
-    const auto descriptor = makeVideoDescriptor(Codec::H264, 1);
-    std::atomic<bool> key_frame_entered{false};
-    std::atomic<bool> release_key_frame{false};
-    std::atomic<int> stop_count{0};
-
-    MediaSourceHub::SourceCallbacks callbacks;
-    callbacks.start = [](const MediaSourceHub::FrameSink&,
-                         const MediaSourceHub::CancelPredicate&) {
-        return true;
-    };
-    callbacks.stop = [&] { ++stop_count; };
-    callbacks.request_key_frame = [&] {
-        key_frame_entered.store(true, std::memory_order_release);
-        while (!release_key_frame.load(std::memory_order_acquire)) {
-            std::this_thread::sleep_for(1ms);
-        }
-        return true;
-    };
-    CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 2));
-    auto subscription = hub.subscribe(descriptor->id);
-    CHECK_TRUE(subscription.valid());
-
-    auto key_frame = std::async(std::launch::async, [&] {
-        return hub.requestKeyFrame(descriptor->id);
-    });
-    const auto enter_deadline = std::chrono::steady_clock::now() + 500ms;
-    while (!key_frame_entered.load(std::memory_order_acquire) &&
-           std::chrono::steady_clock::now() < enter_deadline) {
-        std::this_thread::sleep_for(1ms);
-    }
-    CHECK_TRUE(key_frame_entered.load(std::memory_order_acquire));
-
-    auto stop = std::async(std::launch::async, [&] { subscription.reset(); });
-    CHECK_TRUE(stop.wait_for(20ms) == std::future_status::timeout);
-    CHECK_TRUE(stop_count.load(std::memory_order_acquire) == 0);
-
-    release_key_frame.store(true, std::memory_order_release);
-    CHECK_TRUE(key_frame.get());
-    CHECK_TRUE(stop.wait_for(500ms) == std::future_status::ready);
-    stop.get();
-    CHECK_TRUE(stop_count.load(std::memory_order_acquire) == 1);
-    CHECK_TRUE(!hub.requestKeyFrame(descriptor->id));
-}
-
-void testHubCancelsBlockedStartWithoutBlockingShutdown() {
-    MediaSourceHub hub;
-    const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1);
-    std::atomic<bool> start_entered{false};
-    std::atomic<bool> start_exited{false};
-
-    MediaSourceHub::SourceCallbacks callbacks;
-    callbacks.start = [&](const MediaSourceHub::FrameSink&,
-                          const MediaSourceHub::CancelPredicate& cancelled) {
-        start_entered.store(true, std::memory_order_release);
-        while (!cancelled()) {
-            std::this_thread::sleep_for(2ms);
-        }
-        start_exited.store(true, std::memory_order_release);
-        return false;
-    };
-    CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 2));
-
-    auto subscription_future = std::async(std::launch::async, [&] {
-        return hub.subscribe(descriptor->id);
-    });
-    const auto start_deadline = std::chrono::steady_clock::now() + 500ms;
-    while (!start_entered.load(std::memory_order_acquire) &&
-           std::chrono::steady_clock::now() < start_deadline) {
-        std::this_thread::sleep_for(2ms);
-    }
-
-    auto shutdown_future = std::async(std::launch::async, [&] { hub.shutdown(); });
-    const bool shutdown_completed = shutdown_future.wait_for(500ms) ==
-                                    std::future_status::ready;
-    if (shutdown_completed) shutdown_future.get();
-    const bool subscribe_completed = subscription_future.wait_for(500ms) ==
-                                     std::future_status::ready;
-    bool invalid_subscription = false;
-    if (subscribe_completed) {
-        invalid_subscription = !subscription_future.get().valid();
-    }
-    const auto exit_deadline = std::chrono::steady_clock::now() + 500ms;
-    while (!start_exited.load(std::memory_order_acquire) &&
-           std::chrono::steady_clock::now() < exit_deadline) {
-        std::this_thread::sleep_for(2ms);
-    }
-
-    CHECK_TRUE(start_entered.load(std::memory_order_acquire));
-    CHECK_TRUE(shutdown_completed);
-    CHECK_TRUE(subscribe_completed);
-    CHECK_TRUE(invalid_subscription);
-    CHECK_TRUE(start_exited.load(std::memory_order_acquire));
-}
-
-void testHubQuarantinesNonCooperativeStart() {
-    MediaSourceHub hub;
-    const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1);
-    std::atomic<bool> start_entered{false};
-    std::atomic<bool> release_start{false};
-    std::atomic<bool> start_exited{false};
-    std::atomic<int> stop_count{0};
-
-    MediaSourceHub::SourceCallbacks callbacks;
-    callbacks.start = [&](const MediaSourceHub::FrameSink&,
-                          const MediaSourceHub::CancelPredicate&) {
-        start_entered.store(true, std::memory_order_release);
-        while (!release_start.load(std::memory_order_acquire)) {
-            std::this_thread::sleep_for(2ms);
-        }
-        start_exited.store(true, std::memory_order_release);
-        return true;
-    };
-    callbacks.stop = [&] { ++stop_count; };
-    CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 2));
-
-    auto subscription_future = std::async(std::launch::async, [&] {
-        return hub.subscribe(descriptor->id);
-    });
-    const auto start_deadline = std::chrono::steady_clock::now() + 500ms;
-    while (!start_entered.load(std::memory_order_acquire) &&
-           std::chrono::steady_clock::now() < start_deadline) {
-        std::this_thread::sleep_for(2ms);
-    }
-
-    const auto shutdown_started = std::chrono::steady_clock::now();
-    hub.shutdown();
-    const bool shutdown_was_bounded =
-        std::chrono::steady_clock::now() - shutdown_started < 500ms;
-    const bool subscribe_completed = subscription_future.wait_for(500ms) ==
-                                     std::future_status::ready;
-    bool invalid_subscription = false;
-    if (subscribe_completed) {
-        invalid_subscription = !subscription_future.get().valid();
-    }
-
-    // Release the deliberately non-cooperative test callback before its stack
-    // captures go out of scope. Late successful startup must be stopped once.
-    release_start.store(true, std::memory_order_release);
-    const auto exit_deadline = std::chrono::steady_clock::now() + 500ms;
-    while ((!start_exited.load(std::memory_order_acquire) || stop_count.load() != 1) &&
-           std::chrono::steady_clock::now() < exit_deadline) {
-        std::this_thread::sleep_for(2ms);
-    }
-
-    CHECK_TRUE(start_entered.load(std::memory_order_acquire));
-    CHECK_TRUE(shutdown_was_bounded);
-    CHECK_TRUE(subscribe_completed);
-    CHECK_TRUE(invalid_subscription);
-    CHECK_TRUE(start_exited.load(std::memory_order_acquire));
-    CHECK_TRUE(stop_count.load() == 1);
-}
-
-} // namespace
-
-int main() {
-    testMediaMetadataValidation();
-    testLegacySpmcCompatibility();
-    testBroadcastFrameRing();
-    testBroadcastDiscardPending();
-    testBroadcastDiscardConcurrentPublish();
-    testBroadcastConcurrency();
-    testHubLifecycleAndDescriptorRefresh();
-    testSubscriptionDiscardPending();
-    testHubFailedStartAndShutdown();
-    testKeyFrameRequestIsOrderedBeforeStop();
-    testHubCancelsBlockedStartWithoutBlockingShutdown();
-    testHubQuarantinesNonCooperativeStart();
-
-    if (failures != 0) {
-        std::cerr << failures << " media_source_hub checks failed\n";
-        return 1;
-    }
-    std::cout << "media_source_hub self-test passed\n";
-    return 0;
-}

cmvr-es/manager/task_manager/CMakeLists.txt

@@ -11,6 +11,7 @@ target_link_libraries(task_manager
         PRIVATE
         cmvr_es::common
         cmvr_es::device_manager
+        cmvr_es::stop_all_admission_gate
 )
 
 add_library(cmvr_es::task_manager ALIAS task_manager)

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

@@ -8,6 +8,7 @@
 #include <thread>
 #include <unordered_map>
 #include <atomic>
+#include <vector>
 
 #include "task/task.h"
 #include "task/touch_screen_task/include/touch_screen_task.h"
@@ -23,21 +24,31 @@ namespace cmvr::task {
         static TaskManager& getInstance(const config::TaskManagerConfig& cfg);
         static TaskManager& getInstance();
         static void destroyInstance();
+        static std::vector<std::shared_ptr<Task>>
+        activitySnapshotIfInitialized();
+        static bool stopAllActivitiesIfInitialized(
+            std::vector<std::string>* failures = nullptr);
 
         ~TaskManager();
 
-        void startRunTask(double control_period_s = 0.001);
+        bool startRunTask(double control_period_s = 0.001);
         void stopRunTask();
         bool running() const { return running_.load(); }
+        bool initialized() const noexcept { return initialized_; }
 
         std::shared_ptr<Task> getTask(const std::string& task_id) const;
         std::shared_ptr<TouchScreenTask> getTouchScreenTask(const std::string& task_id = "touch_screen") const;
 
+        // Stops command-driven operational activity without stopping the
+        // scheduler or destroying task/device lifecycle state.
+        bool stopAllActivities(std::vector<std::string>* failures = nullptr);
+        std::vector<std::shared_ptr<Task>> activitySnapshot() const;
+
     private:
         explicit TaskManager(const config::TaskManagerConfig& cfg);
 
         void logTaskPlan() const;
-        void initTasks();
+        bool initTasks();
         void runTaskLoop(double control_period_s);
         static TaskRunMode toTaskRunMode(config::TaskConfigEntry::TaskRunMode run_mode);
 
@@ -49,8 +60,10 @@ 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

@@ -1,5 +1,6 @@
 #include "manager/task_manager/include/task_manager.h"
 
+#include <algorithm>
 #include <chrono>
 #include <cmath>
 #include <stdexcept>
@@ -8,6 +9,7 @@
 
 #include "common/base/logging/logger.h"
 #include "common/config/config_files.h"
+#include "service/grpc/stop_all/include/stop_all_admission_gate.h"
 #include "task/task_factory.h"
 
 using namespace cmvr;
@@ -40,6 +42,8 @@ const char* taskConfigTypeToString(const config::TaskConfigEntry::TaskType type)
             return "TASK_TYPE_SELF_COLLISION";
         case config::TaskConfigEntry::TASK_TYPE_QUIC_EDGE:
             return "TASK_TYPE_QUIC_EDGE";
+        case config::TaskConfigEntry::TASK_TYPE_UME_TELEOP:
+            return "TASK_TYPE_UME_TELEOP";
         case config::TaskConfigEntry::TASK_TYPE_UNKNOWN:
         default:
             return "TASK_TYPE_UNKNOWN";
@@ -59,6 +63,21 @@ 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;
@@ -70,7 +89,11 @@ TaskManager::TaskManager(const config::TaskManagerConfig& cfg)
     logSection("Task Plan");
     logTaskPlan();
     logSection("Initialize Tasks");
-    initTasks();
+    initialized_ = initTasks();
+    if (!initialized_) {
+        CMVR_LOG(ERROR) << "[TaskManager] Initialization failed for at least "
+                           "one enabled task";
+    }
 }
 
 TaskManager::~TaskManager()
@@ -97,13 +120,48 @@ TaskManager& TaskManager::getInstance()
 }
 
 void TaskManager::destroyInstance()
+{
+    std::shared_ptr<TaskManager> instance;
     {
         std::lock_guard lock(init_mutex_);
-    if (instance_) {
-        instance_->stopRunTask();
+        instance = instance_;
     }
+    if (instance) {
+        // Task shutdown may wait for an in-flight SystemService handler. That
+        // handler can query the process-wide task snapshot, so never retain
+        // init_mutex_ while stopping tasks or joining service workers.
+        instance->stopRunTask();
+    }
+    {
+        std::lock_guard lock(init_mutex_);
+        if (instance_ == instance) {
             instance_.reset();
         }
+    }
+}
+
+bool TaskManager::stopAllActivitiesIfInitialized(
+    std::vector<std::string>* failures)
+{
+    std::shared_ptr<TaskManager> manager;
+    {
+        std::lock_guard lock(init_mutex_);
+        manager = instance_;
+    }
+    return !manager || manager->stopAllActivities(failures);
+}
+
+std::vector<std::shared_ptr<Task>>
+TaskManager::activitySnapshotIfInitialized()
+{
+    std::shared_ptr<TaskManager> manager;
+    {
+        std::lock_guard lock(init_mutex_);
+        manager = instance_;
+    }
+    return manager ? manager->activitySnapshot()
+                   : std::vector<std::shared_ptr<Task>>{};
+}
 
 std::shared_ptr<TouchScreenTask> TaskManager::getTouchScreenTask(const std::string& task_id) const
 {
@@ -126,16 +184,84 @@ std::shared_ptr<Task> TaskManager::getTask(const std::string& task_id) const
     return it->second;
 }
 
-void TaskManager::startRunTask(const double control_period_s)
+bool TaskManager::stopAllActivities(std::vector<std::string>* failures)
 {
-    if (!std::isfinite(control_period_s) || control_period_s <= 0.0) {
-        CMVR_LOG(ERROR) << "[TaskManager] invalid control_period_s";
-        return;
+    const auto tasks = activitySnapshot();
+
+    bool all_stopped = true;
+    for (const auto& task : tasks) {
+        bool stopped = false;
+        try {
+            stopped = task->stopActivity();
+        } catch (const std::exception& error) {
+            if (failures) {
+                failures->push_back(
+                    task->id() + ": stop threw: " + error.what());
+            }
+        } catch (...) {
+            if (failures) {
+                failures->push_back(
+                    task->id() + ": stop threw an unknown exception");
+            }
+        }
+        if (!stopped) {
+            all_stopped = false;
+            if (failures && (failures->empty() ||
+                failures->back().compare(0, task->id().size(), task->id()) != 0)) {
+                failures->push_back(
+                    task->id() +
+                    ": operational stop was not confirmed");
+            }
+        }
+    }
+    return all_stopped;
 }
 
-    bool expected = false;
-    if (!running_.compare_exchange_strong(expected, true)) {
-        return;
+std::vector<std::shared_ptr<Task>> TaskManager::activitySnapshot() const
+{
+    std::vector<std::shared_ptr<Task>> tasks;
+    std::lock_guard lock(tasks_mutex_);
+    tasks.reserve(tasks_.size());
+    for (const auto& [id, task] : tasks_) {
+        (void)id;
+        if (task) {
+            tasks.push_back(task);
+        }
+    }
+    return tasks;
+}
+
+bool TaskManager::startRunTask(const double control_period_s)
+{
+    auto& admission_gate = service::globalStopAllAdmissionGate();
+    std::uint64_t admission_generation = 0U;
+    {
+        auto admission = admission_gate.lockAdmission();
+        if (!admission.accepting()) {
+            CMVR_LOG(WARNING) << "[TaskManager] task startup is paused by "
+                                 "System StopAll";
+            return false;
+        }
+        admission_generation = admission.generation();
+    }
+
+    std::lock_guard lifecycle_lock(lifecycle_mutex_);
+    const auto admission_current = [&] {
+        auto admission = admission_gate.lockAdmission();
+        return admission.accepting() &&
+            admission.generation() == admission_generation;
+    };
+    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;
+    }
+    if (running_.load()) {
+        return admission_current();
     }
 
     std::vector<std::shared_ptr<Task>> tasks;
@@ -151,39 +277,98 @@ void TaskManager::startRunTask(const double control_period_s)
 
     std::vector<std::shared_ptr<Task>> started_tasks;
     for (const auto& task : tasks) {
-        if (!task->start()) {
-            CMVR_LOG(ERROR) << "[TaskManager] task start failed: " << task->id();
-            for (const auto& started_task : started_tasks) {
-                try {
-                    started_task->stop();
-                } catch (...) {
-                }
+        if (!admission_current()) {
+            CMVR_LOG(WARNING) << "[TaskManager] task startup was interrupted "
+                                 "by System StopAll";
+            for (auto it = started_tasks.rbegin();
+                 it != started_tasks.rend(); ++it) {
+                stopTaskNoThrow(*it);
             }
             running_.store(false);
-            return;
-        }
-        started_tasks.push_back(task);
+            return false;
         }
 
+        bool started = false;
         try {
-        run_thread_ = std::thread(&TaskManager::runTaskLoop, this, control_period_s);
+            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 (auto it = started_tasks.rbegin();
+                 it != started_tasks.rend(); ++it) {
+                stopTaskNoThrow(*it);
+            }
+            running_.store(false);
+            return false;
+        }
+        started_tasks.push_back(task);
+        if (!admission_current()) {
+            CMVR_LOG(WARNING) << "[TaskManager] task startup crossed a System "
+                                 "StopAll boundary: " << task->id();
+            for (auto it = started_tasks.rbegin();
+                 it != started_tasks.rend(); ++it) {
+                stopTaskNoThrow(*it);
+            }
+            running_.store(false);
+            return false;
+        }
+    }
+
+    bool admission_changed = false;
+    try {
+        // Publish the scheduler under a short admission guard. No task/device
+        // call or rollback is made while the global StopAll mutex is held.
+        auto admission = admission_gate.lockAdmission();
+        if (!admission.accepting() ||
+            admission.generation() != admission_generation) {
+            admission_changed = true;
+        } else {
+            running_.store(true);
+            run_thread_ = std::thread(
+                &TaskManager::runTaskLoop, this, control_period_s);
+        }
     } catch (const std::exception& e) {
         CMVR_LOG(ERROR) << "[TaskManager] failed to start run thread: " << e.what();
         running_.store(false);
-        for (const auto& task : started_tasks) {
-            try {
-                task->stop();
+        for (auto it = started_tasks.rbegin();
+             it != started_tasks.rend(); ++it) {
+            stopTaskNoThrow(*it);
+        }
+        return false;
     } catch (...) {
+        CMVR_LOG(ERROR) << "[TaskManager] failed to start run thread with an "
+                           "unknown exception";
+        running_.store(false);
+        for (auto it = started_tasks.rbegin();
+             it != started_tasks.rend(); ++it) {
+            stopTaskNoThrow(*it);
         }
+        return false;
     }
-        return;
+    if (admission_changed) {
+        CMVR_LOG(WARNING) << "[TaskManager] scheduler startup was "
+                             "interrupted by System StopAll";
+        for (auto it = started_tasks.rbegin();
+             it != started_tasks.rend(); ++it) {
+            stopTaskNoThrow(*it);
         }
+        running_.store(false);
+        return false;
+    }
+    return true;
 }
 
 void TaskManager::stopRunTask()
 {
-    bool expected = true;
-    if (!running_.compare_exchange_strong(expected, false)) {
+    std::lock_guard lifecycle_lock(lifecycle_mutex_);
+    if (!running_.exchange(false)) {
         return;
     }
 
@@ -201,22 +386,24 @@ void TaskManager::stopRunTask()
             }
         }
     }
+    std::sort(tasks.begin(), tasks.end(), [](const auto& lhs, const auto& rhs) {
+        return lhs->shutdownPhase() < rhs->shutdownPhase();
+    });
     for (const auto& task : tasks) {
-        try {
-            task->stop();
-        } catch (...) {
-        }
+        stopTaskNoThrow(task);
     }
 }
 
-void TaskManager::initTasks()
+bool TaskManager::initTasks()
 {
+    bool all_initialized = true;
     for (const auto& entry : cfg_.tasks()) {
         if (!entry.enable()) {
             continue;
         }
         if (entry.id().empty()) {
             CMVR_LOG(ERROR) << "[TaskManager] Task ID is empty";
+            all_initialized = false;
             continue;
         }
 
@@ -226,9 +413,30 @@ void TaskManager::initTasks()
                        << ", run_mode=" << taskConfigRunModeToString(entry.run_mode())
                        << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file());
 
-        auto task = TaskFactory::create(entry);
+        std::shared_ptr<Task> task;
+        try {
+            task = TaskFactory::create(entry);
+        } catch (const std::exception& error) {
+            CMVR_LOG(ERROR) << "[TaskManager] Task creation threw: "
+                            << entry.id() << ", error=" << error.what();
+            all_initialized = false;
+            continue;
+        } catch (...) {
+            CMVR_LOG(ERROR) << "[TaskManager] Task creation threw an unknown "
+                               "exception: " << entry.id();
+            all_initialized = false;
+            continue;
+        }
         if (!task || task->id() != entry.id()) {
             CMVR_LOG(ERROR) << "[TaskManager] Task ID mismatch: " << entry.id();
+            all_initialized = false;
+            continue;
+        }
+        if (entry.run_mode() ==
+            config::TaskConfigEntry::TASK_RUN_MODE_UNKNOWN) {
+            CMVR_LOG(ERROR) << "[TaskManager] Task run_mode is unknown: "
+                            << entry.id();
+            all_initialized = false;
             continue;
         }
         const TaskRunMode configured_run_mode = toTaskRunMode(entry.run_mode());
@@ -236,23 +444,39 @@ void TaskManager::initTasks()
             CMVR_LOG(ERROR) << "[TaskManager] Task run_mode mismatch: id=" << entry.id()
                             << ", configured=" << taskRunModeToString(configured_run_mode)
                             << ", actual=" << taskRunModeToString(task->runMode());
+            all_initialized = false;
             continue;
         }
         double control_period_s = entry.control_period_s();
         if (configured_run_mode == TaskRunMode::PERIODIC_STEP &&
             (!std::isfinite(control_period_s) || control_period_s <= 0.0)) {
             CMVR_LOG(ERROR) << "[TaskManager] invalid control_period_s for task: " << entry.id();
+            all_initialized = false;
             continue;
         }
-        if (!task->init()) {
+        bool task_initialized = false;
+        try {
+            task_initialized = task->init();
+        } catch (const std::exception& error) {
+            CMVR_LOG(ERROR) << "[TaskManager] Task init threw: "
+                            << entry.id() << ", error=" << error.what();
+        } catch (...) {
+            CMVR_LOG(ERROR) << "[TaskManager] Task init threw an unknown "
+                               "exception: " << entry.id();
+        }
+        if (!task_initialized) {
             CMVR_LOG(ERROR) << "[TaskManager] Task init failed: " << entry.id()
                             << ", status=" << task->detailStatusString();
+            stopTaskNoThrow(task);
+            all_initialized = false;
             continue;
         }
         {
             std::lock_guard lock(tasks_mutex_);
             if (tasks_.count(entry.id())) {
                 CMVR_LOG(ERROR) << "[TaskManager] Duplicate task ID: " << entry.id();
+                stopTaskNoThrow(task);
+                all_initialized = false;
                 continue;
             }
             if (configured_run_mode == TaskRunMode::PERIODIC_STEP) {
@@ -261,6 +485,7 @@ void TaskManager::initTasks()
             tasks_.emplace(entry.id(), std::move(task));
         }
     }
+    return all_initialized;
 }
 
 void TaskManager::logTaskPlan() const

cmvr-es/runtime/CMakeLists.txt

@@ -13,6 +13,7 @@ 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
 )
 

cmvr-es/runtime/src/cmvr_runtime.cpp

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

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

@@ -1,87 +0,0 @@
-#ifndef CMVR_ES_GRPC_AGV_SERVICE_H
-#define CMVR_ES_GRPC_AGV_SERVICE_H
-
-#include "cmvr/api/agv_service.grpc.pb.h"
-#include "devices/agv/abstract_agv.h"
-#include "manager/device_manager/include/device_manager.h"
-
-namespace cmvr::service {
-
-class gRPCAgvServiceImpl final : public api::AgvService::Service {
-public:
-    gRPCAgvServiceImpl();
-    ~gRPCAgvServiceImpl() override = default;
-
-    grpc::Status getRuntimeState(grpc::ServerContext* context,
-                                 const api::AgvRuntimeStateCommand_Request* request,
-                                 api::AgvRuntimeStateCommand_Feedback* response) override;
-    grpc::Status getNavigationStatus(grpc::ServerContext* context,
-                                     const api::AgvNavigationStatusCommand_Request* request,
-                                     api::AgvNavigationStatusCommand_Feedback* response) override;
-    grpc::Status emergencyStop(grpc::ServerContext* context,
-                               const api::CommandHeader_Request* request,
-                               api::CommandHeader_Feedback* response) override;
-    grpc::Status clearFault(grpc::ServerContext* context,
-                            const api::CommandHeader_Request* request,
-                            api::CommandHeader_Feedback* response) override;
-    grpc::Status navigateToPose(grpc::ServerContext* context,
-                                const api::AgvNavigateToPoseCommand_Request* request,
-                                api::AgvNavigateToPoseCommand_Feedback* response) override;
-    grpc::Status navigateToStation(grpc::ServerContext* context,
-                                   const api::AgvNavigateToStationCommand_Request* request,
-                                   api::AgvNavigateToStationCommand_Feedback* response) override;
-    grpc::Status followPath(grpc::ServerContext* context,
-                            const api::AgvFollowPathCommand_Request* request,
-                            api::AgvFollowPathCommand_Feedback* response) override;
-    grpc::Status pauseNavigation(grpc::ServerContext* context,
-                                 const api::CommandHeader_Request* request,
-                                 api::CommandHeader_Feedback* response) override;
-    grpc::Status resumeNavigation(grpc::ServerContext* context,
-                                  const api::CommandHeader_Request* request,
-                                  api::CommandHeader_Feedback* response) override;
-    grpc::Status cancelNavigation(grpc::ServerContext* context,
-                                  const api::CommandHeader_Request* request,
-                                  api::CommandHeader_Feedback* response) override;
-    grpc::Status setVelocity(grpc::ServerContext* context,
-                             const api::AgvSetVelocityCommand_Request* request,
-                             api::AgvSetVelocityCommand_Feedback* response) override;
-    grpc::Status stopVelocityControl(grpc::ServerContext* context,
-                                     const api::CommandHeader_Request* request,
-                                     api::CommandHeader_Feedback* response) override;
-    grpc::Status listMaps(grpc::ServerContext* context,
-                          const api::AgvListMapsCommand_Request* request,
-                          api::AgvListMapsCommand_Feedback* response) override;
-    grpc::Status listStations(grpc::ServerContext* context,
-                              const api::AgvListStationsCommand_Request* request,
-                              api::AgvListStationsCommand_Feedback* response) override;
-    grpc::Status switchMap(grpc::ServerContext* context,
-                           const api::AgvMapCommand_Request* request,
-                           api::AgvMapCommand_Feedback* response) override;
-    grpc::Status uploadMap(grpc::ServerContext* context,
-                           const api::AgvMapCommand_Request* request,
-                           api::AgvMapCommand_Feedback* response) override;
-    grpc::Status downloadMap(grpc::ServerContext* context,
-                             const api::AgvMapCommand_Request* request,
-                             api::AgvMapCommand_Feedback* response) override;
-    grpc::Status startMapping(grpc::ServerContext* context,
-                              const api::AgvStartMappingCommand_Request* request,
-                              api::AgvStartMappingCommand_Feedback* response) override;
-    grpc::Status streamMap(grpc::ServerContext* context,
-                           const api::AgvMapStreamCommand_Request* request,
-                           grpc::ServerWriter<api::AgvMapStreamCommand_Feedback>* writer) override;
-    grpc::Status stopMapping(grpc::ServerContext* context,
-                             const api::CommandHeader_Request* request,
-                             api::CommandHeader_Feedback* response) override;
-
-
-    grpc::Status relocalize(grpc::ServerContext* context,
-                            const api::AgvRelocalizeCommand_Request* request,
-                            api::AgvRelocalizeCommand_Feedback* response) override;
-
-private:
-    device::DeviceManager& dmgr_;
-};
-
-} // namespace cmvr::service
-
-#endif // CMVR_ES_GRPC_AGV_SERVICE_H

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

@@ -1,64 +0,0 @@
-#pragma once
-
-#include "cmvr/api/arm_service.grpc.pb.h"
-#include "devices/arm/robot_arm.h"
-#include "manager/device_manager/include/device_manager.h"
-
-namespace cmvr::service {
-
-class gRPCArmServiceImpl final : public api::ArmService::Service {
-public:
-    gRPCArmServiceImpl();
-    ~gRPCArmServiceImpl() override = default;
-
-    grpc::Status torqueOff(grpc::ServerContext* context,
-                           const api::CommandHeader_Request* request,
-                           api::CommandHeader_Feedback* response) override;
-    grpc::Status torqueOn(grpc::ServerContext* context,
-                          const api::CommandHeader_Request* request,
-                          api::CommandHeader_Feedback* response) override;
-    grpc::Status moveJ(grpc::ServerContext* context,
-                       const api::MoveJ_Request* request,
-                       api::MoveJ_Response* response) override;
-    grpc::Status moveL(grpc::ServerContext* context,
-                       const api::MoveL_Request* request,
-                       api::MoveL_Response* response) override;
-    grpc::Status speedJ(grpc::ServerContext* context,
-                        const api::SpeedJ_Request* request,
-                        api::SpeedJ_Response* response) override;
-    grpc::Status speedL(grpc::ServerContext* context,
-                        const api::SpeedL_Request* request,
-                        api::SpeedL_Response* response) override;
-    grpc::Status servoJ(grpc::ServerContext* context,
-                        const api::ServoJ_Request* request,
-                        api::ServoJ_Response* response) override;
-    grpc::Status stopMotion(grpc::ServerContext* context,
-                            const api::CommandHeader_Request* request,
-                            api::CommandHeader_Feedback* response) override;
-    grpc::Status getJointState(grpc::ServerContext* context,
-                               const api::JointRequest* request,
-                               api::JointResponse* response) override;
-    grpc::Status getRobotState(grpc::ServerContext* context,
-                               const api::GetRobotState_Request* request,
-                               api::GetRobotState_Response* response) override;
-    grpc::Status getPose(grpc::ServerContext* context,
-                         const api::GetPose_Request* request,
-                         api::GetPose_Response* response) override;
-    grpc::Status calibrateZeroQ(grpc::ServerContext* context,
-                                const api::CalibrateZeroQ_Request* request,
-                                api::CalibrateZeroQ_Response* response) override;
-    grpc::Status getPoseMatrix(grpc::ServerContext* context,
-                               const api::GetPoseMatrix_Request* request,
-                               api::GetPoseMatrix_Response* response) override;
-    grpc::Status computeForwardKinematics(grpc::ServerContext* context,
-                                          const api::ComputeForwardKinematics_Request* request,
-                                          api::ComputeForwardKinematics_Response* response) override;
-    grpc::Status clearFault(grpc::ServerContext *context,
-                            const cmvr::api::CommandHeader_Request *request,
-                            cmvr::api::CommandHeader_Feedback *response) override;
-
-private:
-    device::DeviceManager& dmgr_;
-};
-
-} // namespace cmvr::service

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

@@ -1,46 +0,0 @@
-//
-// Created by xtkuang on 2025/6/1.
-//
-
-#ifndef GRPC_CAMERA_SERVICE_H
-#define GRPC_CAMERA_SERVICE_H
-
-#include "cmvr/api/camera_service.grpc.pb.h"
-#include "common/base/grpc_utils.h"
-#include "manager/device_manager/include/device_manager.h"
-#include "devices/camera/abstract_camera.h"
-#include "service/grpc/include/grpc_camera_stream_policy.h"
-
-namespace cmvr::service {
-
-    class gRPCCameraServiceImpl final: public api::CameraService::Service {
-    public:
-        explicit gRPCCameraServiceImpl(
-            CameraStreamLowLatencyConfig stream_config = {});
-        ~gRPCCameraServiceImpl() override = default;
-        grpc::Status GetStatus(grpc::ServerContext* context, const api::GetCameraStateCommand_Request* request, api::GetCameraStateCommand_Feedback* response) override;
-        grpc::Status StartCamera(grpc::ServerContext* context, const api::StartCameraCommand_Request* request, api::StartCameraCommand_Feedback* response) override;
-        grpc::Status StopCamera(grpc::ServerContext* context, const api::StopCameraCommand_Request* request, api::StopCameraCommand_Feedback* response) override;
-        grpc::Status GetRGBImage(grpc::ServerContext* context, const api::GetRGBImageCommand_Request* request, api::GetRGBImageCommand_Feedback* response) override;
-        grpc::Status GetDepthImage(grpc::ServerContext* context, const api::GetDepthImageCommand_Request* request, api::GetDepthImageCommand_Feedback* response) override;
-        grpc::Status GetRGBDImages(grpc::ServerContext* context, const api::GetRGBDImagesCommand_Request* request, api::GetRGBDImagesCommand_Feedback* response) override;
-        grpc::Status StartRecording(grpc::ServerContext* context, const api::StartCameraRecordingCommand_Request* request, api::StartCameraRecordingCommand_Feedback* response) override;
-        grpc::Status StopRecording(grpc::ServerContext* context, const api::StopCameraRecordingCommand_Request* request, api::StopCameraRecordingCommand_Feedback* response) override;
-        grpc::Status ControlPtz(grpc::ServerContext* context, const api::ControlPtzCommand_Request* request, api::ControlPtzCommand_Feedback* response) override;
-        grpc::Status GetDepthImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetDepthImageStreamCommand_Feedback, cmvr::api::GetDepthImageStreamCommand_Request>* stream) override;
-        grpc::Status GetRGBDImagesStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBDImagesStreamCommand_Feedback, cmvr::api::GetRGBDImagesStreamCommand_Request>* stream) override;
-        grpc::Status GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback, cmvr::api::GetRGBImageStreamCommand_Request>* stream) override;
-    private:
-        device::DeviceManager& dmgr_;
-        CameraStreamLowLatencyConfig stream_config_;
-
-        //双向流读写线程
-        std::shared_ptr<std::thread> read_thread_ = nullptr;
-        std::shared_ptr<std::thread> write_thread_ = nullptr;
-        std::atomic<bool> running_{false};
-    };
-}
-
-
-
-#endif //GRPC_CAMERA_SERVICE_H

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

@@ -1,60 +0,0 @@
-#ifndef CMVR_ES_GRPC_CAMERA_STREAM_POLICY_H
-#define CMVR_ES_GRPC_CAMERA_STREAM_POLICY_H
-
-#pragma once
-
-#include <chrono>
-#include <cstddef>
-#include <cstdint>
-#include <optional>
-
-namespace cmvr::service {
-
-inline constexpr size_t kDefaultCameraStreamMaxPendingFrames = 2;
-inline constexpr uint32_t kDefaultCameraStreamMaxFrameAgeMs = 250;
-
-struct CameraStreamLowLatencyConfig {
-    size_t max_pending_frames{kDefaultCameraStreamMaxPendingFrames};
-    std::chrono::milliseconds max_frame_age{
-        kDefaultCameraStreamMaxFrameAgeMs};
-};
-
-inline CameraStreamLowLatencyConfig makeCameraStreamLowLatencyConfig(
-    const uint32_t max_pending_frames,
-    const uint32_t max_frame_age_ms) noexcept {
-    CameraStreamLowLatencyConfig config;
-    config.max_pending_frames = max_pending_frames == 0
-        ? kDefaultCameraStreamMaxPendingFrames
-        : static_cast<size_t>(max_pending_frames);
-    config.max_frame_age = std::chrono::milliseconds(
-        max_frame_age_ms == 0
-            ? kDefaultCameraStreamMaxFrameAgeMs
-            : max_frame_age_ms);
-    return config;
-}
-
-inline std::optional<uint64_t> cameraFrameAgeNs(
-    const uint64_t capture_time_ns,
-    const uint64_t now_ns) noexcept {
-    if (capture_time_ns == 0 || now_ns < capture_time_ns) {
-        return std::nullopt;
-    }
-    return now_ns - capture_time_ns;
-}
-
-inline bool cameraFrameExceedsAgeLimit(
-    const uint64_t capture_time_ns,
-    const uint64_t now_ns,
-    const std::chrono::milliseconds max_frame_age) noexcept {
-    const auto age_ns = cameraFrameAgeNs(capture_time_ns, now_ns);
-    if (!age_ns || max_frame_age.count() <= 0) {
-        return false;
-    }
-    const auto max_age_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
-        max_frame_age).count();
-    return *age_ns > static_cast<uint64_t>(max_age_ns);
-}
-
-} // namespace cmvr::service
-
-#endif // CMVR_ES_GRPC_CAMERA_STREAM_POLICY_H

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

@@ -1,31 +0,0 @@
-//
-// Created by linbo on 2025/7/3.
-//
-
-#ifndef GRPC_DEXHAND_SERVICE_H
-#define GRPC_DEXHAND_SERVICE_H
-
-#include "cmvr/api/dexhand_service.grpc.pb.h"
-#include "common/base/grpc_utils.h"
-#include "manager/device_manager/include/device_manager.h"
-#include "devices/dexhand/abstract_dexhand.h"
-namespace cmvr::service {
-    class gRPCDexHandServiceImpl final: public api::DexHandService::Service {
-    public:
-        gRPCDexHandServiceImpl();
-        ~gRPCDexHandServiceImpl() override = default;
-        grpc::Status GetStatus(grpc::ServerContext* context, const api::GetDexHandStateCommand_Request* request,api::GetDexHandStateCommand_Feedback* response) override;
-        grpc::Status SetDexHandPos(grpc::ServerContext* context, const cmvr::api::SetDexHandPositionsCommand_Request* request, cmvr::api::SetDexHandPositionsCommand_Feedback* response) override;
-        grpc::Status SetDexHandAngle(grpc::ServerContext* context, const cmvr::api::SetDexHandAnglesCommand_Request* request, cmvr::api::SetDexHandAnglesCommand_Feedback* response) override;
-        grpc::Status SetDexHandForce(grpc::ServerContext* context, const cmvr::api::SetDexHandForceCommand_Request* request, cmvr::api::SetDexHandForceCommand_Feedback* response) override;
-        grpc::Status SetDexHandSpeed(grpc::ServerContext* context, const cmvr::api::SetDexHandSpeedCommand_Request* request, cmvr::api::SetDexHandSpeedCommand_Feedback* response) override;
-        grpc::Status SetDexHandPresetAct(grpc::ServerContext* context, const cmvr::api::SetDexHandPresetActCommand_Request* request, cmvr::api::SetDexHandPresetActCommand_Feedback* response) override;
-        grpc::Status GetSensorData(grpc::ServerContext* context, const cmvr::api::GetSensorDataCommand_Request* request, cmvr::api::GetSensorDataCommand_Feedback* response) override;
-        grpc::Status GetSensorDataStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetSensorDataStreamCommand_Feedback, cmvr::api::GetSensorDataStreamCommand_Request>* stream) override;
-    private:
-        device::DeviceManager& dmgr_;
-    };
-
-}
-
-#endif //GRPC_DEXHAND_SERVICE_H

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

@@ -1,50 +0,0 @@
-#ifndef BIO_HEAD_SERVICE_H
-#define BIO_HEAD_SERVICE_H
-
-#include "cmvr/api/biohead_service.grpc.pb.h"
-#include "manager/device_manager/include/device_manager.h"
-#include "devices/biohead/abstract_biohead.h"
-
-namespace cmvr::service
-{
-    class gRPCMBioHeadServiceImpl : public api::BioHeadService::Service {
-    public:
-        gRPCMBioHeadServiceImpl();
-        ~gRPCMBioHeadServiceImpl() override = default;
-
-        grpc::Status SetExpression(grpc::ServerContext* context,
-                                   const api::SetFacialExpression_Request* request,
-                                   api::SetFacialExpression_Feedback* response) override;
-
-        grpc::Status StreamExpression(grpc::ServerContext* context,
-                                             grpc::ServerReaderWriter<api::StreamFacialExpression_Feedback, api::StreamFacialExpression_Request>* stream) override;
-
-
-        grpc::Status GetSystemStatus(grpc::ServerContext* context,
-                                     const api::GetStatus_Request* request,
-                                     api::GetStatus_Feedback* response) override;
-
-        grpc::Status EmergencyStop(grpc::ServerContext* context,
-                                   const api::EmergencyStop_Request* request,
-                                   api::EmergencyStop_Feedback* response) override;
-        grpc::Status SpeakStart(grpc::ServerContext* context, const cmvr::api::SpeakStart_Request* request, cmvr::api::SpeakStart_Feedback* response) override;
-
-        grpc::Status SpeakStop(grpc::ServerContext* context, const cmvr::api::SpeakStop_Request* request, cmvr::api::SpeakStop_Feedback* response) override;
-
-        grpc::Status Happy(grpc::ServerContext* context, const cmvr::api::Happy_Request* request, cmvr::api::Happy_Feedback* response) override;
-
-        grpc::Status Surprise(grpc::ServerContext* context, const cmvr::api::Surprise_Request* request, cmvr::api::Surprise_Feedback* response) override;
-
-        grpc::Status ExpressionTired(grpc::ServerContext* context, const cmvr::api::ExpressionTired_Request* request, cmvr::api::ExpressionTired_Feedback* response) override;
-
-        grpc::Status ExpressionAngry(grpc::ServerContext* context, const cmvr::api::ExpressionAngry_Request* request, cmvr::api::ExpressionAngry_Feedback* response) override;
-
-        grpc::Status ExpressionSadness(grpc::ServerContext* context, const cmvr::api::ExpressionSadness_Request* request, cmvr::api::ExpressionSadness_Feedback* response) override;
-
-        grpc::Status ExpressionYawn(grpc::ServerContext* context, const cmvr::api::ExpressionYawn_Request* request, cmvr::api::ExpressionYawn_Feedback* response) override;
-    private:
-        device::DeviceManager& dmgr_;
-    };
-}  // namespace cmvr::service
-
-#endif  // BIO_HEAD_SERVICE_H

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

@@ -1,21 +0,0 @@
-//
-// Created by lgv on 2025/8/25.
-//
-#pragma once
-
-#include "cmvr/api/hlc_service.grpc.pb.h"
-
-namespace cmvr {
-    namespace service {
-        class gRPCHlcServiceImpl final : public api::HlcService::Service {
-        public:
-            gRPCHlcServiceImpl();
-            ~gRPCHlcServiceImpl() = default;
-            grpc::Status touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) override;
-        };
-
-
-
-    }
-
-}

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

@@ -1,36 +0,0 @@
-//
-// Created by linbo on 2025/6/13.
-// Created by xtkuang on 2025/6/13.
-//
-
-#ifndef GRPC_MICROPHONE_SERVICE_H
-#define GRPC_MICROPHONE_SERVICE_H
-#include "cmvr/api/microphone_service.grpc.pb.h"
-#include "manager/device_manager/include/device_manager.h"
-#include "devices/microphone/abstract_microphone.h"
-#include "common/base/grpc_utils.h"
-
-namespace cmvr::service
-{
-
-    class gRPCMicroPhoneServiceImpl: public api::MicPhoneService::Service {
-    public:
-        gRPCMicroPhoneServiceImpl();
-        ~gRPCMicroPhoneServiceImpl() override = default;
-        grpc::Status ListDevices(
-            grpc::ServerContext* context,
-            const api::ListMicrophoneDevicesCommand_Request* request,
-            api::ListMicrophoneDevicesCommand_Feedback* response) override;
-        grpc::Status GetStatus(grpc::ServerContext* context, const api::GetMicStateCommand_Request* request,api::GetMicStateCommand_Feedback* response) override;
-        grpc::Status StartRecord(grpc::ServerContext* context, const api::StartMicRecordingCommand_Request* request,api::StartMicRecordingCommand_Feedback* response) override;
-        grpc::Status StopRecord(grpc::ServerContext* context, const api::StopMicRecordingCommand_Request* request,api::StopMicRecordingCommand_Feedback* response) override;
-        grpc::Status PauseRecord(grpc::ServerContext* context, const api::PauseMicRecordingCommand_Request* request,api::PauseMicRecordingCommand_Feedback* response) override;
-        grpc::Status ResumeRecord(grpc::ServerContext* context, const api::ResumeMicRecordingCommand_Request* request,api::ResumeMicRecordingCommand_Feedback* response) override;
-        grpc::Status StreamAudio(grpc::ServerContext* context, const api::StreamMicAudioCommand_Request* request, grpc::ServerWriter<api::StreamMicAudioCommand_Feedback>* writer) override;
-        grpc::Status SetVolume(grpc::ServerContext* context, const api::SetMicPhoneVolumeCommand_Request* request,api::SetMicPhoneVolumeCommand_Feedback* response) override;
-        grpc::Status GetVolume(grpc::ServerContext* context, const api::GetMicPhoneVolumeCommand_Request* request,api::GetMicPhoneVolumeCommand_Feedback* response) override;
-    private:
-        device::DeviceManager& dmgr_;
-    };
-}
-#endif //GRPC_MICROPHONE_SERVICE_H

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

@@ -1,32 +0,0 @@
-//
-// Created by xtkuang on 2025/6/10.
-//
-
-#ifndef GRPC_SPEAKER_SERVICE_H
-#define GRPC_SPEAKER_SERVICE_H
-
-#include "cmvr/api/speaker_service.grpc.pb.h"
-#include "manager/device_manager/include/device_manager.h"
-#include "devices/speaker/abstract_speaker.h"
-#include "common/base/grpc_utils.h"
-
-namespace cmvr::service {
-    class gRPCSpeakerServiceImpl: public api::SpeakerService::Service {
-    public:
-        gRPCSpeakerServiceImpl();
-        ~gRPCSpeakerServiceImpl() override = default;
-        grpc::Status GetStatus(grpc::ServerContext* context, const api::GetSpeakerStateCommand_Request* request,api::GetSpeakerStateCommand_Feedback* response) override;
-        grpc::Status PlayAudio(grpc::ServerContext* context, const api::PlayAudioCommand_Request* request,api::PlayAudioCommand_Feedback* response) override;
-        grpc::Status StreamAudio(grpc::ServerContext* context, grpc::ServerReader<api::StreamSpeakerAudioCommand_Request>* reader, api::StreamSpeakerAudioCommand_Feedback* response) override;
-        grpc::Status StopPlayback(grpc::ServerContext* context, const api::StopSpeakerCommand_Request* request,api::StopSpeakerCommand_Feedback* response) override;
-        grpc::Status PausePlayback(grpc::ServerContext* context, const api::PauseSpeakerCommand_Request* request,api::PauseSpeakerCommand_Feedback* response) override;
-        grpc::Status ResumePlayback(grpc::ServerContext* context, const api::ResumeSpeakerCommand_Request* request,api::ResumeSpeakerCommand_Feedback* response) override;
-        grpc::Status SetVolume(grpc::ServerContext* context, const api::SetSpeakerVolumeCommand_Request* request,api::SetSpeakerVolumeCommand_Feedback* response) override;
-        grpc::Status GetVolume(grpc::ServerContext* context, const api::GetSpeakerVolumeCommand_Request* request,api::GetSpeakerVolumeCommand_Feedback* response) override;
-    private:
-        device::DeviceManager& dmgr_;
-    };
-}
-
-
-#endif //GRPC_SPEAKER_SERVICE_H

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

@@ -1,28 +0,0 @@
-//
-// Created by xtkuang on 2025/6/6.
-//
-
-#ifndef GRPC_SYSTEM_SERVICE_H
-#define GRPC_SYSTEM_SERVICE_H
-
-#include "cmvr/api/system_service.grpc.pb.h"
-#include "common/base/grpc_utils.h"
-#include "manager/device_manager/include/device_manager.h"
-
-namespace cmvr::service
-{
-    class gRPCSystemServiceImpl: public api::SystemService::Service {
-    public:
-        gRPCSystemServiceImpl();
-        ~gRPCSystemServiceImpl() override = default;
-        grpc::Status GetSystemInfo(grpc::ServerContext* context, const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response) override;
-        grpc::Status GetSystemStatus(grpc::ServerContext* context, const api::GetSystemStatusCommand_Request* request, api::GetSystemStatusCommand_Feedback* response) override;
-        grpc::Status UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) override;
-        grpc::Status ExecuteJsonCommand(grpc::ServerContext* context, const cmvr::api::JsonDeviceCommand_Request* request, cmvr::api::JsonDeviceCommand_Feedback* response) override;
-        grpc::Status StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) override;
-    private:
-        device::DeviceManager& dmgr_;
-    };
-}
-
-#endif //GRPC_SYSTEM_SERVICE_H

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

@@ -1,752 +0,0 @@
-#include "service/grpc/include/grpc_agv_service.h"
-
-#include <cstdint>
-#include <exception>
-#include <string>
-#include <vector>
-
-#include <google/protobuf/util/time_util.h>
-
-using google::protobuf::util::TimeUtil;
-
-namespace cmvr::service {
-
-namespace {
-
-void fillFeedback(api::CommandHeader_Feedback* feedback,
-                  const bool success,
-                  const std::string& message = {})
-{
-    feedback->set_success(success);
-    feedback->set_error_message(message);
-    *feedback->mutable_timestamp() = TimeUtil::GetCurrentTime();
-}
-
-grpc::Status resultToStatus(const device::AgvResult& result)
-{
-    if (result.ok()) {
-        return grpc::Status::OK;
-    }
-    return grpc::Status(grpc::StatusCode::INTERNAL, result.message);
-}
-
-template <typename Response>
-grpc::Status setResponseResult(Response* response, const device::AgvResult& result)
-{
-    fillFeedback(response->mutable_header(), result.ok(), result.ok() ? "" : result.message);
-    return resultToStatus(result);
-}
-
-grpc::Status setResponseResult(api::CommandHeader_Feedback* response, const device::AgvResult& result)
-{
-    fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
-    return resultToStatus(result);
-}
-
-template <typename Response>
-grpc::Status setDeviceNotFound(Response* response, const std::string& device_id)
-{
-    const std::string message = "AGV device not found: " + device_id;
-    fillFeedback(response->mutable_header(), false, message);
-    return grpc::Status(grpc::StatusCode::NOT_FOUND, message);
-}
-
-grpc::Status setDeviceNotFound(api::CommandHeader_Feedback* response, const std::string& device_id)
-{
-    const std::string message = "AGV device not found: " + device_id;
-    fillFeedback(response, false, message);
-    return grpc::Status(grpc::StatusCode::NOT_FOUND, message);
-}
-
-device::AgvAdapterParams toAdapterParams(const msgs::AgvAdapterParams& src)
-{
-    device::AgvAdapterParams dst;
-    for (const auto& [key, value] : src.values()) {
-        dst.values.emplace(key, value);
-    }
-    return dst;
-}
-
-device::AgvMotionOptions toMotionOptions(const msgs::AgvMotionOptions& src)
-{
-    device::AgvMotionOptions dst;
-    dst.max_speed = src.max_speed();
-    dst.max_angular_speed = src.max_angular_speed();
-    dst.max_acceleration = src.max_acceleration();
-    dst.max_angular_acceleration = src.max_angular_acceleration();
-    dst.reach_distance = src.reach_distance();
-    dst.reach_angle = src.reach_angle();
-    dst.speed_ratio = src.speed_ratio() > 0.0 ? src.speed_ratio() : 1.0;
-    dst.asynchronous = src.asynchronous();
-    return dst;
-}
-
-device::AgvVelocity toVelocity(const msgs::AgvVelocity& src)
-{
-    return {src.vx(), src.vy(), src.wz()};
-}
-
-device::AgvPathSegment toPathSegment(const msgs::AgvPathSegment& src)
-{
-    device::AgvPathSegment dst;
-    dst.source_station = src.source_station();
-    dst.target_station = src.target_station();
-    return dst;
-}
-
-math::Pose2d toPose2d(const msgs::AgvPose2d& src)
-{
-    return {src.x(), src.y(), src.theta()};
-}
-
-device::AgvMapDimension toMapDimension(const msgs::AgvMapDimension src)
-{
-    switch (src) {
-        case msgs::AGV_MAP_2D:
-            return device::AgvMapDimension::Map2D;
-        case msgs::AGV_MAP_3D:
-            return device::AgvMapDimension::Map3D;
-        case msgs::AGV_MAP_2D_AND_3D:
-            return device::AgvMapDimension::Map2DAnd3D;
-        case msgs::AGV_MAP_DIMENSION_UNSPECIFIED:
-        default:
-            return device::AgvMapDimension::Unspecified;
-    }
-}
-
-msgs::AgvMapDimension toProtoMapDimension(const device::AgvMapDimension src)
-{
-    switch (src) {
-        case device::AgvMapDimension::Map2D:
-            return msgs::AGV_MAP_2D;
-        case device::AgvMapDimension::Map3D:
-            return msgs::AGV_MAP_3D;
-        case device::AgvMapDimension::Map2DAnd3D:
-            return msgs::AGV_MAP_2D_AND_3D;
-        case device::AgvMapDimension::Unspecified:
-        default:
-            return msgs::AGV_MAP_DIMENSION_UNSPECIFIED;
-    }
-}
-
-msgs::AgvMapUpdateType toProtoMapUpdateType(const device::AgvMapUpdateType src)
-{
-    switch (src) {
-        case device::AgvMapUpdateType::Snapshot:
-            return msgs::AGV_MAP_UPDATE_SNAPSHOT;
-        case device::AgvMapUpdateType::Incremental:
-            return msgs::AGV_MAP_UPDATE_INCREMENTAL;
-        case device::AgvMapUpdateType::Reset:
-            return msgs::AGV_MAP_UPDATE_RESET;
-        case device::AgvMapUpdateType::Unspecified:
-        default:
-            return msgs::AGV_MAP_UPDATE_UNSPECIFIED;
-    }
-}
-
-msgs::AgvMapObjectType toProtoMapObjectType(const device::AgvMapObjectType src)
-{
-    switch (src) {
-        case device::AgvMapObjectType::Station:
-            return msgs::AGV_MAP_OBJECT_STATION;
-        case device::AgvMapObjectType::Line:
-            return msgs::AGV_MAP_OBJECT_LINE;
-        case device::AgvMapObjectType::Area:
-            return msgs::AGV_MAP_OBJECT_AREA;
-        case device::AgvMapObjectType::QrTag:
-            return msgs::AGV_MAP_OBJECT_QR_TAG;
-        case device::AgvMapObjectType::Reflector:
-            return msgs::AGV_MAP_OBJECT_REFLECTOR;
-        case device::AgvMapObjectType::BinLocation:
-            return msgs::AGV_MAP_OBJECT_BIN_LOCATION;
-        case device::AgvMapObjectType::ExternalDevice:
-            return msgs::AGV_MAP_OBJECT_EXTERNAL_DEVICE;
-        case device::AgvMapObjectType::Unspecified:
-        default:
-            return msgs::AGV_MAP_OBJECT_UNSPECIFIED;
-    }
-}
-
-void fillPose2d(msgs::AgvPose2d* dst, const math::Pose2d& src)
-{
-    dst->set_x(src.x);
-    dst->set_y(src.y);
-    dst->set_theta(src.theta);
-}
-
-void fillVelocity(msgs::AgvVelocity* dst, const device::AgvVelocity& src)
-{
-    dst->set_vx(src.vx);
-    dst->set_vy(src.vy);
-    dst->set_wz(src.wz);
-}
-
-void fillBattery(msgs::AgvBatteryState* dst, const device::AgvBatteryState& src)
-{
-    dst->set_percentage(src.percentage);
-    dst->set_voltage(src.voltage);
-    dst->set_current(src.current);
-    dst->set_temperature(src.temperature);
-    dst->set_charging(src.charging);
-}
-
-void fillRuntimeState(msgs::AgvRuntimeState* dst, const device::AgvRuntimeState& src)
-{
-    dst->set_timestamp(src.timestamp);
-    dst->set_mode(static_cast<int>(src.mode));
-    dst->set_connected(src.connected);
-    dst->set_localized(src.localized);
-    dst->set_moving(src.moving);
-    dst->set_fault(src.fault);
-    dst->set_emergency_stopped(src.emergency_stopped);
-    fillPose2d(dst->mutable_pose(), src.pose);
-    fillVelocity(dst->mutable_velocity(), src.velocity);
-    fillBattery(dst->mutable_battery(), src.battery);
-    dst->set_current_map(src.current_map);
-    dst->set_current_station(src.current_station);
-    dst->set_last_error(src.last_error);
-}
-
-void fillNavigationStatus(msgs::AgvNavigationStatus* dst, const device::AgvNavigationStatus& src)
-{
-    dst->set_state(static_cast<int>(src.state));
-    dst->set_type(static_cast<int>(src.type));
-    dst->set_progress(src.progress);
-    dst->set_message(src.message);
-}
-
-void fillStation(msgs::AgvStation* dst, const device::AgvStation& src)
-{
-    dst->set_id(src.id);
-    dst->set_type(src.type);
-    fillPose2d(dst->mutable_pose(), src.pose);
-    dst->set_description(src.description);
-}
-
-void fillMapPoint3D(msgs::AgvMapPoint3D* dst, const device::AgvMapPoint3D& src)
-{
-    dst->set_x(src.x);
-    dst->set_y(src.y);
-    dst->set_z(src.z);
-}
-
-void fillMapObject(msgs::AgvMapObject* dst, const device::AgvMapObject& src)
-{
-    dst->set_id(src.id);
-    dst->set_type(toProtoMapObjectType(src.type));
-    for (const auto& point : src.points) {
-        fillMapPoint3D(dst->add_points(), point);
-    }
-    dst->set_heading(src.heading);
-    auto* properties = dst->mutable_properties();
-    for (const auto& [key, value] : src.properties) {
-        (*properties)[key] = value;
-    }
-}
-
-void fillUnifiedMap2D(msgs::AgvUnifiedMap2D* dst, const device::AgvUnifiedMap2D& src)
-{
-    dst->set_frame_id(src.frame_id);
-    dst->set_timestamp(src.timestamp);
-    dst->set_resolution(src.resolution);
-    dst->set_width(src.width);
-    dst->set_height(src.height);
-    fillPose2d(dst->mutable_origin(), src.origin);
-    for (const auto value : src.data) {
-        dst->add_data(value);
-    }
-    for (const auto& object : src.objects) {
-        fillMapObject(dst->add_objects(), object);
-    }
-}
-
-void fillUnifiedMap3D(msgs::AgvUnifiedMap3D* dst, const device::AgvUnifiedMap3D& src)
-{
-    dst->set_frame_id(src.frame_id);
-    dst->set_timestamp(src.timestamp);
-    dst->set_voxel_resolution(src.voxel_resolution);
-    for (const auto& point : src.points) {
-        auto* dst_point = dst->add_points();
-        dst_point->set_x(point.x);
-        dst_point->set_y(point.y);
-        dst_point->set_z(point.z);
-        dst_point->set_intensity(point.intensity);
-        dst_point->set_ring(point.ring);
-        dst_point->set_time_offset(point.time_offset);
-    }
-    for (const auto& voxel : src.voxels) {
-        auto* dst_voxel = dst->add_voxels();
-        dst_voxel->set_x(voxel.x);
-        dst_voxel->set_y(voxel.y);
-        dst_voxel->set_z(voxel.z);
-        dst_voxel->set_probability(voxel.probability);
-    }
-    for (const auto& plane : src.planes) {
-        auto* dst_plane = dst->add_planes();
-        fillMapPoint3D(dst_plane->mutable_center(), plane.center);
-        fillMapPoint3D(dst_plane->mutable_normal(), plane.normal);
-        dst_plane->set_d(plane.d);
-        dst_plane->set_radius(plane.radius);
-    }
-    for (const auto& object : src.objects) {
-        fillMapObject(dst->add_objects(), object);
-    }
-}
-
-void fillUnifiedMapUpdate(msgs::AgvUnifiedMapUpdate* dst, const device::AgvUnifiedMapUpdate& src)
-{
-    dst->set_map_id(src.map_id);
-    dst->set_session_id(src.session_id);
-    dst->set_sequence(src.sequence);
-    dst->set_resume_token(src.resume_token);
-    dst->set_dimension(toProtoMapDimension(src.dimension));
-    dst->set_update_type(toProtoMapUpdateType(src.update_type));
-    dst->set_frame_id(src.frame_id);
-    dst->set_timestamp(src.timestamp);
-    dst->set_snapshot_begin(src.snapshot_begin);
-    dst->set_snapshot_end(src.snapshot_end);
-    dst->set_chunk_index(src.chunk_index);
-    dst->set_chunk_count(src.chunk_count);
-    if (src.map_2d) {
-        fillUnifiedMap2D(dst->mutable_map_2d(), *src.map_2d);
-    } else if (src.map_3d) {
-        fillUnifiedMap3D(dst->mutable_map_3d(), *src.map_3d);
-    }
-}
-
-} // namespace
-
-gRPCAgvServiceImpl::gRPCAgvServiceImpl()
-    : dmgr_(device::DeviceManager::getInstance())
-{
-}
-
-grpc::Status gRPCAgvServiceImpl::getRuntimeState(grpc::ServerContext*,
-                                                 const api::AgvRuntimeStateCommand_Request* request,
-                                                 api::AgvRuntimeStateCommand_Feedback* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
-        if (!agv) {
-            return setDeviceNotFound(response, device_id);
-        }
-        fillRuntimeState(response->mutable_state(), agv->runtimeState());
-        fillFeedback(response->mutable_header(), true);
-        return grpc::Status::OK;
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::getNavigationStatus(grpc::ServerContext*,
-                                                     const api::AgvNavigationStatusCommand_Request* request,
-                                                     api::AgvNavigationStatusCommand_Feedback* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
-        if (!agv) {
-            return setDeviceNotFound(response, device_id);
-        }
-        fillNavigationStatus(response->mutable_status(), agv->navigationStatus());
-        fillFeedback(response->mutable_header(), true);
-        return grpc::Status::OK;
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::emergencyStop(grpc::ServerContext*,
-                                               const api::CommandHeader_Request* request,
-                                               api::CommandHeader_Feedback* response)
-{
-    try {
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
-        if (!agv) {
-            return setDeviceNotFound(response, request->device_id());
-        }
-        return setResponseResult(response, agv->emergencyStop());
-    } catch (const std::exception& e) {
-        fillFeedback(response, false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext*,
-                                            const api::CommandHeader_Request* request,
-                                            api::CommandHeader_Feedback* response)
-{
-    try {
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
-        if (!agv) {
-            return setDeviceNotFound(response, request->device_id());
-        }
-        return setResponseResult(response, agv->clearFault());
-    } catch (const std::exception& e) {
-        fillFeedback(response, false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext*,
-                                                const api::AgvNavigateToPoseCommand_Request* request,
-                                                api::AgvNavigateToPoseCommand_Feedback* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
-        if (!agv) {
-            return setDeviceNotFound(response, device_id);
-        }
-        return setResponseResult(response, agv->navigateToPose(
-            toPose2d(request->pose()),
-            toMotionOptions(request->options()),
-            toAdapterParams(request->adapter_params())));
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext*,
-                                                   const api::AgvNavigateToStationCommand_Request* request,
-                                                   api::AgvNavigateToStationCommand_Feedback* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
-        if (!agv) {
-            return setDeviceNotFound(response, device_id);
-        }
-        return setResponseResult(response, agv->navigateToStation(
-            request->station_id(),
-            toMotionOptions(request->options()),
-            toAdapterParams(request->adapter_params())));
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext*,
-                                            const api::AgvFollowPathCommand_Request* request,
-                                            api::AgvFollowPathCommand_Feedback* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
-        if (!agv) {
-            return setDeviceNotFound(response, device_id);
-        }
-        std::vector<device::AgvPathSegment> path;
-        path.reserve(static_cast<std::size_t>(request->path_size()));
-        for (const auto& segment : request->path()) {
-            path.push_back(toPathSegment(segment));
-        }
-        return setResponseResult(response, agv->followPath(path));
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::pauseNavigation(grpc::ServerContext*,
-                                                 const api::CommandHeader_Request* request,
-                                                 api::CommandHeader_Feedback* response)
-{
-    try {
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
-        if (!agv) {
-            return setDeviceNotFound(response, request->device_id());
-        }
-        return setResponseResult(response, agv->pauseNavigation());
-    } catch (const std::exception& e) {
-        fillFeedback(response, false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::resumeNavigation(grpc::ServerContext*,
-                                                  const api::CommandHeader_Request* request,
-                                                  api::CommandHeader_Feedback* response)
-{
-    try {
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
-        if (!agv) {
-            return setDeviceNotFound(response, request->device_id());
-        }
-        return setResponseResult(response, agv->resumeNavigation());
-    } catch (const std::exception& e) {
-        fillFeedback(response, false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::cancelNavigation(grpc::ServerContext*,
-                                                  const api::CommandHeader_Request* request,
-                                                  api::CommandHeader_Feedback* response)
-{
-    try {
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
-        if (!agv) {
-            return setDeviceNotFound(response, request->device_id());
-        }
-        return setResponseResult(response, agv->cancelNavigation());
-    } catch (const std::exception& e) {
-        fillFeedback(response, false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::setVelocity(grpc::ServerContext*,
-                                             const api::AgvSetVelocityCommand_Request* request,
-                                             api::AgvSetVelocityCommand_Feedback* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
-        if (!agv) {
-            return setDeviceNotFound(response, device_id);
-        }
-        return setResponseResult(response, agv->setVelocity(toVelocity(request->velocity())));
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::stopVelocityControl(grpc::ServerContext*,
-                                                     const api::CommandHeader_Request* request,
-                                                     api::CommandHeader_Feedback* response)
-{
-    try {
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
-        if (!agv) {
-            return setDeviceNotFound(response, request->device_id());
-        }
-        return setResponseResult(response, agv->stopVelocityControl());
-    } catch (const std::exception& e) {
-        fillFeedback(response, false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::listMaps(grpc::ServerContext*,
-                                          const api::AgvListMapsCommand_Request* request,
-                                          api::AgvListMapsCommand_Feedback* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
-        if (!agv) {
-            return setDeviceNotFound(response, device_id);
-        }
-        std::vector<std::string> maps;
-        const auto result = agv->listMaps(maps);
-        if (result.ok()) {
-            for (const auto& map : maps) {
-                response->add_maps(map);
-            }
-        }
-        return setResponseResult(response, result);
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::listStations(grpc::ServerContext*,
-                                              const api::AgvListStationsCommand_Request* request,
-                                              api::AgvListStationsCommand_Feedback* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
-        if (!agv) {
-            return setDeviceNotFound(response, device_id);
-        }
-        std::vector<device::AgvStation> stations;
-        const auto result = agv->listStations(stations);
-        if (result.ok()) {
-            for (const auto& station : stations) {
-                fillStation(response->add_stations(), station);
-            }
-        }
-        return setResponseResult(response, result);
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::switchMap(grpc::ServerContext*,
-                                           const api::AgvMapCommand_Request* request,
-                                           api::AgvMapCommand_Feedback* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
-        if (!agv) {
-            return setDeviceNotFound(response, device_id);
-        }
-        return setResponseResult(response, agv->switchMap(request->map_name()));
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::uploadMap(grpc::ServerContext*,
-                                           const api::AgvMapCommand_Request* request,
-                                           api::AgvMapCommand_Feedback* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
-        if (!agv) {
-            return setDeviceNotFound(response, device_id);
-        }
-        return setResponseResult(response, agv->uploadMap(request->map_name(), request->content()));
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::downloadMap(grpc::ServerContext*,
-                                             const api::AgvMapCommand_Request* request,
-                                             api::AgvMapCommand_Feedback* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
-        if (!agv) {
-            return setDeviceNotFound(response, device_id);
-        }
-        std::string content;
-        const auto result = agv->downloadMap(request->map_name(), content);
-        if (result.ok()) {
-            response->set_content(content);
-        }
-        return setResponseResult(response, result);
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::startMapping(grpc::ServerContext*,
-                                              const api::AgvStartMappingCommand_Request* request,
-                                              api::AgvStartMappingCommand_Feedback* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
-        if (!agv) {
-            return setDeviceNotFound(response, device_id);
-        }
-        device::AgvMappingOptions options;
-        options.dimension = toMapDimension(request->dimension());
-        options.map_name = request->map_name();
-        options.real_time = request->real_time();
-        const auto result = agv->startMapping(options);
-        if (result.ok()) {
-            response->set_session_id(device_id + "_mapping");
-        }
-        return setResponseResult(response, result);
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::streamMap(grpc::ServerContext* context,
-                                           const api::AgvMapStreamCommand_Request* request,
-                                           grpc::ServerWriter<api::AgvMapStreamCommand_Feedback>* writer)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
-
-        api::AgvMapStreamCommand_Feedback feedback;
-        if (!agv) {
-            const std::string message = "AGV device not found: " + device_id;
-            fillFeedback(feedback.mutable_header(), false, message);
-            writer->Write(feedback);
-            return grpc::Status(grpc::StatusCode::NOT_FOUND, message);
-        }
-
-        device::AgvMapStreamOptions options;
-        options.dimension = toMapDimension(request->dimension());
-        options.map_name = request->map_name();
-        options.resume_token = request->resume_token();
-        options.snapshot = request->snapshot();
-        options.incremental = request->incremental();
-        options.max_chunk_bytes = request->max_chunk_bytes();
-
-        std::uint64_t after_sequence = 0;
-        if (!request->resume_token().empty()) {
-            try {
-                after_sequence = static_cast<std::uint64_t>(std::stoull(request->resume_token()));
-            } catch (...) {
-                after_sequence = 0;
-            }
-        }
-
-        bool wrote_any = false;
-        while (!context->IsCancelled()) {
-            options.wait_timeout_ms = (!options.incremental && wrote_any) ? 20 : 1000;
-            device::AgvUnifiedMapUpdate update;
-            const auto result = agv->getUnifiedMapUpdate(after_sequence, options, update);
-            if (!result.ok()) {
-                if (result.code == device::AgvErrorCode::Timeout && wrote_any && !options.incremental) {
-                    return grpc::Status::OK;
-                }
-                if (result.code == device::AgvErrorCode::Timeout && wrote_any && options.incremental) {
-                    continue;
-                }
-                fillFeedback(feedback.mutable_header(), false, result.message);
-                writer->Write(feedback);
-                return resultToStatus(result);
-            }
-
-            api::AgvMapStreamCommand_Feedback update_feedback;
-            fillFeedback(update_feedback.mutable_header(), true);
-            fillUnifiedMapUpdate(update_feedback.mutable_update(), update);
-            if (!writer->Write(update_feedback)) {
-                return grpc::Status(grpc::StatusCode::CANCELLED, "AGV map stream writer closed");
-            }
-            wrote_any = true;
-            after_sequence = update.sequence;
-            options.resume_token.clear();
-        }
-
-        return grpc::Status(grpc::StatusCode::CANCELLED, "AGV map stream cancelled");
-    } catch (const std::exception& e) {
-        api::AgvMapStreamCommand_Feedback feedback;
-        fillFeedback(feedback.mutable_header(), false, e.what());
-        writer->Write(feedback);
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCAgvServiceImpl::stopMapping(grpc::ServerContext*,
-                                             const api::CommandHeader_Request* request,
-                                             api::CommandHeader_Feedback* response)
-{
-    try {
-        auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
-        if (!agv) {
-            return setDeviceNotFound(response, request->device_id());
-        }
-        return setResponseResult(response, agv->stopMapping());
-    } catch (const std::exception& e) {
-        fillFeedback(response, false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-} // namespace cmvr::service

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

@@ -1,109 +0,0 @@
-#include <grpcpp/grpcpp.h>
-#include "common/base/logging/logger.h"
-#include <gtest/gtest.h>
-#include <google/protobuf/util/time_util.h>
-
-#include "cmvr/api/arm_service.grpc.pb.h"
-
-using google::protobuf::util::TimeUtil;
-
-namespace {
-
-std::unique_ptr<cmvr::api::ArmService::Stub> makeStub()
-{
-    auto channel = grpc::CreateChannel("0.0.0.0:50055", grpc::InsecureChannelCredentials());
-    return cmvr::api::ArmService::NewStub(channel);
-}
-
-void fillHeader(cmvr::api::CommandHeader_Request* header)
-{
-    header->set_device_id("right_arm");
-    *header->mutable_timestamp() = TimeUtil::GetCurrentTime();
-}
-
-} // namespace
-
-TEST(GrpcArmClientTest, TorqueOn)
-{
-    auto stub = makeStub();
-    cmvr::api::CommandHeader_Request request;
-    fillHeader(&request);
-
-    cmvr::api::CommandHeader_Feedback response;
-    grpc::ClientContext context;
-    const grpc::Status status = stub->torqueOn(&context, request, &response);
-
-    if (!status.ok()) {
-        CMVR_LOG(ERROR) << "RPC failed: " << status.error_message();
-    }
-}
-
-TEST(GrpcArmClientTest, MoveJ)
-{
-    auto stub = makeStub();
-    cmvr::api::MoveJ_Request request;
-    fillHeader(request.mutable_header());
-
-    const std::vector<double> q{
-        0.00203898, 1.34062, 0.0, 0.322261, 0.0, -0.000210733, -0.0942364
-    };
-    for (double value : q) {
-        request.mutable_target()->add_position(value);
-    }
-    request.mutable_options()->set_velocity(0.8);
-    request.mutable_options()->set_acceleration(0.8);
-
-    cmvr::api::MoveJ_Response response;
-    grpc::ClientContext context;
-    const grpc::Status status = stub->moveJ(&context, request, &response);
-
-    if (!status.ok()) {
-        CMVR_LOG(ERROR) << "RPC failed: " << status.error_message();
-    }
-}
-
-TEST(GrpcArmClientTest, GetJointState)
-{
-    auto stub = makeStub();
-    cmvr::api::JointRequest request;
-    fillHeader(request.mutable_header());
-
-    cmvr::api::JointResponse response;
-    grpc::ClientContext context;
-    const grpc::Status status = stub->getJointState(&context, request, &response);
-
-    if (status.ok()) {
-        const auto& state = response.state();
-        for (int i = 0; i < state.name_size() && i < state.position_size(); ++i) {
-            CMVR_LOG(INFO) << "Joint: " << state.name(i)
-                      << ", Position: " << state.position(i);
-        }
-    } else {
-        CMVR_LOG(ERROR) << "RPC failed: " << status.error_message();
-    }
-}
-
-TEST(GrpcArmClientTest, GetRobotState)
-{
-    auto stub = makeStub();
-    cmvr::api::GetRobotState_Request request;
-    fillHeader(request.mutable_header());
-
-    cmvr::api::GetRobotState_Response response;
-    grpc::ClientContext context;
-    const grpc::Status status = stub->getRobotState(&context, request, &response);
-
-    if (status.ok()) {
-        const auto& state = response.state();
-        CMVR_LOG(INFO) << "Robot state: connected=" << state.connected()
-                       << ", powered_on=" << state.powered_on()
-                       << ", moving=" << state.moving()
-                       << ", fault=" << state.fault()
-                       << ", robot_mode=" << state.robot_mode()
-                       << ", safety_mode=" << state.safety_mode()
-                       << ", control_mode=" << state.control_mode()
-                       << ", joints=" << state.actual_joint_state().name_size();
-    } else {
-        CMVR_LOG(ERROR) << "RPC failed: " << status.error_message();
-    }
-}

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

@@ -1,573 +0,0 @@
-#include "service/grpc/include/grpc_arm_service.h"
-
-#include <google/protobuf/util/time_util.h>
-
-#include "common/base/logging/logger.h"
-
-using google::protobuf::util::TimeUtil;
-
-namespace cmvr::service {
-
-namespace {
-
-void fillFeedback(api::CommandHeader_Feedback* feedback,
-                  const bool success,
-                  const std::string& message = {})
-{
-    feedback->set_success(success);
-    feedback->set_error_message(message);
-    *feedback->mutable_timestamp() = TimeUtil::GetCurrentTime();
-}
-
-grpc::Status resultToStatus(const device::Result& result)
-{
-    if (result.ok()) {
-        return grpc::Status::OK;
-    }
-    return grpc::Status(grpc::StatusCode::INTERNAL, result.message);
-}
-
-void logRpcSuccess(const char* rpc_name, const std::string& device_id)
-{
-    CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (" << rpc_name
-                    << "): success, id=" << device_id;
-}
-
-device::FrameType toFrameType(const api::ArmFrameType frame)
-{
-    switch (frame) {
-        case api::ARM_FRAME_TOOL:
-            return device::FrameType::Tool;
-        case api::ARM_FRAME_WORLD:
-            return device::FrameType::World;
-        case api::ARM_FRAME_USER:
-            return device::FrameType::User;
-        case api::ARM_FRAME_BASE:
-        default:
-            return device::FrameType::Base;
-    }
-}
-
-device::JointPositionCommand toJointPositionCommand(const api::JointPositionCommand& src)
-{
-    device::JointPositionCommand dst;
-    dst.position.assign(src.position().begin(), src.position().end());
-    return dst;
-}
-
-device::JointVelocityCommand toJointVelocityCommand(const api::JointVelocityCommand& src)
-{
-    device::JointVelocityCommand dst;
-    dst.velocity.assign(src.velocity().begin(), src.velocity().end());
-    return dst;
-}
-
-device::MotionOptions toMotionOptions(const api::MotionOptions& src)
-{
-    device::MotionOptions dst;
-    dst.velocity = src.velocity();
-    dst.acceleration = src.acceleration();
-    dst.blend_radius = src.blend_radius();
-    dst.jerk = src.jerk() > 0.0 ? src.jerk() : 5.0;
-    dst.joint_velocity_limits.assign(src.joint_velocity_limits().begin(),
-                                     src.joint_velocity_limits().end());
-    dst.asynchronous = src.asynchronous();
-    return dst;
-}
-
-device::CartesianPose toCartesianPose(const api::CartesianPose& src)
-{
-    return {src.x(), src.y(), src.z(), src.rx(), src.ry(), src.rz()};
-}
-
-api::CartesianPose toApiCartesianPose(const device::CartesianPose& src)
-{
-    api::CartesianPose dst;
-    dst.set_x(src.x);
-    dst.set_y(src.y);
-    dst.set_z(src.z);
-    dst.set_rx(src.rx);
-    dst.set_ry(src.ry);
-    dst.set_rz(src.rz);
-    return dst;
-}
-
-api::CartesianVelocity toApiCartesianVelocity(const device::CartesianVelocity& src)
-{
-    api::CartesianVelocity dst;
-    dst.set_vx(src.vx);
-    dst.set_vy(src.vy);
-    dst.set_vz(src.vz);
-    dst.set_wx(src.wx);
-    dst.set_wy(src.wy);
-    dst.set_wz(src.wz);
-    return dst;
-}
-
-api::CartesianWrench toApiCartesianWrench(const device::CartesianWrench& src)
-{
-    api::CartesianWrench dst;
-    dst.set_fx(src.fx);
-    dst.set_fy(src.fy);
-    dst.set_fz(src.fz);
-    dst.set_tx(src.tx);
-    dst.set_ty(src.ty);
-    dst.set_tz(src.tz);
-    return dst;
-}
-
-api::ArmRobotMode toApiRobotMode(const device::RobotMode mode)
-{
-    switch (mode) {
-        case device::RobotMode::Disconnected:
-            return api::ARM_ROBOT_MODE_DISCONNECTED;
-        case device::RobotMode::PowerOff:
-            return api::ARM_ROBOT_MODE_POWER_OFF;
-        case device::RobotMode::Idle:
-            return api::ARM_ROBOT_MODE_IDLE;
-        case device::RobotMode::Running:
-            return api::ARM_ROBOT_MODE_RUNNING;
-        case device::RobotMode::Paused:
-            return api::ARM_ROBOT_MODE_PAUSED;
-        case device::RobotMode::Stopped:
-            return api::ARM_ROBOT_MODE_STOPPED;
-        case device::RobotMode::Fault:
-            return api::ARM_ROBOT_MODE_FAULT;
-        case device::RobotMode::Unknown:
-        default:
-            return api::ARM_ROBOT_MODE_UNKNOWN;
-    }
-}
-
-api::ArmSafetyMode toApiSafetyMode(const device::SafetyMode mode)
-{
-    switch (mode) {
-        case device::SafetyMode::Normal:
-            return api::ARM_SAFETY_MODE_NORMAL;
-        case device::SafetyMode::Reduced:
-            return api::ARM_SAFETY_MODE_REDUCED;
-        case device::SafetyMode::ProtectiveStop:
-            return api::ARM_SAFETY_MODE_PROTECTIVE_STOP;
-        case device::SafetyMode::EmergencyStop:
-            return api::ARM_SAFETY_MODE_EMERGENCY_STOP;
-        case device::SafetyMode::SafeguardStop:
-            return api::ARM_SAFETY_MODE_SAFEGUARD_STOP;
-        case device::SafetyMode::SystemEmergencyStop:
-            return api::ARM_SAFETY_MODE_SYSTEM_EMERGENCY_STOP;
-        case device::SafetyMode::Fault:
-            return api::ARM_SAFETY_MODE_FAULT;
-        case device::SafetyMode::Unknown:
-        default:
-            return api::ARM_SAFETY_MODE_UNKNOWN;
-    }
-}
-
-api::ArmControlMode toApiControlMode(const device::ControlMode mode)
-{
-    switch (mode) {
-        case device::ControlMode::Manual:
-            return api::ARM_CONTROL_MODE_MANUAL;
-        case device::ControlMode::Position:
-            return api::ARM_CONTROL_MODE_POSITION;
-        case device::ControlMode::Velocity:
-            return api::ARM_CONTROL_MODE_VELOCITY;
-        case device::ControlMode::Torque:
-            return api::ARM_CONTROL_MODE_TORQUE;
-        case device::ControlMode::Servo:
-            return api::ARM_CONTROL_MODE_SERVO;
-        case device::ControlMode::Freedrive:
-            return api::ARM_CONTROL_MODE_FREEDRIVE;
-        case device::ControlMode::None:
-        default:
-            return api::ARM_CONTROL_MODE_NONE;
-    }
-}
-
-void fillJointState(const device::RobotModel& model,
-                    const device::JointGroupState& state,
-                    api::JointState* msg)
-{
-    for (const auto& name : model.joint_names) msg->add_name(name);
-    for (const double value : state.position) msg->add_position(value);
-    for (const double value : state.velocity) msg->add_velocity(value);
-    for (const double value : state.effort) msg->add_effort(value);
-}
-
-void fillRobotState(const device::RobotModel& model,
-                    const device::ArmState& state,
-                    api::RobotState* msg)
-{
-    msg->set_timestamp(state.timestamp);
-    msg->set_robot_mode(toApiRobotMode(state.robot_mode));
-    msg->set_safety_mode(toApiSafetyMode(state.safety_mode));
-    msg->set_control_mode(toApiControlMode(state.control_mode));
-    msg->set_connected(state.connected);
-    msg->set_powered_on(state.powered_on);
-    msg->set_brake_released(state.brake_released);
-    msg->set_moving(state.moving);
-    msg->set_program_running(state.program_running);
-    msg->set_protective_stopped(state.protective_stopped);
-    msg->set_emergency_stopped(state.emergency_stopped);
-    msg->set_fault(state.fault);
-    msg->set_speed_scaling(state.speed_scaling);
-    fillJointState(model, state.actual_joint_state, msg->mutable_actual_joint_state());
-    fillJointState(model, state.target_joint_state, msg->mutable_target_joint_state());
-    *msg->mutable_actual_tcp_pose() = toApiCartesianPose(state.actual_tcp_pose);
-    *msg->mutable_actual_tcp_velocity() = toApiCartesianVelocity(state.actual_tcp_velocity);
-    *msg->mutable_actual_tcp_wrench() = toApiCartesianWrench(state.actual_tcp_wrench);
-}
-
-device::CartesianVelocity toCartesianVelocity(const api::CartesianVelocity& src)
-{
-    return {src.vx(), src.vy(), src.vz(), src.wx(), src.wy(), src.wz()};
-}
-
-template <typename Response>
-grpc::Status setResponseResult(Response* response, const device::Result& result)
-{
-    fillFeedback(response->mutable_header(), result.ok(), result.ok() ? "" : result.message);
-    return resultToStatus(result);
-}
-
-grpc::Status setDeviceNotFound(api::CommandHeader_Feedback* response, const std::string& device_id)
-{
-    const std::string message = "RobotArm device not found: " + device_id;
-    fillFeedback(response, false, message);
-    return grpc::Status(grpc::StatusCode::NOT_FOUND, message);
-}
-
-template <typename Response>
-grpc::Status setDeviceNotFound(Response* response, const std::string& device_id)
-{
-    const std::string message = "RobotArm device not found: " + device_id;
-    fillFeedback(response->mutable_header(), false, message);
-    return grpc::Status(grpc::StatusCode::NOT_FOUND, message);
-}
-
-} // namespace
-
-gRPCArmServiceImpl::gRPCArmServiceImpl()
-    : dmgr_(device::DeviceManager::getInstance())
-{
-}
-
-grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*,
-                                           const api::CommandHeader_Request* request,
-                                           api::CommandHeader_Feedback* response)
-{
-    try {
-        const std::string device_id = request->device_id();
-        auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
-        if (!arm) {
-            return setDeviceNotFound(response, device_id);
-        }
-        const auto result = arm->torqueOff();
-        fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
-        if (result.ok()) {
-            logRpcSuccess("torqueOff", device_id);
-        }
-        return resultToStatus(result);
-    } catch (const std::exception& e) {
-        fillFeedback(response, false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*,
-                                          const api::CommandHeader_Request* request,
-                                          api::CommandHeader_Feedback* response)
-{
-    try {
-        const std::string device_id = request->device_id();
-        auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
-        if (!arm) {
-            return setDeviceNotFound(response, device_id);
-        }
-        const auto result = arm->torqueOn();
-        fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
-        if (result.ok()) {
-            logRpcSuccess("torqueOn", device_id);
-        }
-        return resultToStatus(result);
-    } catch (const std::exception& e) {
-        fillFeedback(response, false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*,
-                                       const api::MoveJ_Request* request,
-                                       api::MoveJ_Response* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
-        if (!arm) {
-            return setDeviceNotFound(response, device_id);
-        }
-        const auto result = arm->moveJ(toJointPositionCommand(request->target()),
-                                       toMotionOptions(request->options()));
-        if (result.ok()) {
-            CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveJ): success, id=" << device_id
-                            << ", positions=" << request->target().position_size();
-        }
-        return setResponseResult(response, result);
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*,
-                                       const api::MoveL_Request* request,
-                                       api::MoveL_Response* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
-        if (!arm) {
-            return setDeviceNotFound(response, device_id);
-        }
-        const auto result = arm->moveL(toCartesianPose(request->target()),
-                                       toMotionOptions(request->options()),
-                                       toFrameType(request->frame()));
-        if (result.ok()) {
-            CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveL): success, id=" << device_id
-                            << ", frame=" << request->frame();
-        }
-        return setResponseResult(response, result);
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*,
-                                        const api::SpeedJ_Request* request,
-                                        api::SpeedJ_Response* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
-        if (!arm) {
-            return setDeviceNotFound(response, device_id);
-        }
-        const auto result = arm->speedJ(toJointVelocityCommand(request->velocity()),
-                                        request->acceleration(),
-                                        request->duration());
-        if (result.ok()) {
-            CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (speedJ): success, id=" << device_id
-                            << ", velocities=" << request->velocity().velocity_size()
-                            << ", acceleration=" << request->acceleration()
-                            << ", duration=" << request->duration();
-        }
-        return setResponseResult(response, result);
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*,
-                                        const api::SpeedL_Request* request,
-                                        api::SpeedL_Response* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
-        if (!arm) {
-            return setDeviceNotFound(response, device_id);
-        }
-        const auto result = arm->speedL(toCartesianVelocity(request->velocity()),
-                                        request->acceleration(),
-                                        request->duration(),
-                                        toFrameType(request->frame()));
-        if (result.ok()) {
-            CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (speedL): success, id=" << device_id
-                            << ", acceleration=" << request->acceleration()
-                            << ", duration=" << request->duration()
-                            << ", frame=" << request->frame();
-        }
-        return setResponseResult(response, result);
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext*,
-                                        const api::ServoJ_Request* request,
-                                        api::ServoJ_Response* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
-        if (!arm) {
-            return setDeviceNotFound(response, device_id);
-        }
-        const auto result = arm->servoJ(toJointPositionCommand(request->target()));
-        if (result.ok()) {
-            CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (servoJ): success, id=" << device_id
-                            << ", positions=" << request->target().position_size();
-        }
-        return setResponseResult(response, result);
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*,
-                                            const api::CommandHeader_Request* request,
-                                            api::CommandHeader_Feedback* response)
-{
-    try {
-        const std::string device_id = request->device_id();
-        auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
-        if (!arm) {
-            return setDeviceNotFound(response, device_id);
-        }
-        const auto result = arm->stopMotion();
-        fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
-        if (result.ok()) {
-            logRpcSuccess("stopMotion", device_id);
-        }
-        return resultToStatus(result);
-    } catch (const std::exception& e) {
-        fillFeedback(response, false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCArmServiceImpl::getJointState(grpc::ServerContext*,
-                                               const api::JointRequest* request,
-                                               api::JointResponse* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
-        if (!arm) {
-            return setDeviceNotFound(response, device_id);
-        }
-        const auto model = arm->getRobotModel();
-        const auto state = arm->getJointState();
-        auto* msg = response->mutable_state();
-        fillJointState(model, state, msg);
-        fillFeedback(response->mutable_header(), true);
-        // CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (getJointState): success, id=" << device_id
-        //                 << ", joints=" << msg->name_size()
-        //                 << ", positions=" << msg->position_size();
-        return grpc::Status::OK;
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCArmServiceImpl::getRobotState(
-    grpc::ServerContext*,
-    const api::GetRobotState_Request* request,
-    api::GetRobotState_Response* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
-        if (!arm) {
-            return setDeviceNotFound(response, device_id);
-        }
-
-        const auto model = arm->getRobotModel();
-        const auto state = arm->getRobotState();
-        fillRobotState(model, state, response->mutable_state());
-        fillFeedback(response->mutable_header(), true);
-        logRpcSuccess("getRobotState", device_id);
-        return grpc::Status::OK;
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCArmServiceImpl::getPose(grpc::ServerContext*,
-                                         const api::GetPose_Request* request,
-                                         api::GetPose_Response* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
-        if (!arm) {
-            return setDeviceNotFound(response, device_id);
-        }
-        const auto pose = request->base_link().empty() || request->ee_link().empty()
-            ? arm->fk(true)
-            : arm->fk(request->base_link(), request->ee_link());
-        *response->mutable_pose() = toApiCartesianPose(pose);
-        fillFeedback(response->mutable_header(), true);
-        CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (getPose): success, id=" << device_id
-                        << ", pose=(" << pose.x << ", " << pose.y << ", " << pose.z
-                        << ", " << pose.rx << ", " << pose.ry << ", " << pose.rz << ")";
-        return grpc::Status::OK;
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*,
-                                                const api::CalibrateZeroQ_Request* request,
-                                                api::CalibrateZeroQ_Response* response)
-{
-    try {
-        const std::string device_id = request->header().device_id();
-        auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
-        if (!arm) {
-            return setDeviceNotFound(response, device_id);
-        }
-        const auto result = arm->calibrateZeroQ(request->joint_name());
-        if (result.ok()) {
-            CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (calibrateZeroQ): success, id=" << device_id
-                            << ", joint=" << request->joint_name();
-        }
-        return setResponseResult(response, result);
-    } catch (const std::exception& e) {
-        fillFeedback(response->mutable_header(), false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-
-grpc::Status gRPCArmServiceImpl::getPoseMatrix(grpc::ServerContext*,
-                                               const api::GetPoseMatrix_Request*,
-                                               api::GetPoseMatrix_Response* response)
-{
-    fillFeedback(response->mutable_header(), false, "getPoseMatrix is not implemented");
-    return grpc::Status(grpc::StatusCode::UNIMPLEMENTED, "getPoseMatrix is not implemented");
-}
-
-grpc::Status gRPCArmServiceImpl::computeForwardKinematics(grpc::ServerContext*,
-                                                          const api::ComputeForwardKinematics_Request*,
-                                                          api::ComputeForwardKinematics_Response* response)
-{
-    fillFeedback(response->mutable_header(), false, "computeForwardKinematics is not implemented");
-    return grpc::Status(grpc::StatusCode::UNIMPLEMENTED, "computeForwardKinematics is not implemented");
-}
-
-grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context,
-                                            const cmvr::api::CommandHeader_Request *request,
-                                            cmvr::api::CommandHeader_Feedback *response)
-{
-    try {
-        const std::string device_id = request->device_id();
-        auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
-        if (!arm) {
-            return setDeviceNotFound(response, device_id);
-        }
-        const auto result = arm->clearFault();
-        fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
-        return resultToStatus(result);
-    } catch (const std::exception& e) {
-        fillFeedback(response, false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}
-} // namespace cmvr::service
-

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

@@ -1,935 +0,0 @@
-#include "common/base/logging/logger.h"
-#include "manager/media_source_hub/include/device_media_source_adapter.h"
-//
-// Created by xtkuang on 2025/6/1.
-//
-
-#include "../include/grpc_camera_service.h"
-
-#include <algorithm>
-#include <atomic>
-#include <chrono>
-#include <cstdint>
-#include <limits>
-#include <thread>
-#include <utility>
-
-using namespace std;
-using namespace cmvr::service;
-using namespace cmvr::device;
-
-namespace {
-template <typename ResponseT>
-grpc::Status failResponse(ResponseT* response, const std::string& message) {
-    CMVR_LOG(ERROR) << "[gRPCCameraServiceImpl] " << message;
-    response->mutable_header()->set_success(false);
-    response->mutable_header()->set_error_message(message);
-    setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-    return grpc::Status::OK;
-}
-
-bool toPtzCommand(cmvr::api::ControlPtzCommand_Command command, PtzCommand& out)
-{
-    switch (command) {
-        case cmvr::api::ControlPtzCommand_Command_TILT_UP:
-            out = PtzCommand::TiltUp;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_TILT_DOWN:
-            out = PtzCommand::TiltDown;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_PAN_LEFT:
-            out = PtzCommand::PanLeft;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_PAN_RIGHT:
-            out = PtzCommand::PanRight;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_UP_LEFT:
-            out = PtzCommand::UpLeft;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_UP_RIGHT:
-            out = PtzCommand::UpRight;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_DOWN_LEFT:
-            out = PtzCommand::DownLeft;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_DOWN_RIGHT:
-            out = PtzCommand::DownRight;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_ZOOM_IN:
-            out = PtzCommand::ZoomIn;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_ZOOM_OUT:
-            out = PtzCommand::ZoomOut;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_PAN_AUTO:
-            out = PtzCommand::PanAuto;
-            return true;
-        default:
-            return false;
-    }
-}
-
-// The legacy depth/RGBD RPCs acquire the camera's shared producer directly
-// instead of going through MediaSourceHub.  Keep that lease exception-safe:
-// cancellation, a failed Write(), or any conversion error must release exactly
-// the one startStreaming() reference acquired by this call.
-class CameraStreamingLease final {
-public:
-    explicit CameraStreamingLease(std::shared_ptr<AbstractCamera> camera)
-        : camera_(std::move(camera)) {
-        active_ = camera_ && camera_->startStreaming();
-    }
-
-    ~CameraStreamingLease() {
-        if (!active_ || !camera_) {
-            return;
-        }
-        try {
-            camera_->stopStreaming();
-        } catch (const std::exception& e) {
-            CMVR_LOG(ERROR) << "[gRPCCameraServiceImpl] failed to release camera stream lease: "
-                            << e.what();
-        } catch (...) {
-            CMVR_LOG(ERROR) << "[gRPCCameraServiceImpl] failed to release camera stream lease";
-        }
-    }
-
-    CameraStreamingLease(const CameraStreamingLease&) = delete;
-    CameraStreamingLease& operator=(const CameraStreamingLease&) = delete;
-
-    explicit operator bool() const noexcept { return active_; }
-
-private:
-    std::shared_ptr<AbstractCamera> camera_;
-    bool active_{false};
-};
-}
-
-gRPCCameraServiceImpl::gRPCCameraServiceImpl(
-    CameraStreamLowLatencyConfig stream_config)
-    : dmgr_(DeviceManager::getInstance()),
-      stream_config_(stream_config) {}
-
-grpc::Status gRPCCameraServiceImpl::GetStatus(grpc::ServerContext* context,
-    const api::GetCameraStateCommand_Request* request, api::GetCameraStateCommand_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetStatus): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Camera device not found: " + dev_id);
-        }
-        CameraState state{};
-        dev->getState(state);
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        response->mutable_state()->set_is_initialized(state.is_initialized);
-        response->mutable_state()->set_is_opened(state.is_opened);
-        response->mutable_state()->set_is_streaming(state.is_streaming);
-        response->mutable_state()->set_is_recording(state.is_recording);
-        response->mutable_state()->set_is_error(state.is_error);
-        response->mutable_state()->set_error_message(state.error_message);
-        response->mutable_state()->set_fps(state.fps);
-        response->mutable_state()->set_width(state.width);
-        response->mutable_state()->set_height(state.height);
-        return grpc::Status::OK;
-    }
-    catch(const exception &e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCCameraServiceImpl::StartCamera(grpc::ServerContext* context,
-    const api::StartCameraCommand_Request* request, api::StartCameraCommand_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StartCamera): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Camera device not found: " + dev_id);
-        }
-        if (!dev->start()) {
-            return failResponse(response, "Failed to start camera: " + dev_id);
-        }
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-    catch (const exception &e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCCameraServiceImpl::StopCamera(grpc::ServerContext* context,
-    const api::StopCameraCommand_Request* request, api::StopCameraCommand_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StopCamera): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Camera device not found: " + dev_id);
-        }
-        if (!dev->stop()) {
-            return failResponse(response, "Failed to stop camera: " + dev_id);
-        }
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-    catch (exception &e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
-    const api::GetRGBImageCommand_Request* request, api::GetRGBImageCommand_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImage): id=" << dev_id;
-        cv::Mat image;
-        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Camera device not found: " + dev_id);
-        }
-        Rs2Intrinsics intrinsics = {0};
-        dev->getRGBImage(image,intrinsics);
-        if (image.empty()) {
-            return failResponse(response, "Camera returned an empty RGB image: " + dev_id);
-        }
-
-        response->mutable_intrinsics()->set_fx(intrinsics.fx);
-        response->mutable_intrinsics()->set_fy(intrinsics.fy);
-        response->mutable_intrinsics()->set_cx(intrinsics.cx);
-        response->mutable_intrinsics()->set_cy(intrinsics.cy);
-        for (int i = 0; i < 5 ; i++) {
-            response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]);
-        }
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-
-        // Keep the single-frame contract explicit: FrameData::U8C3 is RGB
-        // byte order, while OpenCV/RealSense images are normally BGR.
-        if (image.type() == CV_8UC3) {
-            cv::Mat rgb_image;
-            cv::cvtColor(image, rgb_image, cv::COLOR_BGR2RGB);
-            image = std::move(rgb_image);
-        }
-        auto imageType = image.type();
-        if (imageType == CV_8UC1) {
-            response->mutable_color_frame()->set_type(api::FrameData::U8C1);
-        }
-        else if (imageType == CV_8UC3) {
-            response->mutable_color_frame()->set_type(api::FrameData::U8C3);
-        }
-        else if (imageType == CV_16UC3) {
-            response->mutable_color_frame()->set_type(api::FrameData::U16C3);
-        }
-        else {
-            return failResponse(response, "unsupported image type");
-        }
-        response->mutable_color_frame()->set_data(
-            reinterpret_cast<const char*>(image.data), image.total() * image.elemSize());
-        response->mutable_color_frame()->set_height(image.rows);
-        response->mutable_color_frame()->set_width(image.cols);
-        response->mutable_color_frame()->set_codec("none");
-        return grpc::Status::OK;
-    }
-    catch (exception &e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context,
-    const api::GetDepthImageCommand_Request* request, api::GetDepthImageCommand_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImage): id=" << dev_id;
-        cv::Mat image;
-        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Camera device not found: " + dev_id);
-        }
-        Rs2Intrinsics intrinsics = {0};
-        dev->getDepthImage(image,intrinsics);
-        if (image.empty()) {
-            return failResponse(response, "Camera returned an empty depth image: " + dev_id);
-        }
-        response->mutable_header()->set_success(true);
-
-        response->mutable_intrinsics()->set_fx(intrinsics.fx);
-        response->mutable_intrinsics()->set_fy(intrinsics.fy);
-        response->mutable_intrinsics()->set_cx(intrinsics.cx);
-        response->mutable_intrinsics()->set_cy(intrinsics.cy);
-        for (int i = 0; i < 5 ; i++) {
-            response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]);
-        }
-
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-
-        if (image.type() == CV_8UC1) {
-            response->mutable_depth_frame()->set_type(api::FrameData::U8C1);
-        }
-        else if (image.type() == CV_16UC1) {
-            response->mutable_depth_frame()->set_type(api::FrameData::U16C1);
-        }
-        else if (image.type() == CV_16FC1) {
-            response->mutable_depth_frame()->set_type(api::FrameData::F16C1);
-        }
-        else if (image.type() == CV_32FC1) {
-            response->mutable_depth_frame()->set_type(api::FrameData::F32C1);
-        }
-        else {
-            return failResponse(response, "unsupported image type");
-        }
-
-        response->mutable_depth_frame()->set_data(
-            reinterpret_cast<const char*>(image.data), image.total() * image.elemSize());
-        response->mutable_depth_frame()->set_height(image.rows);
-        response->mutable_depth_frame()->set_width(image.cols);
-        response->mutable_depth_frame()->set_codec("none");
-        return grpc::Status::OK;
-    }
-    catch (exception &e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context,
-    const api::GetRGBDImagesCommand_Request* request, api::GetRGBDImagesCommand_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImages): id=" << dev_id;
-        cv::Mat color_image, depth_image;
-        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Camera device not found: " + dev_id);
-        }
-        Rs2Intrinsics intrinsics = {0};
-        dev->getRGBDImages(color_image,depth_image, intrinsics);
-        if (color_image.empty()) {
-            return failResponse(response, "Camera returned an empty RGB image: " + dev_id);
-        }
-        if (depth_image.empty()) {
-            return failResponse(response, "Camera returned an empty depth image: " + dev_id);
-        }
-        response->mutable_header()->set_success(true);
-
-        response->mutable_intrinsics()->set_fx(intrinsics.fx);
-        response->mutable_intrinsics()->set_fy(intrinsics.fy);
-        response->mutable_intrinsics()->set_cx(intrinsics.cx);
-        response->mutable_intrinsics()->set_cy(intrinsics.cy);
-        for (int i = 0; i < 5 ; i++) {
-            response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]);
-        }
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-
-        // Keep the single-frame contract explicit: FrameData::U8C3 is RGB
-        // byte order, while OpenCV/RealSense images are normally BGR.
-        if (color_image.type() == CV_8UC3) {
-            cv::Mat rgb_image;
-            cv::cvtColor(color_image, rgb_image, cv::COLOR_BGR2RGB);
-            color_image = std::move(rgb_image);
-            response->mutable_color_frame()->set_type(api::FrameData::U8C3);
-        }
-        else if (color_image.type() == CV_16UC3) {
-            response->mutable_color_frame()->set_type(api::FrameData::U16C3);
-        }
-        else {
-            return failResponse(response, "unsupported image type");
-        }
-        response->mutable_color_frame()->set_data(
-            reinterpret_cast<const char*>(color_image.data), color_image.total() * color_image.elemSize());
-        response->mutable_color_frame()->set_height(color_image.rows);
-        response->mutable_color_frame()->set_width(color_image.cols);
-        response->mutable_color_frame()->set_codec("none");
-
-        // depth image
-        if (depth_image.type() == CV_8UC1) {
-            response->mutable_depth_frame()->set_type(api::FrameData::U8C1);
-        }
-        else if (depth_image.type() == CV_16UC1) {
-            response->mutable_depth_frame()->set_type(api::FrameData::U16C1);
-        }
-        else if (depth_image.type() == CV_16FC1) {
-            response->mutable_depth_frame()->set_type(api::FrameData::F16C1);
-        }
-        else if (depth_image.type() == CV_32FC1) {
-            response->mutable_depth_frame()->set_type(api::FrameData::F32C1);
-        }
-        else {
-            return failResponse(response, "unsupported depth image type");
-        }
-        response->mutable_depth_frame()->set_data(
-            reinterpret_cast<const char*>(depth_image.data), depth_image.total() * depth_image.elemSize());
-        response->mutable_depth_frame()->set_height(depth_image.rows);
-        response->mutable_depth_frame()->set_width(depth_image.cols);
-        response->mutable_depth_frame()->set_codec("none");
-        return grpc::Status::OK;
-    }
-    catch (exception &e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCCameraServiceImpl::StartRecording(grpc::ServerContext* context,
-    const api::StartCameraRecordingCommand_Request* request, api::StartCameraRecordingCommand_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StartRecording): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Camera device not found: " + dev_id);
-        }
-        dev->startRecording(request->video_path());
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-    catch (exception &e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCCameraServiceImpl::StopRecording(grpc::ServerContext* context,
-    const api::StopCameraRecordingCommand_Request* request, api::StopCameraRecordingCommand_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StopRecording): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Camera device not found: " + dev_id);
-        }
-        dev->stopRecording();
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-    catch (exception &e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCCameraServiceImpl::ControlPtz(grpc::ServerContext* context,
-    const api::ControlPtzCommand_Request* request, api::ControlPtzCommand_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (ControlPtz): id=" << dev_id
-                       << ", command=" << request->command()
-                       << ", action=" << request->action()
-                       << ", speed=" << request->speed();
-        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Camera device not found: " + dev_id);
-        }
-
-        PtzCommand command{};
-        if (!toPtzCommand(request->command(), command)) {
-            return failResponse(response, "Invalid PTZ command");
-        }
-
-        if (request->action() != api::ControlPtzCommand_Action_START &&
-            request->action() != api::ControlPtzCommand_Action_STOP) {
-            return failResponse(response, "Invalid PTZ action");
-        }
-        const bool stop = request->action() == api::ControlPtzCommand_Action_STOP;
-        if (!dev->controlPtz(command, stop, static_cast<int>(request->speed()))) {
-            CameraState state{};
-            dev->getState(state);
-            const std::string error_message =
-                state.error_message.empty() ? "Failed to control PTZ: " + dev_id : state.error_message;
-            return failResponse(response, error_message);
-        }
-
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-    catch (exception &e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-
-grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* context
-    , grpc::ServerReaderWriter<cmvr::api::GetDepthImageStreamCommand_Feedback, cmvr::api::GetDepthImageStreamCommand_Request>* stream){
-    try {
-        //读取首次传递的数据，获取设备id
-        api::GetDepthImageStreamCommand_Request request;
-        if (!stream->Read(&request)) {
-            return grpc::Status::OK;
-        }
-        string dev_id = request.header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImageStream): start,id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
-        if (!dev) {
-            api::GetDepthImageStreamCommand_Feedback response;
-            response.mutable_header()->set_success(false);
-            response.mutable_header()->set_error_message("Camera device not found: " + dev_id);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-            stream->Write(response);
-            return grpc::Status::OK;
-        }
-        CameraStreamingLease stream_lease(dev);
-        if (!stream_lease) {
-            api::GetDepthImageStreamCommand_Feedback response;
-            response.mutable_header()->set_success(false);
-            response.mutable_header()->set_error_message("Failed to start camera stream: " + dev_id);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-            stream->Write(response);
-            return grpc::Status::OK;
-        }
-
-        int nFrameCount = 0;
-        size_t index = 0;
-        while (true)
-        {
-            if (context->IsCancelled())
-            {
-                CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled,id=" << dev_id;
-                break;
-            }
-
-            api::GetDepthImageStreamCommand_Feedback response;
-            cmvr::device::StreamFrameData frame_data;
-            if (dev->waitEncodedFrame(frame_data, index, std::chrono::milliseconds(100)) &&
-                !frame_data.depthFrame.empty()) {
-                response.mutable_header()->set_success(true);
-                setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-                response.mutable_depth_frame()->set_type(api::FrameData::U16C1);
-                response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size());
-                response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey);
-                response.mutable_depth_frame()->set_codec("none");
-                response.mutable_depth_frame()->set_width(frame_data.depth_width > 0 ? frame_data.depth_width : frame_data.width);
-                response.mutable_depth_frame()->set_height(frame_data.depth_height > 0 ? frame_data.depth_height : frame_data.height);
-
-                response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
-                response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy);
-                response.mutable_intrinsics()->set_cx(frame_data.intrinsics.cx);
-                response.mutable_intrinsics()->set_cy(frame_data.intrinsics.cy);
-                for (int i = 0; i < 5 ; i++) {
-                    response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]);
-                }
-
-                response.set_seq_no(nFrameCount++);
-
-                if (!stream->Write(response)) {
-                    CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id;
-                    break;
-                }
-            }
-        }
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImageStream): end,id=" << dev_id;
-        return grpc::Status::OK;
-    }
-    catch (const exception &e) {
-        api::GetDepthImageStreamCommand_Feedback response;
-        response.mutable_header()->set_success(false);
-        response.mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-        stream->Write(response);
-        return grpc::Status::OK;
-    }
-}
-grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* context
-    , grpc::ServerReaderWriter<cmvr::api::GetRGBDImagesStreamCommand_Feedback, cmvr::api::GetRGBDImagesStreamCommand_Request>* stream){
-    try {
-        //读取首次传递的数据，获取设备id
-        api::GetRGBDImagesStreamCommand_Request request;
-        if (!stream->Read(&request)) {
-            return grpc::Status::OK;
-        }
-        string dev_id = request.header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): start,id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
-        if (!dev) {
-            api::GetRGBDImagesStreamCommand_Feedback response;
-            response.mutable_header()->set_success(false);
-            response.mutable_header()->set_error_message("Camera device not found: " + dev_id);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-            stream->Write(response);
-            return grpc::Status::OK;
-        }
-        CameraStreamingLease stream_lease(dev);
-        if (!stream_lease) {
-            api::GetRGBDImagesStreamCommand_Feedback response;
-            response.mutable_header()->set_success(false);
-            response.mutable_header()->set_error_message("Failed to start camera stream: " + dev_id);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-            stream->Write(response);
-            return grpc::Status::OK;
-        }
-
-        int nFrameCount = 0;
-        size_t index = 0;
-        while (true)
-        {
-            if (context->IsCancelled())
-            {
-                CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBDImagesStream) context is cancelled,id=" << dev_id;
-                break;
-            }
-
-            api::GetRGBDImagesStreamCommand_Feedback response;
-            cmvr::device::StreamFrameData frame_data;
-            if (dev->waitEncodedFrame(frame_data, index, std::chrono::milliseconds(100)) &&
-                !frame_data.rgbFrame.empty() &&
-                !frame_data.depthFrame.empty()) {
-                response.mutable_header()->set_success(true);
-                setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-                response.mutable_color_frame()->set_data(frame_data.rgbFrame.data(), frame_data.rgbFrame.size());
-                response.mutable_color_frame()->set_is_key_frame(frame_data.bKey);
-                response.mutable_color_frame()->set_codec(frame_data.codec);
-                response.mutable_color_frame()->set_width(frame_data.width);
-                response.mutable_color_frame()->set_height(frame_data.height);
-
-                response.mutable_depth_frame()->set_type(api::FrameData::U16C1);
-                response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size());
-                response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey);
-                response.mutable_depth_frame()->set_codec("none");
-                response.mutable_depth_frame()->set_width(frame_data.depth_width > 0 ? frame_data.depth_width : frame_data.width);
-                response.mutable_depth_frame()->set_height(frame_data.depth_height > 0 ? frame_data.depth_height : frame_data.height);
-
-                response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
-                response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy);
-                response.mutable_intrinsics()->set_cx(frame_data.intrinsics.cx);
-                response.mutable_intrinsics()->set_cy(frame_data.intrinsics.cy);
-                for (int i = 0; i < 5 ; i++) {
-                    response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]);
-                }
-
-                response.set_seq_no(nFrameCount++);
-
-                if (!stream->Write(response)) {
-                    CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id;
-                    break;
-                }
-            }
-        }
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): end,id=" << dev_id;
-        return grpc::Status::OK;
-    }
-    catch (const exception &e) {
-        api::GetRGBDImagesStreamCommand_Feedback response;
-        response.mutable_header()->set_success(false);
-        response.mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-        stream->Write(response);
-        return grpc::Status::OK;
-    }
-}
-grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback, cmvr::api::GetRGBImageStreamCommand_Request>* stream){
-    try {
-        //读取首次传递的数据，获取设备id
-        api::GetRGBImageStreamCommand_Request request;
-        if (!stream->Read(&request)) {
-            return grpc::Status::OK;
-        }
-        string dev_id = request.header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id
-                       << ", max_pending_frames=" << stream_config_.max_pending_frames
-                       << ", max_frame_age_ms=" << stream_config_.max_frame_age.count();
-        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
-        if (!dev) {
-            api::GetRGBImageStreamCommand_Feedback response;
-            response.mutable_header()->set_success(false);
-            response.mutable_header()->set_error_message("Camera device not found: " + dev_id);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-            stream->Write(response);
-            return grpc::Status::OK;
-        }
-        auto& media_hub = cmvr::media::globalMediaSourceHub();
-        const std::string track_id = cmvr::media::cameraColorTrackId(dev_id);
-        if (!cmvr::media::ensureCameraMediaSource(media_hub, dev)) {
-            api::GetRGBImageStreamCommand_Feedback response;
-            response.mutable_header()->set_success(false);
-            response.mutable_header()->set_error_message("Failed to register camera media source: " + dev_id);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-            stream->Write(response);
-            return grpc::Status::OK;
-        }
-        auto subscription = media_hub.subscribe(
-            track_id,
-            cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED,
-            [context] { return context->IsCancelled(); });
-        if (!subscription) {
-            api::GetRGBImageStreamCommand_Feedback response;
-            response.mutable_header()->set_success(false);
-            response.mutable_header()->set_error_message("Failed to subscribe camera media source: " + dev_id);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-            stream->Write(response);
-            return grpc::Status::OK;
-        }
-
-        std::atomic<bool> client_eof_requested{false};
-        std::atomic<bool> request_stream_closed{false};
-        std::atomic<uint64_t> control_requests_read{1};
-        std::thread request_reader([&] {
-            api::GetRGBImageStreamCommand_Request control_request;
-            while (stream->Read(&control_request)) {
-                ++control_requests_read;
-                if (control_request.eof()) {
-                    client_eof_requested.store(true, std::memory_order_release);
-                    CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] client requested RGB stream EOF"
-                                   << ", id=" << dev_id
-                                   << ", peer=" << context->peer()
-                                   << ", control_requests=" << control_requests_read.load();
-                    break;
-                }
-            }
-            request_stream_closed.store(true, std::memory_order_release);
-        });
-        struct RequestReaderJoiner {
-            std::thread& thread;
-            ~RequestReaderJoiner() {
-                if (thread.joinable()) {
-                    thread.join();
-                }
-            }
-        } request_reader_joiner{request_reader};
-        const auto join_request_reader = [&] {
-            if (request_reader.joinable()) {
-                request_reader.join();
-            }
-        };
-
-        bool waiting_for_key_frame = true;
-        const char* exit_reason = "unknown";
-        auto last_key_frame_request = std::chrono::steady_clock::now();
-        auto last_latency_log = std::chrono::steady_clock::time_point{};
-        uint64_t discarded_since_log = 0;
-        std::chrono::microseconds last_write_duration{0};
-        media_hub.requestKeyFrame(track_id);
-        const auto request_key_frame_if_due = [&] {
-            const auto now = std::chrono::steady_clock::now();
-            if (now - last_key_frame_request >= std::chrono::milliseconds(500)) {
-                media_hub.requestKeyFrame(track_id);
-                last_key_frame_request = now;
-            }
-        };
-        const auto request_key_frame_now = [&] {
-            waiting_for_key_frame = true;
-            media_hub.requestKeyFrame(track_id);
-            last_key_frame_request = std::chrono::steady_clock::now();
-        };
-        const auto log_latency_event = [&](
-            const char* reason,
-            const uint64_t discarded,
-            const uint64_t frame_age_ns,
-            const std::chrono::microseconds write_duration) {
-            discarded_since_log += discarded;
-            const auto now = std::chrono::steady_clock::now();
-            if (last_latency_log != std::chrono::steady_clock::time_point{} &&
-                now - last_latency_log < std::chrono::seconds(1)) {
-                return;
-            }
-            const double frame_age_ms = static_cast<double>(frame_age_ns) / 1'000'000.0;
-            const double write_ms = static_cast<double>(write_duration.count()) / 1'000.0;
-            CMVR_LOG(WARNING) << "[gRPCCameraServiceImpl] low-latency camera stream event"
-                              << ", id=" << dev_id
-                              << ", reason=" << reason
-                              << ", discarded=" << discarded_since_log
-                              << ", age_ms=" << frame_age_ms
-                              << ", write_ms=" << write_ms
-                              << ", max_pending_frames="
-                              << stream_config_.max_pending_frames
-                              << ", max_frame_age_ms="
-                              << stream_config_.max_frame_age.count();
-            discarded_since_log = 0;
-            last_latency_log = now;
-        };
-        while (true)
-        {
-            if (client_eof_requested.load(std::memory_order_acquire)) {
-                exit_reason = "client_eof";
-                break;
-            }
-            if (context->IsCancelled())
-            {
-                exit_reason = "context_cancelled";
-                CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled"
-                               << ", id=" << dev_id
-                               << ", peer=" << context->peer()
-                               << ", client_eof=" << client_eof_requested.load()
-                               << ", request_stream_closed=" << request_stream_closed.load();
-                break;
-            }
-
-            const auto read = subscription.waitRead(std::chrono::milliseconds(100));
-            if (!read || !read->value || read->value->empty()) {
-                if (!subscription.valid()) {
-                    exit_reason = "subscription_invalid";
-                    break;
-                }
-                if (waiting_for_key_frame) {
-                    request_key_frame_if_due();
-                }
-                continue;
-            }
-            const auto& frame = *read->value;
-            const auto descriptor = frame.descriptor;
-            if (!descriptor) {
-                continue;
-            }
-            const uint64_t now_ns = static_cast<uint64_t>(
-                std::chrono::duration_cast<std::chrono::nanoseconds>(
-                    std::chrono::steady_clock::now().time_since_epoch()).count());
-            const auto frame_age = cameraFrameAgeNs(frame.capture_time_ns, now_ns);
-            if (frame_age &&
-                cameraFrameExceedsAgeLimit(
-                    frame.capture_time_ns,
-                    now_ns,
-                    stream_config_.max_frame_age)) {
-                // This frame is already outside the latency budget. Flush all
-                // currently queued frames and wait for a fresh IDR; sending any
-                // P/B frame after an intentional gap would break decoder continuity.
-                const uint64_t discarded =
-                    1 + subscription.discardPendingIfExceeds(0);
-                request_key_frame_now();
-                log_latency_event(
-                    "stale_frame",
-                    discarded,
-                    *frame_age,
-                    last_write_duration);
-                continue;
-            }
-            const bool inter_frame_codec = descriptor->codec == cmvr::media::Codec::H264 ||
-                                           descriptor->codec == cmvr::media::Codec::H265;
-            if (!inter_frame_codec || descriptor->payload_format != cmvr::media::PayloadFormat::ANNEX_B) {
-                api::GetRGBImageStreamCommand_Feedback response;
-                response.mutable_header()->set_success(false);
-                response.mutable_header()->set_error_message(
-                    "Unsupported camera stream codec or payload format: " + dev_id);
-                setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-                stream->Write(response);
-                exit_reason = "unsupported_stream";
-                break;
-            }
-            if (read->dropped_since_last_read > 0 || read->generation_changed || frame.discontinuity) {
-                request_key_frame_now();
-            }
-            if (waiting_for_key_frame && !frame.key_frame) {
-                request_key_frame_if_due();
-                continue;
-            }
-            waiting_for_key_frame = false;
-
-            api::GetRGBImageStreamCommand_Feedback response;
-            response.mutable_header()->set_success(true);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-            response.mutable_color_frame()->set_data(frame.data(), frame.size());
-            response.mutable_color_frame()->set_is_key_frame(frame.key_frame);
-            response.mutable_color_frame()->set_codec(
-                descriptor->codec == cmvr::media::Codec::H264 ? "h264" :
-                descriptor->codec == cmvr::media::Codec::H265 ? "h265" : "unknown");
-            response.mutable_color_frame()->set_width(static_cast<int32_t>(descriptor->width));
-            response.mutable_color_frame()->set_height(static_cast<int32_t>(descriptor->height));
-            response.mutable_color_frame()->set_capture_utc_ns(frame.capture_utc_ns);
-            response.mutable_color_frame()->set_source_sequence(frame.sequence);
-            response.mutable_color_frame()->set_pts(frame.pts);
-            response.mutable_color_frame()->set_dts(frame.dts);
-            response.mutable_color_frame()->set_source_fps(descriptor->nominal_rate);
-            response.mutable_color_frame()->set_source_timestamp(frame.source_timestamp);
-            response.mutable_color_frame()->set_source_frame_number(frame.source_frame_number);
-
-            response.mutable_intrinsics()->set_fx(descriptor->fx);
-            response.mutable_intrinsics()->set_fy(descriptor->fy);
-            response.mutable_intrinsics()->set_cx(descriptor->cx);
-            response.mutable_intrinsics()->set_cy(descriptor->cy);
-            for (const float coefficient : descriptor->distortion) {
-                response.mutable_intrinsics()->add_coeffs(coefficient);
-            }
-
-            response.set_seq_no(static_cast<int32_t>(std::min<uint64_t>(
-                frame.sequence,
-                static_cast<uint64_t>(std::numeric_limits<int32_t>::max()))));
-
-            const auto write_started = std::chrono::steady_clock::now();
-            if (!stream->Write(response)) {
-                exit_reason = "write_failed";
-                CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed"
-                               << ", id=" << dev_id
-                               << ", peer=" << context->peer()
-                               << ", context_cancelled=" << context->IsCancelled()
-                               << ", client_eof=" << client_eof_requested.load()
-                               << ", request_stream_closed=" << request_stream_closed.load()
-                               << ", control_requests=" << control_requests_read.load()
-                               << ", write_ms="
-                               << std::chrono::duration_cast<std::chrono::microseconds>(
-                                      std::chrono::steady_clock::now() - write_started).count() / 1000.0;
-                break;
-            }
-            last_write_duration = std::chrono::duration_cast<std::chrono::microseconds>(
-                std::chrono::steady_clock::now() - write_started);
-
-            // A successful synchronous Write may have been flow-controlled long
-            // enough for the source to outpace this consumer. Once the pending
-            // count crosses the configured trigger, discard the whole pending
-            // batch and require a fresh key frame before resuming.
-            const uint64_t discarded = subscription.discardPendingIfExceeds(
-                stream_config_.max_pending_frames);
-            if (discarded > 0) {
-                request_key_frame_now();
-                log_latency_event(
-                    "write_backpressure",
-                    discarded,
-                    frame_age.value_or(0),
-                    last_write_duration);
-            }
-        }
-        join_request_reader();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end"
-                       << ", id=" << dev_id
-                       << ", reason=" << exit_reason
-                       << ", peer=" << context->peer()
-                       << ", context_cancelled=" << context->IsCancelled()
-                       << ", client_eof=" << client_eof_requested.load()
-                       << ", request_stream_closed=" << request_stream_closed.load()
-                       << ", control_requests=" << control_requests_read.load();
-        return grpc::Status::OK;
-    }
-    catch (const exception &e) {
-        api::GetRGBImageStreamCommand_Feedback response;
-        response.mutable_header()->set_success(false);
-        response.mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-        stream->Write(response);
-        return grpc::Status::OK;
-    }
-}

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

@@ -1,484 +0,0 @@
-#include "common/base/logging/logger.h"
-//
-// Created by linbo on 2025/7/3.
-//
-
-#include "../include/grpc_dexhand_service.h"
-
-#include <cmath>
-#include <chrono>
-#include <memory>
-#include <thread>
-#include <vector>
-
-#include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h"
-
-using namespace std;
-using namespace cmvr::service;
-using namespace cmvr::device;
-
-#define DEXHAND_MAX_POSITION 2000
-#define DEXHAND_MAX_ANGLE 1000
-#define DEXHAND_MAX_FORCE 3000
-#define DEXHAND_MAX_SPEED 1000
-
-namespace {
-
-constexpr int kDexHandDofCount = 6;
-
-cmvr::api::SensorData::FingerType toProtoFingerType(const AbstractDexHand::FingerType finger) {
-    switch (finger) {
-        case AbstractDexHand::FingerType::PINKY:
-            return cmvr::api::SensorData::PINKY;
-        case AbstractDexHand::FingerType::RING:
-            return cmvr::api::SensorData::RING;
-        case AbstractDexHand::FingerType::MIDDLE:
-            return cmvr::api::SensorData::MIDDLE_FINGER;
-        case AbstractDexHand::FingerType::INDEX:
-            return cmvr::api::SensorData::INDEX;
-        case AbstractDexHand::FingerType::THUMB:
-            return cmvr::api::SensorData::THUMB;
-        case AbstractDexHand::FingerType::PALM:
-            return cmvr::api::SensorData::PALM;
-    }
-    return cmvr::api::SensorData::PINKY;
-}
-
-cmvr::api::SensorData::PartType toProtoPartType(const AbstractDexHand::TactileRegion region) {
-    switch (region) {
-        case AbstractDexHand::TactileRegion::TIP:
-            return cmvr::api::SensorData::TIP;
-        case AbstractDexHand::TactileRegion::FINGER:
-            return cmvr::api::SensorData::FINGER;
-        case AbstractDexHand::TactileRegion::PAD:
-            return cmvr::api::SensorData::PAD;
-        case AbstractDexHand::TactileRegion::THUMB_MIDDLE:
-            return cmvr::api::SensorData::THUMB_MIDDLE;
-        case AbstractDexHand::TactileRegion::PALM_PAD:
-            return cmvr::api::SensorData::PALM_PAD;
-    }
-    return cmvr::api::SensorData::TIP;
-}
-
-void fillSensorData(const AbstractDexHand::TactileRegionData& tactile_data,
-                    cmvr::api::SensorData* sensor_data) {
-    sensor_data->set_rows(tactile_data.view.rows);
-    sensor_data->set_cols(tactile_data.view.cols);
-    sensor_data->set_finger_type(toProtoFingerType(tactile_data.finger));
-    sensor_data->set_part_type(toProtoPartType(tactile_data.region));
-    sensor_data->set_sensor_name(tactile_data.name == nullptr ? "" : tactile_data.name);
-
-    for (int row = 0; row < tactile_data.view.rows; ++row) {
-        auto* row_data = sensor_data->add_data();
-        const auto* values = tactile_data.view.rowData(row);
-        for (int col = 0; col < tactile_data.view.cols; ++col) {
-            // Keep the existing scalar wire format by exposing the normal-force projection.
-            row_data->add_values(static_cast<int32_t>(values[col].fz));
-        }
-    }
-}
-
-template <typename ResponseT>
-void appendSensorData(const std::vector<AbstractDexHand::TactileRegionData>& tactile_regions,
-                      ResponseT* response) {
-    for (const auto& tactile_region : tactile_regions) {
-        if (!tactile_region.valid()) {
-            continue;
-        }
-        fillSensorData(tactile_region, response->add_sensor());
-    }
-}
-
-template <typename FreedomCollection>
-bool applyFreedomValues(const FreedomCollection& freedoms,
-                        const int scale,
-                        std::vector<int>& targets,
-                        std::string* error_message) {
-    for (const auto& freedom : freedoms) {
-        if (freedom.id() < 0 || freedom.id() >= static_cast<int>(targets.size())) {
-            if (error_message) {
-                *error_message = "Invalid dexhand DOF id: " + std::to_string(freedom.id());
-            }
-            return false;
-        }
-        if (!std::isfinite(freedom.value())) {
-            if (error_message) {
-                *error_message = "Invalid dexhand command value: not finite.";
-            }
-            return false;
-        }
-        targets[static_cast<size_t>(freedom.id())] = static_cast<int>(freedom.value() * scale);
-    }
-    return true;
-}
-
-template <typename ResponseT>
-grpc::Status failResponse(ResponseT* response, const std::string& message) {
-    CMVR_LOG(ERROR) << "[gRPCDexHandServiceImpl] " << message;
-    response->mutable_header()->set_success(false);
-    response->mutable_header()->set_error_message(message);
-    setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-    return grpc::Status::OK;
-}
-
-std::vector<int> readCurrentAngles(const std::shared_ptr<AbstractDexHand>& dev) {
-    DexHandState state{};
-    dev->getState(state);
-
-    std::vector<int> current_angles(static_cast<size_t>(kDexHandDofCount), 0);
-    for (int i = 0; i < kDexHandDofCount; ++i) {
-        current_angles[static_cast<size_t>(i)] = state.hands[i].angle;
-    }
-    return current_angles;
-}
-
-bool respondUnsupportedForRh56(const std::shared_ptr<AbstractDexHand>& dev,
-                               const char* rpc_name,
-                               const char* hint,
-                               cmvr::api::CommandHeader_Feedback* header) {
-    if (std::dynamic_pointer_cast<RH56DFTPDexhand>(dev) == nullptr) {
-        return false;
-    }
-
-    header->set_success(false);
-    header->set_error_message(std::string(rpc_name) + " is not supported by RH56DFTPDexhand. " + hint);
-    setCurrentTimestamp(header->mutable_timestamp());
-    return true;
-}
-
-std::vector<AbstractDexHand::TactileRegionKey> buildRh56AllTactileRegions() {
-    using FingerType = AbstractDexHand::FingerType;
-    using TactileRegion = AbstractDexHand::TactileRegion;
-
-    return {
-        {FingerType::PINKY, TactileRegion::TIP},
-        {FingerType::PINKY, TactileRegion::FINGER},
-        {FingerType::PINKY, TactileRegion::PAD},
-        {FingerType::RING, TactileRegion::TIP},
-        {FingerType::RING, TactileRegion::FINGER},
-        {FingerType::RING, TactileRegion::PAD},
-        {FingerType::MIDDLE, TactileRegion::TIP},
-        {FingerType::MIDDLE, TactileRegion::FINGER},
-        {FingerType::MIDDLE, TactileRegion::PAD},
-        {FingerType::INDEX, TactileRegion::TIP},
-        {FingerType::INDEX, TactileRegion::FINGER},
-        {FingerType::INDEX, TactileRegion::PAD},
-        {FingerType::THUMB, TactileRegion::TIP},
-        {FingerType::THUMB, TactileRegion::FINGER},
-        {FingerType::THUMB, TactileRegion::THUMB_MIDDLE},
-        {FingerType::THUMB, TactileRegion::PAD},
-        {FingerType::PALM, TactileRegion::PALM_PAD}
-    };
-}
-
-void maybeConfigureRh56FullTactilePolling(const std::shared_ptr<AbstractDexHand>& dev) {
-    auto rh56 = std::dynamic_pointer_cast<RH56DFTPDexhand>(dev);
-    if (!rh56) {
-        return;
-    }
-    rh56->setTactilePollingRegions(buildRh56AllTactileRegions());
-}
-
-} // namespace
-
-gRPCDexHandServiceImpl::gRPCDexHandServiceImpl(): dmgr_(DeviceManager::getInstance()) {}
-
-grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context,
-    const api::GetDexHandStateCommand_Request* request, api::GetDexHandStateCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (GetStatus): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
-        if (!dev) {
-            return failResponse(response, "DexHand device not found: " + dev_id);
-        }
-        DexHandState state{};
-        dev->getState(state);
-        response->mutable_state()->set_is_initialized(state.is_initialized);
-        for (int i = 0; i < kDexHandDofCount; i++) {
-            auto hand = response->mutable_state()->add_hands();
-            hand->set_dof_id(i);
-            hand->set_angle(state.hands[i].angle);
-            hand->set_current(state.hands[i].current);
-            hand->set_force(state.hands[i].force);
-            hand->set_position(state.hands[i].position);
-            hand->set_speed(state.hands[i].speed);
-            hand->set_temperature(state.hands[i].temperature);
-            hand->set_error(state.hands[i].error);
-            for (auto& errormessage : state.hands[i].error_message) {
-                hand->add_error_message(errormessage);
-            }
-        }
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetStatus): success, id=" << dev_id
-                        << ", initialized=" << state.is_initialized
-                        << ", dof=" << response->state().hands_size();
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context
-    , const cmvr::api::SetDexHandPositionsCommand_Request* request
-    , cmvr::api::SetDexHandPositionsCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPos): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
-        if (!dev) {
-            return failResponse(response, "DexHand device not found: " + dev_id);
-        }
-        if (respondUnsupportedForRh56(dev,
-                                      "SetDexHandPos",
-                                      "Use SetDexHandAngle for RH56 joint commands.",
-                                      response->mutable_header())) {
-            return grpc::Status::OK;
-        }
-
-        std::vector<int> finger_joint_targets(static_cast<size_t>(kDexHandDofCount), -1);
-        std::string error_message;
-        if (!applyFreedomValues(request->values(), DEXHAND_MAX_POSITION, finger_joint_targets, &error_message)) {
-            return failResponse(response, error_message);
-        }
-        dev->setPositions(finger_joint_targets);
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandPos): success, id=" << dev_id
-                        << ", values=" << request->values_size();
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* context
-    , const cmvr::api::SetDexHandAnglesCommand_Request* request
-    , cmvr::api::SetDexHandAnglesCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandAngle): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
-        if (!dev) {
-            return failResponse(response, "DexHand device not found: " + dev_id);
-        }
-
-        if (const auto rh56 = std::dynamic_pointer_cast<RH56DFTPDexhand>(dev)) {
-            std::vector<int> finger_joint_targets = readCurrentAngles(dev);
-            std::string error_message;
-            if (!applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets, &error_message)) {
-                return failResponse(response, error_message);
-            }
-            rh56->setAngles(finger_joint_targets);
-        } else {
-            std::vector<int> finger_joint_targets(static_cast<size_t>(kDexHandDofCount), -1);
-            std::string error_message;
-            if (!applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets, &error_message)) {
-                return failResponse(response, error_message);
-            }
-            dev->setAngles(finger_joint_targets);
-        }
-
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandAngle): success, id=" << dev_id
-                        << ", values=" << request->values_size();
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* context
-    , const cmvr::api::SetDexHandForceCommand_Request* request
-    , cmvr::api::SetDexHandForceCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandForce): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
-        if (!dev) {
-            return failResponse(response, "DexHand device not found: " + dev_id);
-        }
-        if (respondUnsupportedForRh56(dev,
-                                      "SetDexHandForce",
-                                      "RH56DFTPDexhand currently exposes angle and tactile APIs only.",
-                                      response->mutable_header())) {
-            return grpc::Status::OK;
-        }
-
-        std::vector<int> finger_joint_targets(static_cast<size_t>(kDexHandDofCount), -1);
-        std::string error_message;
-        if (!applyFreedomValues(request->values(), DEXHAND_MAX_FORCE, finger_joint_targets, &error_message)) {
-            return failResponse(response, error_message);
-        }
-        dev->setForce(finger_joint_targets);
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandForce): success, id=" << dev_id
-                        << ", values=" << request->values_size();
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* context
-    , const cmvr::api::SetDexHandSpeedCommand_Request* request
-    , cmvr::api::SetDexHandSpeedCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandSpeed): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
-        if (!dev) {
-            return failResponse(response, "DexHand device not found: " + dev_id);
-        }
-        if (respondUnsupportedForRh56(dev,
-                                      "SetDexHandSpeed",
-                                      "RH56DFTPDexhand currently exposes angle and tactile APIs only.",
-                                      response->mutable_header())) {
-            return grpc::Status::OK;
-        }
-
-        std::vector<int> finger_joint_targets(static_cast<size_t>(kDexHandDofCount), -1);
-        std::string error_message;
-        if (!applyFreedomValues(request->values(), DEXHAND_MAX_SPEED, finger_joint_targets, &error_message)) {
-            return failResponse(response, error_message);
-        }
-        dev->setVelocities(finger_joint_targets);
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandSpeed): success, id=" << dev_id
-                        << ", values=" << request->values_size();
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCDexHandServiceImpl::SetDexHandPresetAct(grpc::ServerContext* context
-    , const cmvr::api::SetDexHandPresetActCommand_Request* request
-    , cmvr::api::SetDexHandPresetActCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPresetAct): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
-        if (!dev) {
-            return failResponse(response, "DexHand device not found: " + dev_id);
-        }
-        if (respondUnsupportedForRh56(dev,
-                                      "SetDexHandPresetAct",
-                                      "RH56DFTPDexhand currently exposes angle and tactile APIs only.",
-                                      response->mutable_header())) {
-            return grpc::Status::OK;
-        }
-
-        auto presetActId = request->presetactid();
-        dev->setPresetAct(presetActId);
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandPresetAct): success, id=" << dev_id
-                        << ", preset_act_id=" << presetActId;
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context
-    , const cmvr::api::GetSensorDataCommand_Request* request
-    , cmvr::api::GetSensorDataCommand_Feedback* response) {
-
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorData): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
-        if (!dev) {
-            return failResponse(response, "DexHand device not found: " + dev_id);
-        }
-        maybeConfigureRh56FullTactilePolling(dev);
-        appendSensorData(dev->getSensorData(), response);
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorData): success, id=" << dev_id
-                        << ", sensors=" << response->sensor_size();
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* context
-    , grpc::ServerReaderWriter<cmvr::api::GetSensorDataStreamCommand_Feedback, cmvr::api::GetSensorDataStreamCommand_Request>* stream)
-{
-    try {
-        api::GetSensorDataStreamCommand_Request request;
-        if (!stream->Read(&request)) {
-            return grpc::Status::OK;
-        }
-
-        string dev_id = request.header().device_id();
-        CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): start,id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
-        if (!dev) {
-            api::GetSensorDataStreamCommand_Feedback response;
-            response.mutable_header()->set_success(false);
-            response.mutable_header()->set_error_message("DexHand device not found: " + dev_id);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-            stream->Write(response);
-            return grpc::Status::OK;
-        }
-        maybeConfigureRh56FullTactilePolling(dev);
-        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): streaming success, id=" << dev_id;
-
-        while (!context->IsCancelled())
-        {
-            api::GetSensorDataStreamCommand_Feedback response;
-            appendSensorData(dev->getSensorData(), &response);
-            response.mutable_header()->set_success(true);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-
-            if (!stream->Write(response)) {
-                CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (stream->Write) failed,id=" << dev_id;
-                break;
-            }
-
-            std::this_thread::sleep_for(std::chrono::milliseconds(33));
-        }
-        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): finished, id=" << dev_id;
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        CMVR_LOG(ERROR) << "[gRPCDexHandServiceImpl] (GetSensorDataStream) exception: " << e.what();
-        return grpc::Status::OK;
-    }
-
-}

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

@@ -1,475 +0,0 @@
-#include "common/base/logging/logger.h"
-#include "../include/grpc_head_service.h"
-
-#include "cmvr/api/biohead_service.grpc.pb.h"
-#include "manager/device_manager/include/device_manager.h"
-#include "common/base/grpc_utils.h"
-#include "biohead/biohead_esp32/include/biohead_esp32.h"
-#include <chrono>
-#include <algorithm>
-#include <iostream>
-
-using namespace std;
-using namespace cmvr::service;
-using namespace cmvr::device;
-using namespace cmvr::api;
-
-namespace {
-template <typename ResponseT>
-grpc::Status failResponse(ResponseT* response, const std::string& message) {
-    CMVR_LOG(ERROR) << "[gRPCMBioHeadServiceImpl] " << message;
-    response->mutable_header()->set_success(false);
-    response->mutable_header()->set_error_message(message);
-    setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-    return grpc::Status::OK;
-}
-
-void logSuccess(const char* rpc_name, const std::string& device_id) {
-    CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (" << rpc_name
-                    << "): success, id=" << device_id;
-}
-}
-
-gRPCMBioHeadServiceImpl::gRPCMBioHeadServiceImpl()
-    : dmgr_(DeviceManager::getInstance()) {}
-
-
-// 设置表情（一次性）
-grpc::Status gRPCMBioHeadServiceImpl::SetExpression(
-    grpc::ServerContext* context,
-    const SetFacialExpression_Request* request,
-    SetFacialExpression_Feedback* response) {
-
-    try {
-        std::string dev_id = request->header().device_id();
-        auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
-        if (!robot) {
-            return failResponse(response, "Biohead device not found: " + dev_id);
-        }
-
-        FacialExpressionState& expression_state = robot->expression_state_;
-
-        expression_state.left_eyebrow_outside_y = request->expression().eyebrow().left_outside_y();
-        expression_state.left_eyebrow_inside_y = request->expression().eyebrow().left_inside_y();
-        expression_state.right_eyebrow_outside_y = request->expression().eyebrow().right_outside_y();
-        expression_state.right_eyebrow_inside_y = request->expression().eyebrow().right_inside_y();
-
-        expression_state.left_eye_upper_lid_y = request->expression().eyelid().left_upper_y();
-        expression_state.left_eye_lower_lid_y = request->expression().eyelid().left_lower_y();
-        expression_state.right_eye_upper_lid_y = request->expression().eyelid().right_upper_y();
-        expression_state.right_eye_lower_lid_y = request->expression().eyelid().right_lower_y();
-
-        expression_state.left_eye_ball_x = request->expression().eyeball().left_x();
-        expression_state.left_eye_ball_y = request->expression().eyeball().left_y();
-        expression_state.right_eye_ball_x = request->expression().eyeball().right_x();
-        expression_state.right_eye_ball_y = request->expression().eyeball().right_y();
-
-        expression_state.left_nose_y = request->expression().nose().left_y();
-        expression_state.right_nose_y = request->expression().nose().right_y();
-
-        expression_state.upper_lip_y = request->expression().mouth().upper_lip_y();
-        expression_state.lower_lip_y = request->expression().mouth().lower_lip_y();
-
-        robot->setExpressionPose(expression_state);
-
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        logSuccess("SetExpression", dev_id);
-        return grpc::Status::OK;
-    } catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-
-// 流式控制接口
-grpc::Status gRPCMBioHeadServiceImpl::StreamExpression(
-    grpc::ServerContext* context,
-    grpc::ServerReaderWriter<StreamFacialExpression_Feedback, StreamFacialExpression_Request>* stream)
-{
-    StreamFacialExpression_Feedback feedback_msg;
-    std::string dev_id;
-    std::shared_ptr<AbstractBiohead> robot;
-    bool first_message = true;
-
-    try {
-        StreamFacialExpression_Request request_msg;
-        constexpr float control_frequency = 10;
-        const auto time_interval = std::chrono::milliseconds(static_cast<int>(1000 / control_frequency));
-        auto last_control_time = std::chrono::steady_clock::now();
-
-        CMVR_LOG(INFO) << "StreamExpression started.";
-
-        while (stream->Read(&request_msg)) {
-            if (first_message) {
-                dev_id = request_msg.header().device_id();
-                if (dev_id.empty()) {
-                    CMVR_LOG(ERROR) << "[gRPCMBioHeadServiceImpl] Device ID is empty in first message";
-                    feedback_msg.mutable_header()->set_success(false);
-                    feedback_msg.mutable_header()->set_error_message("Device ID is empty in first message");
-                    setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp());
-                    stream->Write(feedback_msg);
-                    return grpc::Status::OK;
-                }
-                robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
-                if (!robot) {
-                    const std::string message = "Biohead device not found: " + dev_id;
-                    CMVR_LOG(ERROR) << "[gRPCMBioHeadServiceImpl] " << message;
-                    feedback_msg.mutable_header()->set_success(false);
-                    feedback_msg.mutable_header()->set_error_message(message);
-                    setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp());
-                    stream->Write(feedback_msg);
-                    return grpc::Status::OK;
-                }
-
-                // ✅ 重置紧急停止标志
-                robot->emergency_stop_requested = false;
-
-                first_message = false;
-                CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (StreamExpression): streaming success, id=" << dev_id;
-            }
-
-            // ✅ 如果紧急停止触发，直接退出
-            if (robot->emergency_stop_requested) {
-                CMVR_LOG(WARNING) << "[Stream] Emergency stop requested. Terminating stream for device: " << dev_id;
-                break;
-            }
-
-            auto current_time = std::chrono::steady_clock::now();
-            auto elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(current_time - last_control_time);
-            if (elapsed_time < time_interval) continue;
-
-            FacialExpressionState expression_state;
-
-            // 眉毛
-            expression_state.left_eyebrow_outside_y = request_msg.expr().eyebrow().left_outside_y();
-            expression_state.left_eyebrow_inside_y = request_msg.expr().eyebrow().left_inside_y();
-            expression_state.right_eyebrow_outside_y = request_msg.expr().eyebrow().right_outside_y();
-            expression_state.right_eyebrow_inside_y = request_msg.expr().eyebrow().right_inside_y();
-
-            // 眼睑
-            expression_state.left_eye_upper_lid_y = request_msg.expr().eyelid().left_upper_y();
-            expression_state.left_eye_lower_lid_y = request_msg.expr().eyelid().left_lower_y();
-            expression_state.right_eye_upper_lid_y = request_msg.expr().eyelid().right_upper_y();
-            expression_state.right_eye_lower_lid_y = request_msg.expr().eyelid().right_lower_y();
-
-            // 眼球
-            expression_state.left_eye_ball_y = request_msg.expr().eyeball().left_y();
-            expression_state.right_eye_ball_y = request_msg.expr().eyeball().right_y();
-
-            // 鼻子
-            expression_state.left_nose_y = request_msg.expr().nose().left_y();
-            expression_state.right_nose_y = request_msg.expr().nose().right_y();
-
-            // 嘴部
-            expression_state.upper_lip_y = request_msg.expr().mouth().upper_lip_y();
-            expression_state.lower_lip_y = request_msg.expr().mouth().lower_lip_y();
-
-            // 嘴角
-            expression_state.left_corner_lip_x = request_msg.expr().mouth().left_lip().upper_y();
-            expression_state.left_corner_lip_y = request_msg.expr().mouth().left_lip().corner_y();
-            expression_state.lower_left_lip_y = request_msg.expr().mouth().left_lip().lower_y();
-
-            expression_state.right_corner_lip_x = request_msg.expr().mouth().right_lip().upper_y();
-            expression_state.lower_right_lip_y = request_msg.expr().mouth().right_lip().corner_y();
-            expression_state.right_corner_lip_y = request_msg.expr().mouth().right_lip().lower_y();
-
-            // 下巴
-            expression_state.jaw_x = request_msg.expr().jaw().x();
-            expression_state.jaw_y = request_msg.expr().jaw().y();
-
-            robot->streamFacialPose(expression_state, 0, 0);
-            last_control_time = current_time;
-
-            feedback_msg.mutable_header()->set_success(true);
-            feedback_msg.mutable_header()->clear_error_message();
-            setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp());
-            if (!stream->Write(feedback_msg)) break;
-        }
-
-        CMVR_LOG(INFO) << "StreamExpression finished for device: " << dev_id;
-        CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (StreamExpression): finished, id=" << dev_id;
-        return grpc::Status::OK;
-    } catch (const std::exception& e) {
-        CMVR_LOG(ERROR) << "StreamExpression error: " << e.what();
-        feedback_msg.mutable_header()->set_success(false);
-        feedback_msg.mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp());
-        if (stream) stream->Write(feedback_msg);
-        return grpc::Status::OK;
-    }
-}
-
-
-// 获取设备状态
-grpc::Status gRPCMBioHeadServiceImpl::GetSystemStatus(
-    grpc::ServerContext* context,
-    const GetStatus_Request* request,
-    GetStatus_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
-        if (!robot) {
-            return failResponse(response, "Biohead device not found: " + dev_id);
-        }
-
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        logSuccess("GetSystemStatus", dev_id);
-        return grpc::Status::OK;
-    }
-    catch (const exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-
-// 紧急停止
-grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop(
-    grpc::ServerContext* context,
-    const EmergencyStop_Request* request,
-    EmergencyStop_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
-
-        if (!robot) {
-            return failResponse(response, "Biohead device not found: " + dev_id);
-        }
-
-        robot->eStop();  // 停止执行
-        robot->emergency_stop_requested = true;  // ✅ 设置中断标志
-
-
-
-
-
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        logSuccess("EmergencyStop", dev_id);
-        return grpc::Status::OK;
-    }
-    catch (const exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCMBioHeadServiceImpl::SpeakStart(grpc::ServerContext* context, const cmvr::api::SpeakStart_Request* request, cmvr::api::SpeakStart_Feedback* response)
-{
-
-    {
-
-        try {
-            string dev_id = request->header().device_id();
-            auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
-
-
-            if (!robot) {
-                return failResponse(response, "Biohead device not found: " + dev_id);
-            }
-
-            robot->speakstart();  // kaish开始
-
-
-            response->mutable_header()->set_success(true);
-            setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-            logSuccess("SpeakStart", dev_id);
-            return grpc::Status::OK;
-        }
-        catch (const exception& e) {
-            response->mutable_header()->set_success(false);
-            response->mutable_header()->set_error_message(e.what());
-            setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-            return grpc::Status::OK;
-        }
-    }
-}
-
-
-
-grpc::Status gRPCMBioHeadServiceImpl::SpeakStop(grpc::ServerContext* context, const cmvr::api::SpeakStop_Request* request, cmvr::api::SpeakStop_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
-        if (!robot) {
-            return failResponse(response, "Biohead device not found: " + dev_id);
-        }
-
-        robot->speakstop();  // 停止执行
-
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        logSuccess("SpeakStop", dev_id);
-        return grpc::Status::OK;
-    }
-    catch (const exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-
-
-grpc::Status gRPCMBioHeadServiceImpl::Happy(grpc::ServerContext* context, const cmvr::api::Happy_Request* request, cmvr::api::Happy_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
-        if (!robot) {
-            return failResponse(response, "Biohead device not found: " + dev_id);
-        }
-
-        robot->expressionHappy();  // 停止执行
-
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        logSuccess("Happy", dev_id);
-        return grpc::Status::OK;
-    }
-    catch (const exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCMBioHeadServiceImpl::Surprise(grpc::ServerContext* context, const cmvr::api::Surprise_Request* request, cmvr::api::Surprise_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
-        if (!robot) {
-            return failResponse(response, "Biohead device not found: " + dev_id);
-        }
-
-        robot->expressionSurprised();  //
-
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        logSuccess("Surprise", dev_id);
-        return grpc::Status::OK;
-    }
-    catch (const exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-
-grpc::Status gRPCMBioHeadServiceImpl::ExpressionTired(grpc::ServerContext* context, const cmvr::api::ExpressionTired_Request* request, cmvr::api::ExpressionTired_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
-        if (!robot) {
-            return failResponse(response, "Biohead device not found: " + dev_id);
-        }
-
-        robot->expressionTired();  // 停止执行
-
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        logSuccess("ExpressionTired", dev_id);
-        return grpc::Status::OK;
-    }
-    catch (const exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-
-
-grpc::Status gRPCMBioHeadServiceImpl::ExpressionAngry(grpc::ServerContext* context, const cmvr::api::ExpressionAngry_Request* request, cmvr::api::ExpressionAngry_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
-        if (!robot) {
-            return failResponse(response, "Biohead device not found: " + dev_id);
-        }
-
-        robot->expressionAngry();  // 停止执行
-
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        logSuccess("ExpressionAngry", dev_id);
-        return grpc::Status::OK;
-    }
-    catch (const exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-
-
-grpc::Status gRPCMBioHeadServiceImpl::ExpressionSadness(grpc::ServerContext* context, const cmvr::api::ExpressionSadness_Request* request, cmvr::api::ExpressionSadness_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
-        if (!robot) {
-            return failResponse(response, "Biohead device not found: " + dev_id);
-        }
-
-        robot->expressionSadness();  // 停止执行
-
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        logSuccess("ExpressionSadness", dev_id);
-        return grpc::Status::OK;
-    }
-    catch (const exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-
-grpc::Status gRPCMBioHeadServiceImpl::ExpressionYawn(grpc::ServerContext* context, const cmvr::api::ExpressionYawn_Request* request, cmvr::api::ExpressionYawn_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
-        if (!robot) {
-            return failResponse(response, "Biohead device not found: " + dev_id);
-        }
-
-        robot->expressionYawn();  // 停止执行
-
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        logSuccess("ExpressionYawn", dev_id);
-        return grpc::Status::OK;
-    }
-    catch (const exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}

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

@@ -1,46 +0,0 @@
-//
-// Created by lgv on 2025/8/25.
-//
-#include "gtest/gtest.h"
-#include "common/base/logging/logger.h"
-#include <grpcpp/grpcpp.h>
-#include "../include/grpc_hlc_service.h"
-#include "google/protobuf/timestamp.pb.h"
-#include <iostream>
-#include <google/protobuf/util/time_util.h>
-
-using namespace cmvr::api;
-
-TEST(GrpcHlcClientTest, MyTest) {
-    // 连接服务端
-    auto channel = grpc::CreateChannel("0.0.0.0:50052", grpc::InsecureChannelCredentials());
-    auto stub = cmvr::api::HlcService::NewStub(channel);
-
-
-    grpc::ClientContext context;
-    cmvr::api::Touch_Request request;
-    cmvr::api::Touch_Response response;
-
-
-    request.mutable_header()->set_device_id("hc01");
-    *request.mutable_header()->mutable_timestamp() = google::protobuf::util::TimeUtil::GetCurrentTime();
-
-
-    request.set_u(600);
-    request.set_v(360);
-    request.set_max_force(1300);
-
-
-
-    // 调用
-    grpc::Status status = stub->touch(&context, request, &response);
-
-    if (status.ok()) {
-        CMVR_LOG(INFO) << "Touch RPC succeeded." << std::endl;
-        CMVR_LOG(INFO) << "Success: " << response.mutable_header()->success() << std::endl;
-        CMVR_LOG(INFO) << "Error message: " << response.mutable_header()->error_message() << std::endl;
-        CMVR_LOG(INFO) << "Timestamp: " << response.mutable_header()->timestamp().seconds() << std::endl;
-    } else {
-        CMVR_LOG(ERROR) << "Touch RPC failed: " << status.error_message() << std::endl;
-    }
-}

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

@@ -1,87 +0,0 @@
-//
-// Created by lgv on 2025/8/25.
-//
-
-
-#include "../include/grpc_hlc_service.h"
-
-#include <chrono>
-#include <exception>
-#include <thread>
-
-#include <google/protobuf/util/time_util.h>
-
-#include "common/base/logging/logger.h"
-#include "manager/task_manager/include/task_manager.h"
-#include "task/touch_screen_task/include/touch_screen_task.h"
-
-
-using namespace cmvr::service;
-using namespace cmvr::api;
-using google::protobuf::util::TimeUtil;
-
-namespace {
-
-std::string buildTouchFailureMessage(const cmvr::task::TouchScreenTask& task,
-                                     const std::string& prefix) {
-    return prefix + ", phase=" +
-           cmvr::task::TouchScreenTask::phaseToString(task.phase()) +
-           ", status=" +
-           cmvr::task::TouchScreenTask::statusToString(task.lastStatus());
-}
-
-void fillTouchResponse(Touch_Response* response,
-                       const bool success,
-                       const std::string& error_message) {
-    response->mutable_header()->set_success(success);
-    response->mutable_header()->set_error_message(error_message);
-    *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
-}
-
-} // namespace
-
-gRPCHlcServiceImpl::gRPCHlcServiceImpl() = default;
-
-grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) {
-    try {
-        auto touch_task = task::TaskManager::getInstance().getTouchScreenTask();
-        if (!touch_task) {
-            const std::string error = "TouchScreenTask not found or not initialized";
-            fillTouchResponse(response, false, error);
-            return grpc::Status(grpc::StatusCode::NOT_FOUND, error);
-        }
-
-        if (!touch_task->touch(request->u(), request->v())) {
-            const std::string error =
-                buildTouchFailureMessage(*touch_task, "TouchScreenTask touch request rejected");
-            fillTouchResponse(response, false, error);
-            return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, error);
-        }
-
-        while (touch_task->isBusy()) {
-            if (context != nullptr && context->IsCancelled()) {
-                const std::string error = "touch request cancelled";
-                fillTouchResponse(response, false, error);
-                return grpc::Status(grpc::StatusCode::CANCELLED, error);
-            }
-            std::this_thread::sleep_for(std::chrono::milliseconds(10));
-        }
-
-        if (!touch_task->isFinished()) {
-            const std::string error =
-                buildTouchFailureMessage(*touch_task, "TouchScreenTask touch failed");
-            fillTouchResponse(response, false, error);
-            return grpc::Status(grpc::StatusCode::INTERNAL, error);
-        }
-
-        fillTouchResponse(response, true, "");
-        CMVR_LOG(DEBUG) << "[gRPCHlcServiceImpl] (touch): success, u=" << request->u()
-                        << ", v=" << request->v()
-                        << ", phase=" << cmvr::task::TouchScreenTask::phaseToString(touch_task->phase())
-                        << ", status=" << cmvr::task::TouchScreenTask::statusToString(touch_task->lastStatus());
-        return grpc::Status::OK;
-    } catch (const std::exception& e) {
-        fillTouchResponse(response, false, e.what());
-        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
-    }
-}

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

@@ -1,382 +0,0 @@
-#include "common/base/logging/logger.h"
-#include "devices/microphone/microphone_device_discovery.h"
-#include "manager/media_source_hub/include/device_media_source_adapter.h"
-#include <algorithm>
-#include <chrono>
-#include <cstdint>
-#include <limits>
-//
-// Created by linbo on 2025/6/13.
-// Created by xtkuang on 2025/6/13.
-//
-
-#include "../include/grpc_microphone_service.h"
-using namespace std;
-using namespace cmvr::device;
-using namespace cmvr::service;
-
-namespace {
-template <typename ResponseT>
-grpc::Status failResponse(ResponseT* response, const std::string& message) {
-    CMVR_LOG(ERROR) << "[gRPCMicroPhoneServiceImpl] " << message;
-    response->mutable_header()->set_success(false);
-    response->mutable_header()->set_error_message(message);
-    setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-    return grpc::Status::OK;
-}
-
-std::string selectInputDevice(
-    const std::shared_ptr<AbstractMicrophone>& microphone,
-    const std::string& input_device) {
-    if (input_device.empty()) {
-        return "Microphone input_device is required";
-    }
-
-    const auto available_devices = listAvailableMicrophoneInputDevices();
-    const auto selected = std::find_if(
-        available_devices.begin(),
-        available_devices.end(),
-        [&input_device](const MicrophoneInputDeviceInfo& device) {
-            return device.input_device == input_device;
-        });
-    if (selected == available_devices.end()) {
-        return "Microphone input device is unavailable: " + input_device;
-    }
-    if (!microphone->selectInputDevice(input_device)) {
-        return "Cannot select microphone input device while capture is active: " + input_device;
-    }
-    return {};
-}
-
-}
-
-gRPCMicroPhoneServiceImpl::gRPCMicroPhoneServiceImpl(): dmgr_(DeviceManager::getInstance()) {}
-
-grpc::Status gRPCMicroPhoneServiceImpl::ListDevices(
-    grpc::ServerContext* context,
-    const api::ListMicrophoneDevicesCommand_Request* request,
-    api::ListMicrophoneDevicesCommand_Feedback* response) {
-    (void)context;
-    (void)request;
-    try {
-        const auto devices = listAvailableMicrophoneInputDevices();
-        for (const auto& device : devices) {
-            auto* item = response->add_devices();
-            item->set_input_device(device.input_device);
-            item->set_display_name(device.display_name);
-            item->set_backend(device.backend);
-            item->set_is_default(device.is_default);
-        }
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (ListDevices): found "
-                       << devices.size() << " microphone input device(s)";
-        return grpc::Status::OK;
-    } catch (const std::exception& e) {
-        return failResponse(response, e.what());
-    }
-}
-
-grpc::Status gRPCMicroPhoneServiceImpl::GetStatus(grpc::ServerContext* context,
-    const api::GetMicStateCommand_Request* request, api::GetMicStateCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (GetStatus): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractMicrophone>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Microphone device not found: " + dev_id);
-        }
-        MicrophoneState state;
-        dev->getState(state);
-        response->mutable_state()->set_is_initialized(state.is_initialized);
-        response->mutable_state()->set_is_running(state.is_running);
-        response->mutable_state()->set_is_recording(state.is_recording);
-        response->mutable_state()->set_volume(state.volume);
-        response->mutable_state()->set_error_message(state.error_message);
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (GetStatus): success, id=" << dev_id
-                        << ", initialized=" << state.is_initialized
-                        << ", running=" << state.is_running
-                        << ", recording=" << state.is_recording
-                        << ", volume=" << state.volume;
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCMicroPhoneServiceImpl::StartRecord(grpc::ServerContext* context,
-    const api::StartMicRecordingCommand_Request* request, api::StartMicRecordingCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StartRecord): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractMicrophone>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Microphone device not found: " + dev_id);
-        }
-        const std::string selection_error = selectInputDevice(dev, request->input_device());
-        if (!selection_error.empty()) {
-            return failResponse(response, selection_error);
-        }
-        if (!dev->start()) {
-            return failResponse(response, "Failed to start microphone: " + dev_id);
-        }
-        if (!dev->startRecording(request->file_path())) {
-            MicrophoneState state;
-            dev->getState(state);
-            return failResponse(
-                response,
-                state.error_message.empty()
-                    ? "Failed to start microphone recording: " + dev_id
-                    : state.error_message);
-        }
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (StartRecord): success, id=" << dev_id
-                        << ", path=" << request->file_path();
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCMicroPhoneServiceImpl::StopRecord(grpc::ServerContext* context,
-    const api::StopMicRecordingCommand_Request* request, api::StopMicRecordingCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StopRecord): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractMicrophone>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Microphone device not found: " + dev_id);
-        }
-        dev->stopRecording();
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (StopRecord): success, id=" << dev_id;
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCMicroPhoneServiceImpl::PauseRecord(grpc::ServerContext* context,
-    const api::PauseMicRecordingCommand_Request* request, api::PauseMicRecordingCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (PauseRecord): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractMicrophone>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Microphone device not found: " + dev_id);
-        }
-        dev->pause();
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (PauseRecord): success, id=" << dev_id;
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCMicroPhoneServiceImpl::ResumeRecord(grpc::ServerContext* context,
-    const api::ResumeMicRecordingCommand_Request* request, api::ResumeMicRecordingCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (ResumeRecord): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractMicrophone>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Microphone device not found: " + dev_id);
-        }
-        dev->resume();
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (ResumeRecord): success, id=" << dev_id;
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context,
-    const api::StreamMicAudioCommand_Request* request,
-    grpc::ServerWriter<api::StreamMicAudioCommand_Feedback>* writer) {
-    try {
-        const string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StreamAudio): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractMicrophone>(dev_id);
-        if (!dev) {
-            api::StreamMicAudioCommand_Feedback feedback;
-            feedback.mutable_header()->set_success(false);
-            feedback.mutable_header()->set_error_message("Microphone device not found: " + dev_id);
-            setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp());
-            writer->Write(feedback);
-            return grpc::Status::OK;
-        }
-
-        const std::string selection_error = selectInputDevice(dev, request->input_device());
-        if (!selection_error.empty()) {
-            api::StreamMicAudioCommand_Feedback feedback;
-            feedback.mutable_header()->set_success(false);
-            feedback.mutable_header()->set_error_message(selection_error);
-            setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp());
-            writer->Write(feedback);
-            return grpc::Status::OK;
-        }
-
-        auto& media_hub = cmvr::media::globalMediaSourceHub();
-        const std::string track_id = cmvr::media::microphoneTrackId(dev_id);
-        if (!cmvr::media::ensureMicrophoneMediaSource(media_hub, dev)) {
-            api::StreamMicAudioCommand_Feedback feedback;
-            feedback.mutable_header()->set_success(false);
-            feedback.mutable_header()->set_error_message("Failed to register microphone media source: " + dev_id);
-            setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp());
-            writer->Write(feedback);
-            return grpc::Status::OK;
-        }
-
-        auto subscription = media_hub.subscribe(
-            track_id,
-            cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED,
-            [context] { return context->IsCancelled(); });
-        if (!subscription) {
-            api::StreamMicAudioCommand_Feedback feedback;
-            feedback.mutable_header()->set_success(false);
-            feedback.mutable_header()->set_error_message("Failed to subscribe microphone media source: " + dev_id);
-            setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp());
-            writer->Write(feedback);
-            return grpc::Status::OK;
-        }
-
-        while (!context->IsCancelled()) {
-            const auto read = subscription.waitRead(std::chrono::milliseconds(100));
-            if (!read || !read->value || read->value->empty()) {
-                if (!subscription.valid()) {
-                    break;
-                }
-                continue;
-            }
-            const auto& frame = *read->value;
-            const auto descriptor = frame.descriptor;
-            if (!descriptor) {
-                continue;
-            }
-
-            api::StreamMicAudioCommand_Feedback feedback;
-            feedback.mutable_header()->set_success(true);
-            setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp());
-            auto* audio = feedback.mutable_audio();
-            audio->set_data(frame.data(), frame.size());
-            audio->set_sample_rate(static_cast<int32_t>(std::min<uint32_t>(
-                descriptor->sample_rate,
-                static_cast<uint32_t>(std::numeric_limits<int32_t>::max()))));
-            audio->set_channels(static_cast<int32_t>(std::min<uint32_t>(
-                descriptor->channels,
-                static_cast<uint32_t>(std::numeric_limits<int32_t>::max()))));
-            if (descriptor->codec == cmvr::media::Codec::PCM_S16LE) {
-                audio->set_format(cmvr::api::AudioData_AudioFormat_PCM);
-                audio->set_codec("pcm_s16le");
-            } else if (descriptor->codec == cmvr::media::Codec::OPUS) {
-                audio->set_format(cmvr::api::AudioData_AudioFormat_OPUS);
-                audio->set_codec("opus");
-            } else if (descriptor->codec == cmvr::media::Codec::AAC) {
-                audio->set_format(cmvr::api::AudioData_AudioFormat_AAC);
-                audio->set_codec("aac");
-            } else {
-                feedback.mutable_header()->set_success(false);
-                feedback.mutable_header()->set_error_message(
-                    "Unsupported microphone stream codec: " + dev_id);
-                feedback.clear_audio();
-                writer->Write(feedback);
-                break;
-            }
-            audio->set_pts(frame.pts);
-            const int64_t sample_count = frame.duration > 0
-                ? frame.duration
-                : static_cast<int64_t>(descriptor->nominal_rate);
-            audio->set_nb_samples(static_cast<int32_t>(std::clamp<int64_t>(
-                sample_count,
-                0,
-                std::numeric_limits<int32_t>::max())));
-            if (!writer->Write(feedback)) {
-                break;
-            }
-        }
-        return grpc::Status::OK;
-    } catch (const std::exception& error) {
-        api::StreamMicAudioCommand_Feedback feedback;
-        feedback.mutable_header()->set_success(false);
-        feedback.mutable_header()->set_error_message(error.what());
-        setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp());
-        writer->Write(feedback);
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCMicroPhoneServiceImpl::SetVolume(grpc::ServerContext* context,
-    const api::SetMicPhoneVolumeCommand_Request* request, api::SetMicPhoneVolumeCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (SetVolume): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractMicrophone>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Microphone device not found: " + dev_id);
-        }
-        dev->setVolume(request->volume());
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (SetVolume): success, id=" << dev_id
-                        << ", volume=" << request->volume();
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCMicroPhoneServiceImpl::GetVolume(grpc::ServerContext* context,
-    const api::GetMicPhoneVolumeCommand_Request* request, api::GetMicPhoneVolumeCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (GetVolume): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractMicrophone>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Microphone device not found: " + dev_id);
-        }
-        response->set_volume(dev->getVolume());
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (GetVolume): success, id=" << dev_id
-                        << ", volume=" << response->volume();
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}

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

@@ -1,271 +0,0 @@
-#include "common/base/logging/logger.h"
-#include <memory>
-//
-// Created by xtkuang on 2025/6/10.
-//
-
-#include "../include/grpc_speaker_service.h"
-
-using namespace std;
-using namespace cmvr::device;
-using namespace cmvr::service;
-
-namespace {
-template <typename ResponseT>
-grpc::Status failResponse(ResponseT* response, const std::string& message) {
-    CMVR_LOG(ERROR) << "[gRPCSpeakerServiceImpl] " << message;
-    response->mutable_header()->set_success(false);
-    response->mutable_header()->set_error_message(message);
-    setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-    return grpc::Status::OK;
-}
-
-AudioStreamFormat fromProtoAudioFormat(cmvr::api::AudioData_AudioFormat format) {
-    switch (format) {
-    case cmvr::api::AudioData_AudioFormat_PCM:
-        return AudioStreamFormat::PCM;
-    case cmvr::api::AudioData_AudioFormat_MP3:
-        return AudioStreamFormat::MP3;
-    case cmvr::api::AudioData_AudioFormat_AAC:
-        return AudioStreamFormat::AAC;
-    case cmvr::api::AudioData_AudioFormat_WAV:
-        return AudioStreamFormat::WAV;
-    default:
-        return AudioStreamFormat::UNKNOWN;
-    }
-}
-
-AudioStreamFrameData fromProtoAudioData(const cmvr::api::AudioData& audio) {
-    AudioStreamFrameData frame;
-    frame.data.assign(audio.data().begin(), audio.data().end());
-    frame.sample_rate = audio.sample_rate() > 0 ? audio.sample_rate() : 44100;
-    frame.channels = audio.channels() > 0 ? audio.channels() : 2;
-    frame.format = fromProtoAudioFormat(audio.format());
-    frame.codec = audio.codec();
-    frame.pts = audio.pts();
-    frame.nb_samples = audio.nb_samples();
-    return frame;
-}
-}
-
-gRPCSpeakerServiceImpl::gRPCSpeakerServiceImpl(): dmgr_(DeviceManager::getInstance()) {}
-
-grpc::Status gRPCSpeakerServiceImpl::GetStatus(grpc::ServerContext* context,
-    const api::GetSpeakerStateCommand_Request* request, api::GetSpeakerStateCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        //CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (GetStatus): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractSpeaker>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Speaker device not found: " + dev_id);
-        }
-        SpeakerState state;
-        dev->getState(state);
-        response->mutable_state()->set_is_initialized(state.is_initialized);
-        response->mutable_state()->set_is_running(state.is_running);
-        response->mutable_state()->set_is_decoding(state.is_decoding);
-        response->mutable_state()->set_is_paused(state.is_paused);
-        response->mutable_state()->set_volume(state.volume);
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (GetStatus): success, id=" << dev_id
-                        << ", initialized=" << state.is_initialized
-                        << ", running=" << state.is_running
-                        << ", decoding=" << state.is_decoding
-                        << ", paused=" << state.is_paused
-                        << ", volume=" << state.volume;
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCSpeakerServiceImpl::PlayAudio(grpc::ServerContext* context,
-    const api::PlayAudioCommand_Request* request, api::PlayAudioCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (PlayAudio): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractSpeaker>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Speaker device not found: " + dev_id);
-        }
-        //dev->start();
-        dev->play(request->audio_path());
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (PlayAudio): success, id=" << dev_id
-                        << ", path=" << request->audio_path();
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCSpeakerServiceImpl::StreamAudio(grpc::ServerContext* context,
-    grpc::ServerReader<api::StreamSpeakerAudioCommand_Request>* reader,
-    api::StreamSpeakerAudioCommand_Feedback* response) {
-    try {
-        api::StreamSpeakerAudioCommand_Request request;
-        std::shared_ptr<AbstractSpeaker> dev;
-        std::string dev_id;
-
-        while (reader->Read(&request)) {
-            if (!dev) {
-                dev_id = request.header().device_id();
-                CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (StreamAudio): id=" << dev_id;
-                dev = dmgr_.getDevice<AbstractSpeaker>(dev_id);
-                if (!dev) {
-                    return failResponse(response, "Speaker device not found: " + dev_id);
-                }
-                if (!dev->start()) {
-                    return failResponse(response, "Failed to start speaker: " + dev_id);
-                }
-            }
-
-            if (!dev->pushAudioFrame(fromProtoAudioData(request.audio()))) {
-                dev->stopStreaming();
-                return failResponse(response, "Failed to push speaker audio frame: " + dev_id);
-            }
-        }
-
-        if (dev) {
-            dev->stopStreaming();
-        }
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCSpeakerServiceImpl::StopPlayback(grpc::ServerContext* context,
-    const api::StopSpeakerCommand_Request* request, api::StopSpeakerCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (StopPlayback): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractSpeaker>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Speaker device not found: " + dev_id);
-        }
-        if (!dev->stop()) {
-            return failResponse(response, "Failed to stop speaker: " + dev_id);
-        }
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (StopPlayback): success, id=" << dev_id;
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCSpeakerServiceImpl::PausePlayback(grpc::ServerContext* context,
-    const api::PauseSpeakerCommand_Request* request, api::PauseSpeakerCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (PausePlayback): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractSpeaker>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Speaker device not found: " + dev_id);
-        }
-        dev->pause();
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (PausePlayback): success, id=" << dev_id;
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCSpeakerServiceImpl::ResumePlayback(grpc::ServerContext* context,
-    const api::ResumeSpeakerCommand_Request* request, api::ResumeSpeakerCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (ResumePlayback): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractSpeaker>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Speaker device not found: " + dev_id);
-        }
-        dev->resume();
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (ResumePlayback): success, id=" << dev_id;
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCSpeakerServiceImpl::SetVolume(grpc::ServerContext* context,
-    const api::SetSpeakerVolumeCommand_Request* request, api::SetSpeakerVolumeCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (SetVolume): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractSpeaker>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Speaker device not found: " + dev_id);
-        }
-        dev->setVolume(request->volume());
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (SetVolume): success, id=" << dev_id
-                        << ", volume=" << request->volume();
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCSpeakerServiceImpl::GetVolume(grpc::ServerContext* context,
-    const api::GetSpeakerVolumeCommand_Request* request, api::GetSpeakerVolumeCommand_Feedback* response) {
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (GetVolume): id=" << dev_id;
-        const auto dev = dmgr_.getDevice<AbstractSpeaker>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Speaker device not found: " + dev_id);
-        }
-        response->set_volume(dev->getVolume());
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (GetVolume): success, id=" << dev_id
-                        << ", volume=" << response->volume();
-        return grpc::Status::OK;
-    }
-    catch (const std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}

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

@@ -1,141 +0,0 @@
-//
-// Created by xtkuang on 2025/6/6.
-//
-
-#include "../include/grpc_system_service.h"
-
-#include "common/base/logging/logger.h"
-
-using namespace cmvr::device;
-using namespace cmvr::device;
-using namespace cmvr::service;
-
-gRPCSystemServiceImpl::gRPCSystemServiceImpl(): dmgr_(DeviceManager::getInstance()) {}
-
-grpc::Status gRPCSystemServiceImpl::GetSystemInfo(grpc::ServerContext* context,
-    const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response)
-{
-    try {
-        response->set_version(dmgr_.version());
-        response->set_system_name(dmgr_.name());
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (GetSystemInfo): success, name="
-                        << response->system_name() << ", version=" << response->version();
-        return grpc::Status::OK;
-    }
-    catch (std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCSystemServiceImpl::GetSystemStatus(grpc::ServerContext* context,
-    const api::GetSystemStatusCommand_Request* request, api::GetSystemStatusCommand_Feedback* response)
-{
-    try {
-        std::list<std::pair<std::string, std::string>> dev_list;
-        dmgr_.getDeviceList(dev_list);
-        for (auto &pair: dev_list) {
-            auto* dev = response->add_device_list();
-            dev->set_device_id(pair.first);
-            if (pair.second == "AGV") {
-                dev->set_device_type(api::DeviceType::AGV);
-            }
-            else if (pair.second == "Battery") {
-                dev->set_device_type(api::DeviceType::Battery);
-            }
-            else if (pair.second == "Camera") {
-                dev->set_device_type(api::DeviceType::Camera);
-            }
-            else if (pair.second == "DexHand") {
-                dev->set_device_type(api::DeviceType::DexHand);
-            }
-            else if (pair.second == "Gripper") {
-                dev->set_device_type(api::DeviceType::Gripper);
-            }
-            else if (pair.second == "Microphone") {
-                dev->set_device_type(api::DeviceType::Microphone);
-            }
-            else if (pair.second == "Robot") {
-                dev->set_device_type(api::DeviceType::Robot);
-            }
-            else if (pair.second == "Speaker") {
-                dev->set_device_type(api::DeviceType::Speaker);
-            }
-            else if (pair.second == "Unknown") {
-                dev->set_device_type(api::DeviceType::Unknown);
-            }
-        }
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (GetSystemStatus): success, devices="
-                        << response->device_list_size();
-        return grpc::Status::OK;
-    }
-    catch (const std::exception &e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCSystemServiceImpl::UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response)
-{
-    response->mutable_header()->set_success(false);
-    response->mutable_header()->set_error_message("UpdateParams is no longer supported. Use typed device commands or reload configuration.");
-    setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-    return grpc::Status::OK;
-}
-
-grpc::Status gRPCSystemServiceImpl::ExecuteJsonCommand(grpc::ServerContext* context,
-    const cmvr::api::JsonDeviceCommand_Request* request, cmvr::api::JsonDeviceCommand_Feedback* response)
-{
-    try {
-        const std::string& dev_id = request->header().device_id();
-        auto dev = dmgr_.getDeviceBase(dev_id);
-        if (!dev) {
-            response->mutable_header()->set_success(false);
-            response->mutable_header()->set_error_message("Device not found: " + dev_id);
-            setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-            return grpc::Status::OK;
-        }
-
-        std::string response_json;
-        const bool success = dev->executeJsonCommand(request->request_json(), response_json);
-        response->mutable_header()->set_success(success);
-        if (!success) {
-            response->mutable_header()->set_error_message(response_json);
-        }
-        response->set_response_json(response_json);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-    catch (std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context,
-    const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response)
-{
-    try {
-        dmgr_.stop();
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (StopAll): success";
-        return grpc::Status::OK;
-    }
-    catch (std::exception& e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}

cmvr-es/service/grpc/tests/grpc_camera_stream_policy_test.cpp

@@ -1,54 +0,0 @@
-#include "service/grpc/include/grpc_camera_stream_policy.h"
-
-#include <chrono>
-#include <cstdint>
-#include <iostream>
-
-namespace {
-
-bool check(const bool condition, const char* expression, const int line) {
-    if (condition) {
-        return true;
-    }
-    std::cerr << "CHECK failed at line " << line << ": " << expression << '\n';
-    return false;
-}
-
-#define CHECK_TRUE(expression) \
-    do { \
-        if (!check(static_cast<bool>(expression), #expression, __LINE__)) { \
-            return 1; \
-        } \
-    } while (false)
-
-} // namespace
-
-int main() {
-    using namespace std::chrono_literals;
-    using cmvr::service::cameraFrameAgeNs;
-    using cmvr::service::cameraFrameExceedsAgeLimit;
-    using cmvr::service::makeCameraStreamLowLatencyConfig;
-
-    const auto defaults = makeCameraStreamLowLatencyConfig(0, 0);
-    CHECK_TRUE(defaults.max_pending_frames == 2);
-    CHECK_TRUE(defaults.max_frame_age == 250ms);
-
-    const auto configured = makeCameraStreamLowLatencyConfig(7, 900);
-    CHECK_TRUE(configured.max_pending_frames == 7);
-    CHECK_TRUE(configured.max_frame_age == 900ms);
-
-    CHECK_TRUE(!cameraFrameAgeNs(0, 1'000'000'000ULL).has_value());
-    CHECK_TRUE(!cameraFrameAgeNs(2'000'000'000ULL, 1'000'000'000ULL).has_value());
-    CHECK_TRUE(cameraFrameAgeNs(1'000'000'000ULL, 1'250'000'000ULL).value() ==
-               250'000'000ULL);
-
-    CHECK_TRUE(!cameraFrameExceedsAgeLimit(
-        1'000'000'000ULL, 1'250'000'000ULL, 250ms));
-    CHECK_TRUE(cameraFrameExceedsAgeLimit(
-        1'000'000'000ULL, 1'250'000'001ULL, 250ms));
-    CHECK_TRUE(!cameraFrameExceedsAgeLimit(
-        1'000'000'000ULL, 2'000'000'000ULL, 0ms));
-
-    std::cout << "grpc_camera_stream_policy_test: PASS\n";
-    return 0;
-}

cmvr-es/task/grpc_server_task/include/grpc_server_task.h

@@ -11,6 +11,12 @@
 #include "cmvr/config/grpc_server_config/grpc_server_config.pb.h"
 #include "task/task.h"
 
+namespace cmvr::service {
+class ArmTeleopBackend;
+class GrpcSecurityGateway;
+class RecoveryAuditSink;
+}
+
 namespace cmvr::task {
 
 class GrpcServerTask final : public Task {
@@ -20,6 +26,10 @@ public:
 
     const std::string& id() const override { return id_; }
     TaskRunMode runMode() const override { return TaskRunMode::BLOCKING_SERVICE; }
+    TaskShutdownPhase shutdownPhase() const override
+    {
+        return TaskShutdownPhase::COMMAND_INGRESS;
+    }
     bool init() override;
     bool start() override;
     bool step(double dt) override;
@@ -55,6 +65,12 @@ private:
     std::unique_ptr<grpc::Service> dexhand_service_;
     std::unique_ptr<grpc::Service> biohand_service_;
     std::unique_ptr<grpc::Service> arm_service_;
+    std::unique_ptr<grpc::Service> arm_teleop_service_;
+    std::shared_ptr<cmvr::service::ArmTeleopBackend>
+        arm_teleop_backend_;
+    std::shared_ptr<cmvr::service::GrpcSecurityGateway> security_gateway_;
+    std::shared_ptr<cmvr::service::RecoveryAuditSink> recovery_audit_sink_;
+    std::unique_ptr<grpc::Service> motor_service_;
     std::unique_ptr<grpc::Service> agv_service_;
     std::unique_ptr<grpc::Service> hlc_service_;
 };

cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp

@@ -1,5 +1,6 @@
 #include "task/grpc_server_task/include/grpc_server_task.h"
 
+#include <chrono>
 #include <exception>
 
 #include <grpcpp/ext/proto_server_reflection_plugin.h>
@@ -8,21 +9,36 @@
 #include "cmvr/config/task_manager_config/task_manager_config.pb.h"
 #include "common/base/logging/logger.h"
 #include "common/config/config_files.h"
-#include "service/grpc/include/grpc_agv_service.h"
-#include "service/grpc/include/grpc_arm_service.h"
-#include "service/grpc/include/grpc_camera_service.h"
-#include "service/grpc/include/grpc_dexhand_service.h"
-#include "service/grpc/include/grpc_head_service.h"
-#include "service/grpc/include/grpc_hlc_service.h"
-#include "service/grpc/include/grpc_microphone_service.h"
-#include "service/grpc/include/grpc_speaker_service.h"
-#include "service/grpc/include/grpc_system_service.h"
+#include "devices/arm/robot_arm.h"
+#include "manager/device_manager/include/device_manager.h"
+#include "service/grpc/server/include/grpc_agv_service.h"
+#include "service/grpc/server/include/grpc_arm_service.h"
+#include "service/grpc/server/include/grpc_arm_teleop_service.h"
+#include "service/grpc/server/include/grpc_robot_arm_teleop_backend.h"
+#include "service/grpc/server/include/grpc_recovery_audit.h"
+#include "service/grpc/server/include/grpc_security.h"
+#include "service/grpc/server/include/grpc_camera_service.h"
+#include "service/grpc/server/include/grpc_dexhand_service.h"
+#include "service/grpc/server/include/grpc_error_logging_interceptor.h"
+#include "service/grpc/server/include/grpc_head_service.h"
+#include "service/grpc/server/include/grpc_hlc_service.h"
+#include "service/grpc/server/include/grpc_microphone_service.h"
+#include "service/grpc/server/include/grpc_motor_service.h"
+#include "service/grpc/server/include/grpc_speaker_service.h"
+#include "service/grpc/server/include/grpc_system_service.h"
 #include "task/task_factory.h"
 
 namespace cmvr::task {
 
 namespace {
 
+std::uint64_t unixTimeMs() noexcept
+{
+    const auto value = std::chrono::duration_cast<std::chrono::milliseconds>(
+        std::chrono::system_clock::now().time_since_epoch()).count();
+    return value > 0 ? static_cast<std::uint64_t>(value) : 1U;
+}
+
 std::shared_ptr<Task> createGrpcServerTask(const config::TaskConfigEntry& entry)
 {
     if (entry.id().empty()) {
@@ -98,18 +114,45 @@ bool GrpcServerTask::start()
     camera_service_ = std::make_unique<service::gRPCCameraServiceImpl>(
         service::makeCameraStreamLowLatencyConfig(
             cfg_.camera_stream_max_pending_frames(),
-            cfg_.camera_stream_max_frame_age_ms()));
-    system_service_ = std::make_unique<service::gRPCSystemServiceImpl>();
-    speaker_service_ = std::make_unique<service::gRPCSpeakerServiceImpl>();
-    microphone_service_ = std::make_unique<service::gRPCMicroPhoneServiceImpl>();
-    dexhand_service_ = std::make_unique<service::gRPCDexHandServiceImpl>();
-    biohand_service_ = std::make_unique<service::gRPCMBioHeadServiceImpl>();
-    arm_service_ = std::make_unique<service::gRPCArmServiceImpl>();
-    agv_service_ = std::make_unique<service::gRPCAgvServiceImpl>();
-    hlc_service_ = std::make_unique<service::gRPCHlcServiceImpl>();
+            cfg_.camera_stream_max_frame_age_ms()),
+        security_gateway_);
+    system_service_ = std::make_unique<service::gRPCSystemServiceImpl>(
+        std::chrono::seconds(15),
+        security_gateway_,
+        recovery_audit_sink_);
+    speaker_service_ =
+        std::make_unique<service::gRPCSpeakerServiceImpl>(security_gateway_);
+    microphone_service_ =
+        std::make_unique<service::gRPCMicroPhoneServiceImpl>(security_gateway_);
+    dexhand_service_ =
+        std::make_unique<service::gRPCDexHandServiceImpl>(security_gateway_);
+    biohand_service_ =
+        std::make_unique<service::gRPCMBioHeadServiceImpl>(security_gateway_);
+    arm_service_ =
+        std::make_unique<service::gRPCArmServiceImpl>(security_gateway_);
+    arm_teleop_service_ =
+        std::make_unique<service::ArmTeleopServiceImpl>(
+            arm_teleop_backend_ ? arm_teleop_backend_
+                                : service::makeDisabledArmTeleopBackend(),
+            nullptr,
+            security_gateway_,
+            &device::DeviceManager::getInstance().safetyManager());
+    motor_service_ =
+        std::make_unique<service::gRPCMotorServiceImpl>(security_gateway_);
+    agv_service_ =
+        std::make_unique<service::gRPCAgvServiceImpl>(security_gateway_);
+    hlc_service_ =
+        std::make_unique<service::gRPCHlcServiceImpl>(security_gateway_);
 
     grpc::ServerBuilder builder;
     builder.AddListeningPort(local_address, grpc::InsecureServerCredentials());
+    std::vector<std::unique_ptr<
+        grpc::experimental::ServerInterceptorFactoryInterface>>
+        interceptor_factories;
+    interceptor_factories.emplace_back(
+        service::makeGrpcErrorLoggingInterceptorFactory());
+    builder.experimental().SetInterceptorCreators(
+        std::move(interceptor_factories));
     builder.RegisterService(camera_service_.get());
     builder.RegisterService(system_service_.get());
     builder.RegisterService(speaker_service_.get());
@@ -117,6 +160,8 @@ bool GrpcServerTask::start()
     builder.RegisterService(dexhand_service_.get());
     builder.RegisterService(biohand_service_.get());
     builder.RegisterService(arm_service_.get());
+    builder.RegisterService(arm_teleop_service_.get());
+    builder.RegisterService(motor_service_.get());
     builder.RegisterService(agv_service_.get());
     builder.RegisterService(hlc_service_.get());
 
@@ -131,7 +176,15 @@ bool GrpcServerTask::start()
 
     address_ = local_address;
     state_ = TaskState::RUNNING;
-    CMVR_LOG(INFO) << "[GrpcServerTask] gRPC server started, address=" << address_;
+    const auto& security = security_gateway_->config();
+    CMVR_LOG(INFO) << "[GrpcServerTask] gRPC server started, address=" << address_
+                   << ", transport=" << service::toString(security.transport)
+                   << ", authentication="
+                   << service::toString(security.authentication)
+                   << ", recovery="
+                   << service::toString(security.recovery_exposure)
+                   << ", insecure_non_loopback="
+                   << security.insecure_non_loopback;
     wait_thread_ = std::thread(&GrpcServerTask::waitLoop, this);
     return true;
 }
@@ -149,6 +202,101 @@ bool GrpcServerTask::init()
         state_ = TaskState::FAILED;
         return false;
     }
+
+    const std::string effective_host =
+        cfg_.host().empty() ? "0.0.0.0" : cfg_.host();
+    const auto security_result =
+        service::resolveGrpcSecurityConfig(cfg_, effective_host);
+    if (!security_result.valid) {
+        last_error_ = "invalid gRPC security config: " + security_result.error;
+        CMVR_LOG(ERROR) << "[GrpcServerTask] " << last_error_;
+        state_ = TaskState::FAILED;
+        return false;
+    }
+    for (const auto& warning : security_result.warnings) {
+        CMVR_LOG(WARNING) << "[GrpcServerTask] " << warning;
+    }
+    security_gateway_ = service::makeGrpcSecurityGateway(
+        security_result.config,
+        [](const service::GrpcSecurityAuditRecord& record) {
+            if (!record.allowed) {
+                CMVR_LOG(WARNING)
+                    << "[gRPC security] request denied, correlation_id="
+                    << record.correlation_id
+                    << ", method=" << record.full_method_name
+                    << ", principal=" << record.principal_id
+                    << ", peer=" << record.peer
+                    << ", code=" << static_cast<int>(record.status_code);
+            }
+        });
+    recovery_audit_sink_.reset();
+    if (security_result.config.recovery_exposure !=
+        service::GrpcRecoveryExposure::Disabled) {
+        recovery_audit_sink_ = service::makeFileRecoveryAuditSink(
+            security_result.config.recovery_audit_file);
+        service::RecoveryAuditRecord audit_probe;
+        audit_probe.occurred_at_unix_ms = unixTimeMs();
+        audit_probe.stage = "sink_initialized";
+        audit_probe.principal_id = "system";
+        audit_probe.result = "ready";
+        std::string audit_error;
+        if (!recovery_audit_sink_->append(audit_probe, &audit_error)) {
+            last_error_ =
+                "recovery audit initialization failed: " + audit_error;
+            CMVR_LOG(ERROR) << "[GrpcServerTask] " << last_error_;
+            recovery_audit_sink_.reset();
+            security_gateway_.reset();
+            state_ = TaskState::FAILED;
+            return false;
+        }
+    }
+
+    arm_teleop_backend_ = service::makeDisabledArmTeleopBackend();
+    if (cfg_.has_arm_teleop_backend() &&
+        cfg_.arm_teleop_backend().enable()) {
+        const auto& backend_config = cfg_.arm_teleop_backend();
+        if (backend_config.device_id().empty()) {
+            last_error_ =
+                "enabled ArmTeleop backend requires device_id";
+            state_ = TaskState::FAILED;
+            return false;
+        }
+        auto arm =
+            device::DeviceManager::getInstance()
+                .getDevice<device::RobotArm>(
+                    backend_config.device_id());
+        if (!arm) {
+            last_error_ =
+                "ArmTeleop RobotArm device was not found: " +
+                backend_config.device_id();
+            state_ = TaskState::FAILED;
+            return false;
+        }
+        auto backend =
+            service::makeRobotArmTeleopBackend(
+                std::move(arm), backend_config);
+        if (!backend->available()) {
+            last_error_ =
+                "ArmTeleop backend rejected configuration: " +
+                backend->unavailableReason();
+            state_ = TaskState::FAILED;
+            return false;
+        }
+        // RobotArmTeleopBackend builds robot_id directly from device_id. Keep
+        // this assertion at the registration boundary so the process-wide
+        // control lease resource and unary ArmService device ID cannot drift.
+        if (backend->manifest().robot_id() !=
+            backend_config.device_id()) {
+            last_error_ =
+                "ArmTeleop lease resource must equal device_id";
+            state_ = TaskState::FAILED;
+            return false;
+        }
+        arm_teleop_backend_ = std::move(backend);
+        CMVR_LOG(INFO)
+            << "[GrpcServerTask] ArmTeleop RobotArm backend enabled for "
+            << backend_config.device_id();
+    }
     last_error_.clear();
     state_ = TaskState::IDLE;
     return true;
@@ -164,6 +312,11 @@ void GrpcServerTask::stop()
 {
     {
         std::lock_guard lock(mutex_);
+        if (auto* system_service =
+                dynamic_cast<service::gRPCSystemServiceImpl*>(
+                    system_service_.get())) {
+            system_service->prepareForShutdown();
+        }
         if (server_) {
             server_->Shutdown();
         }
@@ -252,14 +405,19 @@ void GrpcServerTask::waitLoop()
 
 void GrpcServerTask::clearServices()
 {
+    // SystemService owns StopAll workers which can still be draining calls
+    // into operational backends after an RPC deadline. Join them before any
+    // peer service releases its activity registrations or backend state.
+    system_service_.reset();
     hlc_service_.reset();
     agv_service_.reset();
+    motor_service_.reset();
+    arm_teleop_service_.reset();
     arm_service_.reset();
     biohand_service_.reset();
     dexhand_service_.reset();
     microphone_service_.reset();
     speaker_service_.reset();
-    system_service_.reset();
     camera_service_.reset();
 }
 

cmvr-es/task/task.h

@@ -19,17 +19,30 @@ enum class TaskRunMode {
     BLOCKING_SERVICE
 };
 
+enum class TaskShutdownPhase {
+    COMMAND_INGRESS = 0,
+    DEPENDENT_ACTIVITY
+};
+
 class Task {
 public:
     virtual ~Task() = default;
 
     virtual const std::string& id() const = 0;
     virtual TaskRunMode runMode() const { return TaskRunMode::PERIODIC_STEP; }
+    virtual TaskShutdownPhase shutdownPhase() const
+    {
+        return TaskShutdownPhase::DEPENDENT_ACTIVITY;
+    }
     virtual bool init() = 0;
     virtual bool start() { return true; }
     virtual bool step(double dt) = 0;
     virtual void stop() = 0;
 
+    // Cancels current command-driven activity while preserving task lifecycle.
+    // Tasks with no separately stoppable activity remain a successful no-op.
+    virtual bool stopActivity() { return true; }
+
     virtual TaskState state() const = 0;
     virtual bool isBusy() const = 0;
     virtual bool isFinished() const = 0;

cmvr-es/task/touch_screen_task/include/touch_screen_task.h

@@ -4,7 +4,9 @@
 #define CMVR_ES_TOUCH_SCREEN_TASK_H
 
 #include <array>
+#include <atomic>
 #include <chrono>
+#include <functional>
 #include <memory>
 #include <mutex>
 #include <string>
@@ -17,12 +19,16 @@
 #include "devices/camera/abstract_camera.h"
 #include "devices/dexhand/abstract_dexhand.h"
 #include "devices/arm/robot_arm.h"
+#include "manager/control_authority_manager/include/control_authority_manager.h"
 #include "task/task.h"
 #include "algorithms/perception/apriltag/include/apriltag_perception.h"
 #include "algorithms/perception/apriltag/include/tag_relative_target_3d.h"
 
 namespace cmvr::task {
 
+class TouchScreenTaskStopActivityTestPeer;
+class TouchScreenTaskAdmissionTestPeer;
+
 class TouchScreenTask : public Task {
 public:
     enum class Phase {
@@ -55,11 +61,22 @@ public:
         RETRACTING,               // 正在回退离开屏幕。
         DONE,                     // 流程成功完成。
         STOPPED,                  // 被外部 stop() 主动停止。
+        SAFETY_ADMISSION_REVOKED, // 统一安全会话在硬件下发前失效。
         ROBOT_STATE_FAILED,       // 读取机器人状态失败。
         ROBOT_COMMAND_FAILED,     // 向机器人下发控制命令失败。
         TASK_BUSY                 // 已有触屏流程正在运行，新的 touch 请求被拒绝。
     };
 
+    struct SafetyHooks {
+        using HardwareOperation = std::function<bool()>;
+        using Dispatch =
+            std::function<bool(const HardwareOperation&)>;
+
+        std::function<bool()> revalidate;
+        Dispatch dispatch_actuation;
+        Dispatch dispatch_stop;
+    };
+
     explicit TouchScreenTask(const cmvr::config::TouchScreenTaskConfig& cfg);
     ~TouchScreenTask() = default;
 
@@ -70,11 +87,23 @@ public:
 
     const std::string& id() const override { return id_; }
 
+    bool touchIfCurrent(
+        int u,
+        int v,
+        const std::function<bool()>& still_admitted,
+        SafetyHooks safety_hooks = {});
     bool touch(int u, int v);
     bool startFromPixel(int u, int v);
     bool step(double dt) override;
     void stop() override;
 
+    // Stops only the current touch operation. The task remains initialized
+    // and can accept another touch after StopAll admission reopens. Returns
+    // true only after no old step can submit another arm command. When this
+    // task still owns control it also confirms the arm stop; when a safety
+    // barrier already displaced the task, that barrier owns the physical stop.
+    bool stopActivity() override;
+
     Phase phase() const;
     Status lastStatus() const;
     static const char* phaseToString(Phase phase);
@@ -93,18 +122,36 @@ public:
     int lastTouchNonzeroCount() const;
     int lastActiveTagId() const;
     Eigen::Vector3d lastAlignErrorCamera() const;
+    std::string controlDeviceId() const;
 
     const std::shared_ptr<perception::AprilTagPerception>& perception() const { return perception_; }
     const perception::TagRelativeTarget3D& tracker() const { return tracker_; }
     const IbvsController& ibvs() const { return ibvs_; }
 
 private:
+    friend class TouchScreenTaskStopActivityTestPeer;
+    friend class TouchScreenTaskAdmissionTestPeer;
+
     using Clock = std::chrono::steady_clock;
 
     static bool validateConfig(const cmvr::config::TouchScreenTaskConfig& config);
     bool isBusyUnlocked() const;
+    bool beginActivityIfCurrent(std::uint64_t activity_generation);
+    bool acquireActivityControlUnlocked();
+    control::ControlLeaseToken activityControlToken() const;
+    bool activityControlCurrent() const;
+    std::function<bool()> activityCancellationRequested() const;
+    control::ControlDispatchGuard tryBeginActivityDispatch() const;
+    bool activitySafetyCurrent() const;
+    bool runArmActuationIfCurrent(
+        const SafetyHooks::HardwareOperation& operation) const;
+    bool runArmStopIfCurrent(
+        const SafetyHooks::HardwareOperation& operation) const;
+    void clearActivitySafetyHooksUnlocked() noexcept;
+    void releaseActivityControlUnlocked() noexcept;
+    void finishActivityUnlocked(Phase phase, Status status) noexcept;
     bool startFromPixelUnlocked(int u, int v);
-    void stopUnlocked();
+    void resetActivityUnlocked();
     bool applyConfig();
     bool validateControlJointNames() const;
     bool stepAligning(double dt);
@@ -132,8 +179,11 @@ private:
 
 private:
     mutable std::mutex mutex_;
+    mutable std::mutex activity_arm_mutex_;
+    mutable std::mutex activity_control_mutex_;
     std::string id_;
     std::shared_ptr<device::RobotArm> arm_{nullptr};
+    std::shared_ptr<device::RobotArm> activity_arm_{nullptr};
     std::shared_ptr<device::AbstractDexHand> dexhand_{nullptr};
     std::shared_ptr<device::AbstractCamera> camera_{nullptr};
 
@@ -148,6 +198,11 @@ private:
     Status last_status_{Status::NOT_INITIALIZED};
 
     bool initialized_{false};
+    std::atomic<bool> stop_requested_{false};
+    std::atomic<bool> activity_active_{false};
+    std::atomic<std::uint64_t> activity_generation_{1U};
+    control::ControlLeaseToken activity_control_token_;
+    SafetyHooks activity_safety_hooks_;
     bool target_locked_{false};
     bool ibvs_target_initialized_{false};
     bool touch_command_started_{false};

cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp

@@ -12,6 +12,8 @@
 #include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h"
 #include "cmvr/config/touch_screen_algorithm_config.pb.h"
 #include "manager/device_manager/include/device_manager.h"
+#include "service/grpc/server/include/camera_operational_activity_registry.h"
+#include "service/grpc/stop_all/include/stop_all_admission_gate.h"
 #include <visp3/core/vpRotationMatrix.h>
 
 namespace cmvr::task {
@@ -277,6 +279,17 @@ TouchScreenTask::TouchScreenTask(const cmvr::config::TouchScreenTaskConfig& cfg)
 }
 
 bool TouchScreenTask::init() {
+    auto& admission_gate = service::globalStopAllAdmissionGate();
+    std::uint64_t admission_generation = 0U;
+    {
+        auto admission = admission_gate.lockAdmission();
+        if (!admission.accepting()) {
+            last_status_ = Status::NOT_INITIALIZED;
+            return false;
+        }
+        admission_generation = admission.generation();
+    }
+
     if (!config_valid_) {
         last_status_ = Status::INVALID_CONFIG;
         return false;
@@ -294,12 +307,73 @@ bool TouchScreenTask::init() {
     auto arm = dm.getDevice<device::RobotArm>(devices.arm_id());
     auto dexhand = dm.getDevice<device::AbstractDexHand>(devices.dexhand_id());
     auto camera = dm.getDevice<device::AbstractCamera>(devices.camera_id());
-    if (!camera || !camera->start()) {
+    if (!camera) {
         CMVR_LOG(ERROR) << "[TouchScreenTask] Failed to start camera: " << devices.camera_id();
         last_status_ = Status::NOT_INITIALIZED;
         return false;
     }
-    return init(arm, dexhand, camera);
+
+    auto& camera_registry =
+        service::globalCameraOperationalActivityRegistry();
+    service::CameraOperationalActivityRegistry::ActivityToken camera_token;
+    service::CameraOperationalActivityRegistry::DispatchResult camera_start;
+    try {
+        camera_start = camera_registry.start(
+            devices.camera_id(), camera, &camera_token);
+    } catch (...) {
+        last_status_ = Status::NOT_INITIALIZED;
+        return false;
+    }
+    if (camera_start != service::CameraOperationalActivityRegistry::
+                            DispatchResult::Success) {
+        CMVR_LOG(ERROR) << "[TouchScreenTask] Failed to start camera: "
+                        << devices.camera_id();
+        last_status_ = Status::NOT_INITIALIZED;
+        return false;
+    }
+
+    const auto admission_current = [&] {
+        auto admission = admission_gate.lockAdmission();
+        return admission.accepting() &&
+            admission.generation() == admission_generation;
+    };
+    const auto rollback_camera = [&] {
+        if (camera_registry.stopIfCurrent(camera_token)) {
+            return;
+        }
+        const auto ticket = admission_gate.beginStopAll();
+        (void)admission_gate.finishStopAll(ticket, false);
+    };
+    const auto mark_interrupted = [this] {
+        std::lock_guard lock(mutex_);
+        initialized_ = false;
+        last_status_ = Status::NOT_INITIALIZED;
+    };
+
+    if (!admission_current()) {
+        rollback_camera();
+        mark_interrupted();
+        return false;
+    }
+
+    bool initialized = false;
+    try {
+        initialized = init(arm, dexhand, camera);
+    } catch (...) {
+        rollback_camera();
+        throw;
+    }
+    if (!initialized) {
+        rollback_camera();
+        return false;
+    }
+    if (admission_current()) {
+        return true;
+    }
+
+    rollback_camera();
+    mark_interrupted();
+    return false;
 }
 
 bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
@@ -307,6 +381,10 @@ bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
                           const std::shared_ptr<device::AbstractCamera>& camera) {
     std::lock_guard<std::mutex> lock(mutex_);
     arm_ = arm;
+    {
+        std::lock_guard arm_lock(activity_arm_mutex_);
+        activity_arm_ = arm;
+    }
     dexhand_ = dexhand;
     camera_ = camera;
 
@@ -383,16 +461,283 @@ bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
 }
 
 bool TouchScreenTask::touch(const int u, const int v) {
-    std::lock_guard<std::mutex> lock(mutex_);
-    if (isBusyUnlocked()) {
+    return touchIfCurrent(u, v, [] { return true; });
+}
+
+bool TouchScreenTask::touchIfCurrent(
+    const int u,
+    const int v,
+    const std::function<bool()>& still_admitted,
+    SafetyHooks safety_hooks)
+{
+    auto& admission_gate = service::globalStopAllAdmissionGate();
+    std::uint64_t admission_generation = 0U;
+    {
+        auto admission = admission_gate.lockAdmission();
+        if (!admission.accepting()) {
             return false;
         }
-    return startFromPixelUnlocked(u, v);
+        admission_generation = admission.generation();
+    }
+    if (stop_requested_.load(std::memory_order_acquire)) {
+        return false;
+    }
+    const auto activity_generation =
+        activity_generation_.load(std::memory_order_acquire);
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (!still_admitted || !still_admitted()) {
+        return false;
+    }
+    if (!initialized_ || !camera_ || camera_->id().empty()) {
+        last_status_ = Status::NOT_INITIALIZED;
+        return false;
+    }
+
+    auto& camera_registry =
+        service::globalCameraOperationalActivityRegistry();
+    service::CameraOperationalActivityRegistry::ActivityToken camera_token;
+    service::CameraOperationalActivityRegistry::DispatchResult camera_start;
+    try {
+        camera_start = camera_registry.start(
+            camera_->id(), camera_, &camera_token);
+    } catch (...) {
+        last_status_ = Status::NOT_INITIALIZED;
+        return false;
+    }
+    if (camera_start != service::CameraOperationalActivityRegistry::
+                            DispatchResult::Success) {
+        last_status_ = Status::NOT_INITIALIZED;
+        return false;
+    }
+
+    const auto rollback_camera = [&] {
+        if (camera_registry.stopIfCurrent(camera_token)) {
+            return;
+        }
+        const auto ticket = admission_gate.beginStopAll();
+        (void)admission_gate.finishStopAll(ticket, false);
+    };
+
+    bool admission_current = false;
+    bool admitted = false;
+    {
+        // The arm lease and activity marker are the publication point. Holding
+        // admission here makes that point linearizable with beginStopAll().
+        auto admission = admission_gate.lockAdmission();
+        admission_current = admission.accepting() &&
+            admission.generation() == admission_generation;
+        if (admission_current) {
+            admitted = beginActivityIfCurrent(activity_generation);
+        }
+    }
+    if (!admission_current || !admitted) {
+        rollback_camera();
+        return false;
+    }
+
+    resetActivityUnlocked();
+    activity_safety_hooks_ = std::move(safety_hooks);
+    if (!activitySafetyCurrent()) {
+        last_status_ = Status::SAFETY_ADMISSION_REVOKED;
+        activity_active_.store(false, std::memory_order_release);
+        clearActivitySafetyHooksUnlocked();
+        releaseActivityControlUnlocked();
+        rollback_camera();
+        return false;
+    }
+
+    const bool started = startFromPixelUnlocked(u, v);
+    if (!started) {
+        activity_active_.store(false, std::memory_order_release);
+        clearActivitySafetyHooksUnlocked();
+        releaseActivityControlUnlocked();
+        rollback_camera();
+        return false;
+    }
+
+    // startFromPixelUnlocked() may perform an interruptible initialization
+    // move. Do not retain the global gate across device work; reject and roll
+    // back if StopAll changed the generation while that work was in flight.
+    admission_current = false;
+    {
+        auto admission = admission_gate.lockAdmission();
+        admission_current = admission.accepting() &&
+            admission.generation() == admission_generation;
+    }
+    if (admission_current) {
+        return true;
+    }
+    activity_active_.store(false, std::memory_order_release);
+    resetActivityUnlocked();
+    clearActivitySafetyHooksUnlocked();
+    releaseActivityControlUnlocked();
+    rollback_camera();
+    return false;
+}
+
+bool TouchScreenTask::beginActivityIfCurrent(
+    const std::uint64_t activity_generation)
+{
+    if (stop_requested_.load(std::memory_order_acquire) ||
+        activity_generation_.load(std::memory_order_acquire) !=
+            activity_generation) {
+        return false;
+    }
+    if (isBusyUnlocked()) {
+        last_status_ = Status::TASK_BUSY;
+        return false;
+    }
+    if (!acquireActivityControlUnlocked()) {
+        last_status_ = Status::TASK_BUSY;
+        return false;
+    }
+    if (stop_requested_.load(std::memory_order_acquire) ||
+        activity_generation_.load(std::memory_order_acquire) !=
+            activity_generation) {
+        releaseActivityControlUnlocked();
+        return false;
+    }
+    activity_active_.store(true, std::memory_order_release);
+    return true;
+}
+
+bool TouchScreenTask::acquireActivityControlUnlocked()
+{
+    if (!arm_ || arm_->id().empty() || activityControlToken().valid()) {
+        return false;
+    }
+    static std::atomic<std::uint64_t> sequence{0U};
+    const auto acquired = control::ControlAuthorityManager::instance()
+        .tryAcquire(
+            arm_->id(),
+            "touch-screen:" + id_ + ":" +
+                std::to_string(
+                    sequence.fetch_add(1U, std::memory_order_relaxed) + 1U),
+            std::chrono::duration_cast<
+                control::ControlAuthorityManager::Duration>(
+                    std::chrono::hours(24)));
+    if (!acquired.acquired) {
+        return false;
+    }
+    {
+        std::lock_guard lock(activity_control_mutex_);
+        activity_control_token_ = acquired.token;
+    }
+    return true;
+}
+
+control::ControlLeaseToken TouchScreenTask::activityControlToken() const
+{
+    std::lock_guard lock(activity_control_mutex_);
+    return activity_control_token_;
+}
+
+bool TouchScreenTask::activityControlCurrent() const
+{
+    const auto token = activityControlToken();
+    return token.valid() &&
+        control::ControlAuthorityManager::instance().validate(
+            token);
+}
+
+std::function<bool()> TouchScreenTask::activityCancellationRequested() const
+{
+    const auto token = activityControlToken();
+    return [this, token] {
+        return stop_requested_.load(std::memory_order_acquire) ||
+            !control::ControlAuthorityManager::instance().validate(token);
+    };
+}
+
+control::ControlDispatchGuard
+TouchScreenTask::tryBeginActivityDispatch() const
+{
+    const auto token = activityControlToken();
+    return control::ControlAuthorityManager::instance().tryBeginDispatch(
+        token);
+}
+
+bool TouchScreenTask::activitySafetyCurrent() const
+{
+    if (!activity_safety_hooks_.revalidate) {
+        return true;
+    }
+    try {
+        return activity_safety_hooks_.revalidate();
+    } catch (...) {
+        return false;
+    }
+}
+
+bool TouchScreenTask::runArmActuationIfCurrent(
+    const SafetyHooks::HardwareOperation& operation) const
+{
+    if (!operation || !activitySafetyCurrent()) {
+        return false;
+    }
+    auto authority_dispatch = tryBeginActivityDispatch();
+    if (!authority_dispatch.acquired()) {
+        return false;
+    }
+    try {
+        return activity_safety_hooks_.dispatch_actuation
+            ? activity_safety_hooks_.dispatch_actuation(operation)
+            : operation();
+    } catch (...) {
+        return false;
+    }
+}
+
+bool TouchScreenTask::runArmStopIfCurrent(
+    const SafetyHooks::HardwareOperation& operation) const
+{
+    if (!operation) {
+        return false;
+    }
+    auto authority_dispatch = tryBeginActivityDispatch();
+    if (!authority_dispatch.acquired()) {
+        return false;
+    }
+    try {
+        return activity_safety_hooks_.dispatch_stop
+            ? activity_safety_hooks_.dispatch_stop(operation)
+            : operation();
+    } catch (...) {
+        return false;
+    }
+}
+
+void TouchScreenTask::clearActivitySafetyHooksUnlocked() noexcept
+{
+    activity_safety_hooks_ = {};
+}
+
+void TouchScreenTask::releaseActivityControlUnlocked() noexcept
+{
+    control::ControlLeaseToken token;
+    {
+        std::lock_guard lock(activity_control_mutex_);
+        token = std::move(activity_control_token_);
+        activity_control_token_ = {};
+    }
+    control::ControlAuthorityManager::instance().release(token);
+}
+
+void TouchScreenTask::finishActivityUnlocked(
+    const Phase phase,
+    const Status status) noexcept
+{
+    phase_ = phase;
+    last_status_ = status;
+    touch_command_started_ = false;
+    retract_command_started_ = false;
+    activity_active_.store(false, std::memory_order_release);
+    clearActivitySafetyHooksUnlocked();
+    releaseActivityControlUnlocked();
 }
 
 bool TouchScreenTask::startFromPixel(const int u, const int v) {
-    std::lock_guard<std::mutex> lock(mutex_);
-    return startFromPixelUnlocked(u, v);
+    return touchIfCurrent(u, v, [] { return true; });
 }
 
 bool TouchScreenTask::startFromPixelUnlocked(int u, int v) {
@@ -405,7 +750,6 @@ bool TouchScreenTask::startFromPixelUnlocked(int u, int v) {
         return false;
     }
 
-    stopUnlocked();
     if (!moveToInitPositionBeforeStartIfEnabled()) {
         return false;
     }
@@ -442,12 +786,32 @@ bool TouchScreenTask::startFromPixelUnlocked(int u, int v) {
 
 bool TouchScreenTask::step(const double dt) {
     std::lock_guard<std::mutex> lock(mutex_);
+    if (stop_requested_.load(std::memory_order_acquire)) {
+        return true;
+    }
+    if (activity_active_.load(std::memory_order_acquire) &&
+        !activityControlCurrent()) {
+        activity_active_.store(false, std::memory_order_release);
+        resetActivityUnlocked();
+        clearActivitySafetyHooksUnlocked();
+        releaseActivityControlUnlocked();
+        return true;
+    }
+    if (activity_active_.load(std::memory_order_acquire) &&
+        !activitySafetyCurrent()) {
+        (void)runArmStopIfCurrent([this] {
+            return arm_ && arm_->stopMotion().ok();
+        });
+        finishActivityUnlocked(
+            Phase::FAILED, Status::SAFETY_ADMISSION_REVOKED);
+        return false;
+    }
     if (!initialized_) {
         last_status_ = Status::NOT_INITIALIZED;
         return false;
     }
     if (!std::isfinite(dt) || dt <= 0.0) {
-        last_status_ = Status::INVALID_CONFIG;
+        enterFailed(Status::INVALID_CONFIG);
         return false;
     }
 
@@ -494,26 +858,102 @@ bool TouchScreenTask::step(const double dt) {
         case Phase::FAILED:
             return false;
     }
-    last_status_ = Status::INVALID_CONFIG;
+    enterFailed(Status::INVALID_CONFIG);
     return false;
 }
 
 void TouchScreenTask::stop() {
-    std::lock_guard<std::mutex> lock(mutex_);
-    stopUnlocked();
+    (void)stopActivity();
 }
 
-void TouchScreenTask::stopUnlocked() {
-    if (arm_) {
+bool TouchScreenTask::stopActivity() {
+    activity_generation_.fetch_add(1U, std::memory_order_acq_rel);
+    stop_requested_.store(true, std::memory_order_release);
+
+    std::shared_ptr<device::RobotArm> arm;
+    {
+        std::lock_guard lock(activity_arm_mutex_);
+        arm = activity_arm_;
+    }
+
+    static std::atomic<std::uint64_t> stop_sequence{0U};
+    auto& authority = control::ControlAuthorityManager::instance();
+    const auto expected_token = activityControlToken();
+    control::ControlAcquireResult stop_barrier;
+    bool barrier_error = false;
+    if (arm && expected_token.valid()) {
         try {
-            arm_->stopL();
+            stop_barrier = authority.preemptAcquireIfCurrent(
+                expected_token,
+                "touch-screen-stop:" + id_ + ":" +
+                    std::to_string(
+                        stop_sequence.fetch_add(
+                            1U, std::memory_order_relaxed) + 1U),
+                std::chrono::duration_cast<
+                    control::ControlAuthorityManager::Duration>(
+                        std::chrono::hours(24)));
+        } catch (...) {
+            barrier_error = true;
+            (void)authority.quarantineIfCurrent(expected_token);
+        }
+    }
+
+    // Only the caller which atomically converted this task's exact lease may
+    // touch the driver. If StopAll already owns the safety barrier, its arm
+    // stop runs independently while this task only drains its old step.
+    if (stop_barrier.acquired) {
+        try {
+            (void)arm->stopMotion();
         } catch (...) {
         }
     }
 
-    sendZeroJointVelocity();
+    bool was_active = false;
+    {
+        // A step holds this mutex through all of its arm submissions. Taking
+        // it here proves that the old step has exited before state is reset.
+        std::lock_guard<std::mutex> lock(mutex_);
+        was_active = activity_active_.exchange(
+            false, std::memory_order_acq_rel);
+        if (was_active || activityControlToken().valid() ||
+            isBusyUnlocked()) {
+            resetActivityUnlocked();
+        }
+        clearActivitySafetyHooksUnlocked();
+        releaseActivityControlUnlocked();
+    }
+
+    bool stopped = !barrier_error;
+    if (stop_barrier.acquired) {
+        const bool handler_released = authority.waitForPreemptedRelease(
+            stop_barrier.token,
+            control::ControlAuthorityManager::Duration::zero());
+        if (handler_released) {
+            // A backend may have allowed the cancellation request to return
+            // without fully quiescing. Confirm once more after the old task
+            // step and every guarded dispatch have drained.
+            try {
+                stopped = arm->stopMotion().ok();
+            } catch (...) {
+                stopped = false;
+            }
+        } else {
+            stopped = false;
+        }
+
+        if (stopped) {
+            authority.release(stop_barrier.token);
+        } else {
+            (void)authority.retireSafetyHolder(stop_barrier.token);
+        }
+    }
+
+    stop_requested_.store(false, std::memory_order_release);
+    return stopped;
+}
+
+void TouchScreenTask::resetActivityUnlocked() {
     ibvs_.resetTwistCommandState();
-    holdCurrentControlledPosition();
 
     phase_ = Phase::IDLE;
     phase_after_retract_ = Phase::DONE;
@@ -617,6 +1057,14 @@ Eigen::Vector3d TouchScreenTask::lastAlignErrorCamera() const {
     return last_align_error_camera_;
 }
 
+std::string TouchScreenTask::controlDeviceId() const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (arm_ && !arm_->id().empty()) {
+        return arm_->id();
+    }
+    return config_.devices().arm_id();
+}
+
 std::string TouchScreenTask::stateString() const {
     return taskStateToString(state());
 }
@@ -660,6 +1108,7 @@ const char* TouchScreenTask::statusToString(const Status status) {
         case Status::RETRACTING: return "RETRACTING";
         case Status::DONE: return "DONE";
         case Status::STOPPED: return "STOPPED";
+        case Status::SAFETY_ADMISSION_REVOKED: return "SAFETY_ADMISSION_REVOKED";
         case Status::ROBOT_STATE_FAILED: return "ROBOT_STATE_FAILED";
         case Status::ROBOT_COMMAND_FAILED: return "ROBOT_COMMAND_FAILED";
         case Status::TASK_BUSY: return "TASK_BUSY";
@@ -1328,21 +1777,22 @@ bool TouchScreenTask::stepRetracting() {
                        << ", final_tcp_delta_base=unavailable";
     }
 
-    try {
-        arm_->stopL();
-    } catch (...) {
+    if (!runArmStopIfCurrent([this] {
+            return arm_ && arm_->stopL().ok();
+        })) {
         enterFailed(Status::ROBOT_COMMAND_FAILED);
         return false;
     }
     holdCurrentControlledPosition();
     if ((phase_after_retract_ == Phase::DONE || phase_after_retract_ == Phase::FAILED) &&
         !moveToInitPositionIfEnabled()) {
-        phase_ = Phase::FAILED;
-        last_status_ = Status::ROBOT_COMMAND_FAILED;
+        finishActivityUnlocked(
+            Phase::FAILED, Status::ROBOT_COMMAND_FAILED);
         return false;
     }
-    phase_ = phase_after_retract_;
-    last_status_ = final_status_after_retract_;
+    const auto completed_phase = phase_after_retract_;
+    const auto completed_status = final_status_after_retract_;
+    finishActivityUnlocked(completed_phase, completed_status);
     return phase_ != Phase::FAILED;
 }
 
@@ -1379,11 +1829,9 @@ bool TouchScreenTask::sendJointVelocity(const std::vector<double>& qdot) const {
 
     device::JointVelocityCommand cmd;
     cmd.velocity = qdot;
-    const auto result = arm_->speedJ(cmd, 0.0, 0.0);
-    if (!result.ok()) {
-        return false;
-    }
-    return true;
+    return runArmActuationIfCurrent([this, &cmd] {
+        return arm_ && arm_->speedJ(cmd, 0.0, 0.0).ok();
+    });
 }
 
 bool TouchScreenTask::sendZeroJointVelocity() const {
@@ -1470,11 +1918,9 @@ bool TouchScreenTask::holdCurrentControlledPosition() const {
         joints.position.push_back(it->second);
     }
 
-    const auto result = arm_->servoJ(joints);
-    if (!result.ok()) {
-        return false;
-    }
-    return true;
+    return runArmActuationIfCurrent([this, &joints] {
+        return arm_ && arm_->servoJ(joints).ok();
+    });
 }
 
 bool TouchScreenTask::buildInitJointPositions(std::vector<double>& positions_out) const {
@@ -1500,8 +1946,10 @@ bool TouchScreenTask::moveToInitPositionBeforeStartIfEnabled() {
     device::MotionOptions options;
     options.velocity = config_.initialization().velocity();
     options.acceleration = config_.initialization().acceleration();
-    const auto result = arm_->moveJ(init_cmd, options);
-    if (!result.ok()) {
+    options.cancellation_requested = activityCancellationRequested();
+    if (!runArmActuationIfCurrent([this, &init_cmd, &options] {
+            return arm_ && arm_->moveJ(init_cmd, options).ok();
+        })) {
         last_status_ = Status::ROBOT_COMMAND_FAILED;
         return false;
     }
@@ -1521,17 +1969,17 @@ bool TouchScreenTask::moveToInitPositionIfEnabled() const {
     device::MotionOptions options;
     options.velocity = config_.initialization().velocity();
     options.acceleration = config_.initialization().acceleration();
-    const auto result = arm_->moveJ(init_cmd, options);
-    if (!result.ok()) {
-        return false;
-    }
-    return true;
+    options.cancellation_requested = activityCancellationRequested();
+    return runArmActuationIfCurrent([this, &init_cmd, &options] {
+        return arm_ && arm_->moveJ(init_cmd, options).ok();
+    });
 }
 
 bool TouchScreenTask::handleTouchTriggered(const bool stop_forward_motion) {
     if (stop_forward_motion) {
-        const auto result = arm_->stopL();
-        if (!result.ok()) {
+        if (!runArmStopIfCurrent([this] {
+                return arm_ && arm_->stopL().ok();
+            })) {
             return false;
         }
     }
@@ -1565,12 +2013,15 @@ bool TouchScreenTask::startTouchPhase() {
             last_status_ = Status::ROBOT_STATE_FAILED;
             return false;
         }
-        const auto result = arm_->speedL(toCartesianVelocity(
-                                             cmvr::common::math::toEigenVec6(speed_l.twist_tool())),
+        const auto velocity = toCartesianVelocity(
+            cmvr::common::math::toEigenVec6(speed_l.twist_tool()));
+        if (!runArmActuationIfCurrent([this, velocity, &speed_l] {
+                return arm_ && arm_->speedL(
+                    velocity,
                     speed_l.acceleration(),
                     0.0,
-                                         device::FrameType::Tool);
-        if (!result.ok()) {
+                    device::FrameType::Tool).ok();
+            })) {
             last_status_ = Status::ROBOT_COMMAND_FAILED;
             return false;
         }
@@ -1596,8 +2047,11 @@ bool TouchScreenTask::startTouchPhase() {
     options.jerk = move_l.jerk();
     options.joint_velocity_limits.assign(move_l.joint_velocity_limits().begin(),
                                          move_l.joint_velocity_limits().end());
-    const auto result = arm_->moveL(pose_cmd, options, device::FrameType::Tool);
-    if (!result.ok()) {
+    options.cancellation_requested = activityCancellationRequested();
+    if (!runArmActuationIfCurrent([this, &pose_cmd, &options] {
+            return arm_ && arm_->moveL(
+                pose_cmd, options, device::FrameType::Tool).ok();
+        })) {
         last_status_ = Status::ROBOT_COMMAND_FAILED;
         return false;
     }
@@ -1607,12 +2061,9 @@ bool TouchScreenTask::startTouchPhase() {
         return false;
     }
 
-    phase_ = Phase::DONE;
-    touch_command_started_ = false;
-    retract_command_started_ = false;
+    finishActivityUnlocked(Phase::DONE, Status::DONE);
     retract_start_position_valid_ = false;
     retract_start_position_base_.setZero();
-    last_status_ = Status::DONE;
     return true;
 }
 
@@ -1644,13 +2095,14 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract,
                        << ", start_tcp_base=unavailable";
     }
 
-    const auto result = arm_->speedL(retract_cmd,
+    if (!runArmActuationIfCurrent([this, &retract_cmd, &retract] {
+            return arm_ && arm_->speedL(
+                retract_cmd,
                 retract.acceleration(),
                 0.0,
-                                     device::FrameType::Tool);
-    if (!result.ok()) {
-        CMVR_LOG(ERROR) << "[TouchScreenTask][RETRACT_START] speedL failed: "
-                        << result.message;
+                device::FrameType::Tool).ok();
+        })) {
+        CMVR_LOG(ERROR) << "[TouchScreenTask][RETRACT_START] speedL failed";
         return false;
     }
 
@@ -1665,18 +2117,15 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract,
 }
 
 void TouchScreenTask::enterFailed(const Status status) {
-    try {
-        if (arm_) {
-            arm_->stopL();
-        }
-    } catch (...) {
-    }
+    (void)runArmStopIfCurrent([this] {
+        return arm_ && arm_->stopL().ok();
+    });
     hardStopIbvsMotion();
     holdCurrentControlledPosition();
-    phase_ = Phase::FAILED;
-    touch_command_started_ = false;
-    retract_command_started_ = false;
-    last_status_ = moveToInitPositionIfEnabled() ? status : Status::ROBOT_COMMAND_FAILED;
+    const auto final_status = moveToInitPositionIfEnabled()
+        ? status
+        : Status::ROBOT_COMMAND_FAILED;
+    finishActivityUnlocked(Phase::FAILED, final_status);
 }
 
 bool TouchScreenTask::updateTouchPressure() {

cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp

@@ -11,6 +11,7 @@
 #include <limits>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <string>
 #include <thread>
 #include <unordered_set>
@@ -33,6 +34,33 @@
 #include "manager/task_manager/include/task_manager.h"
 #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
 
+namespace cmvr::task {
+
+class TouchScreenTaskStopActivityTestPeer final {
+public:
+    static void setActive(
+        TouchScreenTask& task,
+        const std::shared_ptr<device::RobotArm>& arm,
+        const control::ControlLeaseToken& token)
+    {
+        {
+            std::lock_guard lock(task.activity_arm_mutex_);
+            task.activity_arm_ = arm;
+        }
+        {
+            std::lock_guard lock(task.activity_control_mutex_);
+            task.activity_control_token_ = token;
+        }
+        {
+            std::lock_guard lock(task.mutex_);
+            task.phase_ = TouchScreenTask::Phase::ALIGNING;
+            task.activity_active_.store(true, std::memory_order_release);
+        }
+    }
+};
+
+} // namespace cmvr::task
+
 namespace {
 
 constexpr int kRealTargetU = 1280 / 2.0;
@@ -44,6 +72,168 @@ constexpr std::array<const char*, kDof> kJointNames = {
     "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"
 };
 
+class StopCountingRobotArm final : public cmvr::device::RobotArm {
+public:
+    explicit StopCountingRobotArm(std::string id)
+    {
+        id_ = std::move(id);
+    }
+
+    std::string typeName() const override { return "StopCountingRobotArm"; }
+    cmvr::device::RobotModel getRobotModel() const override { return {}; }
+    std::size_t getDof() const override { return 0U; }
+    cmvr::device::ArmState getRobotState() const override { return {}; }
+    cmvr::device::JointGroupState getJointState() const override { return {}; }
+    cmvr::device::CartesianPose getTcpPose(
+        cmvr::device::FrameType = cmvr::device::FrameType::Base) const override
+    {
+        return {};
+    }
+    cmvr::device::RobotMode getRobotMode() const override
+    {
+        return cmvr::device::RobotMode::Idle;
+    }
+    cmvr::device::SafetyMode getSafetyMode() const override
+    {
+        return cmvr::device::SafetyMode::Normal;
+    }
+    cmvr::device::ControlMode getControlMode() const override
+    {
+        return cmvr::device::ControlMode::None;
+    }
+    cmvr::device::Result torqueOn() override { return success(); }
+    cmvr::device::Result torqueOff() override { return success(); }
+    cmvr::device::Result calibrateZeroQ(const std::string&) override
+    {
+        return success();
+    }
+    cmvr::device::Result emergencyStop() override { return success(); }
+    cmvr::device::Result protectiveStop() override { return success(); }
+    cmvr::device::Result setSpeedScaling(double) override { return success(); }
+    double getSpeedScaling() const override { return 1.0; }
+    bool isProtectiveStopped() const override { return false; }
+    bool isEmergencyStopped() const override { return false; }
+    bool isFault() const override { return false; }
+    cmvr::device::Result moveJ(
+        const cmvr::device::JointPositionCommand&,
+        const cmvr::device::MotionOptions&) override
+    {
+        return success();
+    }
+    cmvr::device::Result speedJ(
+        const cmvr::device::JointVelocityCommand&, double, double) override
+    {
+        return success();
+    }
+    cmvr::device::Result stopJ(double) override { return success(); }
+    cmvr::device::Result moveL(
+        const cmvr::device::CartesianPose&,
+        const cmvr::device::MotionOptions&,
+        cmvr::device::FrameType = cmvr::device::FrameType::Base) override
+    {
+        return success();
+    }
+    cmvr::device::Result speedL(
+        const cmvr::device::CartesianVelocity&,
+        double,
+        double,
+        cmvr::device::FrameType = cmvr::device::FrameType::Base) override
+    {
+        return success();
+    }
+    cmvr::device::Result stopL(std::optional<double> = std::nullopt) override
+    {
+        return success();
+    }
+    cmvr::device::Result stopMotion() override
+    {
+        stop_motion_calls_.fetch_add(1, std::memory_order_relaxed);
+        return success();
+    }
+    cmvr::device::Result startServoMode(
+        const cmvr::device::ServoOptions&) override
+    {
+        return success();
+    }
+    cmvr::device::Result servoJ(
+        const cmvr::device::JointPositionCommand&) override
+    {
+        return success();
+    }
+    cmvr::device::Result servoL(
+        const cmvr::device::CartesianPose&,
+        cmvr::device::FrameType = cmvr::device::FrameType::Base) override
+    {
+        return success();
+    }
+    cmvr::device::Result servoSpeedJ(
+        const cmvr::device::JointVelocityCommand&) override
+    {
+        return success();
+    }
+    cmvr::device::Result servoSpeedL(
+        const cmvr::device::CartesianVelocity&,
+        cmvr::device::FrameType = cmvr::device::FrameType::Base) override
+    {
+        return success();
+    }
+    cmvr::device::Result stopServoMode() override { return success(); }
+    cmvr::device::Result connect(const std::string&, int) override
+    {
+        return success();
+    }
+    cmvr::device::Result disconnect() override { return success(); }
+    bool isConnected() const override { return true; }
+    cmvr::device::Result powerOn() override { return success(); }
+    cmvr::device::Result powerOff() override { return success(); }
+    cmvr::device::Result brakeRelease() override { return success(); }
+    cmvr::device::Result shutdown() override { return success(); }
+    cmvr::device::Result clearFault() override { return success(); }
+    cmvr::device::Result unlockProtectiveStop() override { return success(); }
+    cmvr::device::Result loadProgram(const std::string&) override
+    {
+        return success();
+    }
+    cmvr::device::Result playProgram() override { return success(); }
+    cmvr::device::Result pauseProgram() override { return success(); }
+    cmvr::device::Result stopProgram() override { return success(); }
+    std::vector<double> ik(
+        const std::string&,
+        const std::string&,
+        const cmvr::device::CartesianPose&) override
+    {
+        return {};
+    }
+    std::shared_ptr<cmvr::IKSolver> kinematicsSolver() const override
+    {
+        return nullptr;
+    }
+    cmvr::device::CartesianPose fk(
+        const std::string&, const std::string&) override
+    {
+        return {};
+    }
+    cmvr::device::CartesianPose fk(bool = true) override { return {}; }
+    cmvr::device::CartesianVelocity getSpeedLCommandTwistBase() const override
+    {
+        return {};
+    }
+    bool busy() const override { return false; }
+
+    int stopMotionCalls() const
+    {
+        return stop_motion_calls_.load(std::memory_order_relaxed);
+    }
+
+private:
+    static cmvr::device::Result success()
+    {
+        return cmvr::device::Result::success();
+    }
+
+    std::atomic<int> stop_motion_calls_{0};
+};
+
 std::filesystem::path findProjectRoot()
 {
     const std::filesystem::path marker = "model/xiaoyan_description/dual_arm.xml";
@@ -422,6 +612,60 @@ void run_touch_once(int u, int v) {
 
 } // namespace
 
+TEST(TouchScreenTaskTest, StandaloneStopOwnsAndConfirmsPhysicalArmStop)
+{
+    auto& authority = cmvr::control::ControlAuthorityManager::instance();
+    authority.clear();
+    const auto arm = std::make_shared<StopCountingRobotArm>(
+        "touch-standalone-stop-arm");
+    const auto lease = authority.tryAcquire(
+        arm->id(), "touch-task-owner", std::chrono::hours(1));
+    ASSERT_TRUE(lease.acquired) << lease.detail;
+
+    cmvr::task::TouchScreenTask task(
+        cmvr::config::TouchScreenTaskConfig{});
+    cmvr::task::TouchScreenTaskStopActivityTestPeer::setActive(
+        task, arm, lease.token);
+
+    EXPECT_TRUE(task.stopActivity());
+    EXPECT_EQ(arm->stopMotionCalls(), 2);
+    EXPECT_EQ(task.phase(), cmvr::task::TouchScreenTask::Phase::IDLE);
+    EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::STOPPED);
+    EXPECT_FALSE(authority.isLeased(arm->id()));
+    authority.clear();
+}
+
+TEST(TouchScreenTaskTest, StopSkipsArmWhenExternalSafetyBarrierAlreadyPreempted)
+{
+    auto& authority = cmvr::control::ControlAuthorityManager::instance();
+    authority.clear();
+    const auto arm = std::make_shared<StopCountingRobotArm>(
+        "touch-external-stop-arm");
+    const auto lease = authority.tryAcquire(
+        arm->id(), "touch-task-owner", std::chrono::hours(1));
+    ASSERT_TRUE(lease.acquired) << lease.detail;
+
+    cmvr::task::TouchScreenTask task(
+        cmvr::config::TouchScreenTaskConfig{});
+    cmvr::task::TouchScreenTaskStopActivityTestPeer::setActive(
+        task, arm, lease.token);
+    const auto external_barrier = authority.preemptAcquire(
+        arm->id(), "system-stop-all", std::chrono::hours(1));
+    ASSERT_TRUE(external_barrier.acquired) << external_barrier.detail;
+
+    EXPECT_TRUE(task.stopActivity());
+    EXPECT_EQ(arm->stopMotionCalls(), 0);
+    EXPECT_EQ(task.phase(), cmvr::task::TouchScreenTask::Phase::IDLE);
+    EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::STOPPED);
+    EXPECT_TRUE(authority.validate(external_barrier.token));
+    EXPECT_TRUE(authority.waitForPreemptedRelease(
+        external_barrier.token,
+        cmvr::control::ControlAuthorityManager::Duration::zero()));
+    authority.release(external_barrier.token);
+    EXPECT_FALSE(authority.isLeased(arm->id()));
+    authority.clear();
+}
+
 TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) {
     const auto project_root = findProjectRoot();
     ASSERT_FALSE(project_root.empty());

docs/teleoperation/ume_cmvr_architecture.md

@@ -0,0 +1,111 @@
+# UME to CMVR-ES Teleoperation Architecture
+
+## Scope
+
+This document freezes the first implementation stage of the wired,
+cross-machine teleoperation path:
+
+- both edge computers run `cmvr_es`;
+- the UME computer owns the Damiao SocketCAN-FD interfaces;
+- `UmeRobotArm` is a `RobotArm` backend;
+- the UME computer performs the leader/follower model calculations;
+- the robot computer validates and executes joint-servo references;
+- the transport is a versioned gRPC bidirectional stream;
+- the original UME algorithm is migrated before any SEW fusion work.
+
+SEW fusion, passivity research, paper experiments, and physical human testing
+are deliberately outside this implementation stage.
+
+## Target process and ownership boundary
+
+```text
+UME cmvr_es
+  UmeRobotArm
+    Damiao SocketCAN-FD
+    UmeHapticLoop
+    local safety guard
+       |
+       +-- UmeLegacyAlgorithm
+       |
+  UmeTeleopTask
+    GrpcArmTeleopClient
+                  |
+                  | wired Ethernet / gRPC bidi stream
+                  v
+Robot cmvr_es
+  ArmTeleopService
+    control lease
+    latest-only command slot
+    watchdog
+    safe servo executor
+       |
+       v
+    RobotArm
+```
+
+The UME high-frequency loop never performs a network RPC. Network workers and
+hardware loops exchange only bounded latest-state snapshots.
+
+## Implemented boundary in this revision
+
+This revision establishes the device, algorithm-library, session-transport and
+robot-backend boundaries, but it intentionally does not connect them into a
+physical end-to-end controller:
+
+- `UmeRobotArm` owns one eight-axis Damiao bus and a bounded local torque loop.
+  It starts passive, requires an explicit fresh torque command before
+  `torqueOn`, and latches watchdog/transport faults.
+- a successful SocketCAN send means that the complete frame batch was accepted
+  by the local kernel before its deadline. The current MIT transport has no
+  reviewed Disable acknowledgement, so software must not describe that result
+  as actuator-confirmed torque-off.
+- the original UME dynamics, friction, stiction and haptic projection code is a
+  standalone tested library under `algorithms/controllers/ume_legacy`;
+- `UmeTeleopTask` owns reconnect, heartbeat, sequence and a capacity-one
+  outbound mailbox. Its `submitSetpoint()` input is deliberately an algorithm
+  boundary; no production source calls it yet;
+- the server-side `RobotArmTeleopBackend` is implemented and tested behind an
+  explicit capability gate. The current `MotorRobotArm` remains unavailable
+  because its `servoJ` path is sequential per joint rather than an accepted
+  atomic/timed group primitive;
+- server state is currently returned with session events. The configured
+  requested state rate is not yet an independent periodic publisher;
+- follower effort is validated and transported when a backend declares a
+  verified source, but it is not yet consumed by a UME haptic coordinator.
+
+Accordingly, this code is an M0-M9 fail-closed framework and original-algorithm
+migration, not a claim of runnable force-feedback teleoperation. The next
+integration step must add a concrete UME-to-follower retargeting producer,
+connect verified follower effort to the local haptic coordinator, and retain
+the high-frequency/network-thread separation above.
+
+## Safety invariants
+
+1. Opening a CAN interface never enables a motor.
+2. Clearing a fault never arms a motor.
+3. A reconnect creates a new session and never restores active motion.
+4. Cross-machine `steady_clock` values are diagnostic only. A receiver derives
+   command expiry from its local receive time plus `valid_for_us`.
+5. Commands are strictly increasing by sequence within one session.
+6. Queues on the cyclic path are latest-only and bounded.
+7. Invalid or stale follower effort ramps haptic feedback to zero.
+8. Model, joint order, units, and calibration hashes must match before motion.
+9. New physical hardware configurations remain disabled by default.
+10. A software stop does not replace an independent physical emergency stop.
+11. UME hardware enable also requires an explicit firmware-reviewed feedback
+    status whitelist and raw temperature thresholds; empty values never mean
+    "accept all".
+12. UME shutdown timing is diagnosed against a configured budget, but physical
+    torque removal still requires an independent emergency-stop path until a
+    reviewed actuator Disable acknowledgement exists.
+
+## Initial rate boundary
+
+- UME local hardware/haptic loop: configurable, initially 800 Hz to match the
+  legacy UME setup.
+- network command rate: configurable independently of the local loop.
+- robot servo rate: selected from the robot backend capability and never
+  inferred from the network rate.
+
+No hard real-time or stability claim is made until the target computers and
+physical devices have completed staged validation.

docs/teleoperation/ume_cmvr_validation.md

@@ -0,0 +1,118 @@
+# UME / CMVR-ES validation gates
+
+This checklist is part of the first UME migration. Passing a software gate
+does not authorize a physical power-on. The checked-in UME device entries and
+their `hardware_enabled` fields remain `false`.
+
+The current revision has no production setpoint producer for
+`UmeTeleopTask::submitSetpoint()` and no haptic consumer for returned follower
+effort. M9 therefore validates the framework, protocol and migrated original
+algorithm separately; it is not an end-to-end motion or force-feedback test.
+
+## M9: software and network validation
+
+Run these gates on both target CPU architectures before deploying:
+
+1. Build the complete `cmvr_es` target with tests enabled.
+2. Run the UME legacy controller and Pinocchio model-adapter golden tests.
+3. Run the Damiao codec, CAN-FD chain, and `UmeRobotArm` lifecycle tests.
+4. Run the process-wide control-authority tests.
+5. Run the gRPC client, `UmeTeleopTask`, and `ArmTeleopService` tests.
+6. Repeat the concurrent client/task/service tests to screen for shutdown and
+   reconnect races.
+7. Start each checked-in edge profile without UME hardware and verify that it
+   never opens `can4`/`can5` or issues actuator enable frames.
+
+The communication tests must demonstrate all of the following:
+
+- an `OpenSession` manifest mismatch is rejected before backend activation;
+- a second controller cannot acquire the same robot control resource;
+- sequence numbers are non-zero and strictly increasing per session;
+- the sender and receiver use capacity-one, latest-only command storage;
+- a setpoint whose local validity has expired is never dispatched;
+- heartbeat loss, lease loss, stream cancellation, and backend failure call
+  the robot safe-stop boundary;
+- reconnect clears pending motion intent and starts a new sequence space;
+- `StopSession` is attempted before client cancellation;
+- the legacy unary ArmService cannot issue motion, enable, calibration, or
+  fault-reset commands while the teleoperation lease is active;
+- `torqueOff` and `stopMotion` remain available as safety overrides.
+
+Before a real follower backend can be enabled, control authority must also be
+extended to every local arm task and to the underlying MotorService resources.
+The current process-wide lease covers ArmTeleop and unary ArmService only.
+
+For a wired two-computer run, record at least:
+
+- one-way command age at the robot ingress;
+- command mailbox overwrite and rejection counters;
+- heartbeat and control-lease remaining time;
+- servo apply duration and deadline misses;
+- disconnect detection-to-safe-stop time;
+- packet loss, reordering, and delay from an explicit network impairment
+  profile rather than an assumed LAN condition.
+
+No end-to-end stability or transparency claim is supported until those logs
+are tied to a specified controller rate, robot servo period, payload, motion
+envelope, and network impairment profile.
+
+## M10: staged physical commissioning
+
+Every row is a separate sign-off. Do not combine first power-on with a human
+wearing the UME.
+
+- [ ] Independent physical emergency stop is installed and verified.
+- [ ] CAN arbitration/data bitrates and CAN-FD+BRS MTU are verified for each
+      interface.
+- [ ] Motor product, firmware, command ID, feedback ID, and reported motor ID
+      are read back and matched to the configuration.
+- [ ] The four-bit Damiao feedback status meanings and raw temperature limits
+      are verified for the exact product/firmware and entered as an explicit
+      per-joint whitelist/threshold contract.
+- [ ] Joint direction and zero reference are verified one joint at a time with
+      the mechanism unloaded.
+- [ ] Mechanical position, velocity, and torque limits replace the checked-in
+      placeholders and receive an independent review.
+- [ ] Motor feedback timestamps and the 800 Hz cycle are measured on the UME
+      target computer under load.
+- [ ] The exact Damiao firmware's Disable acknowledgement semantics are
+      documented and verified. Until then, SocketCAN send success is only
+      evidence that the local kernel accepted the complete frame batch.
+- [ ] Because MIT feedback has no sequence field, stale request/reply
+      correlation is resolved by a reviewed firmware marker or by measured,
+      enforced bus timing; draining only the frames already queued is not
+      sufficient evidence.
+- [ ] Every UME control-thread I/O operation is shown to be deadline-bounded
+      and interruptible on the target kernel. The configured shutdown timeout
+      is currently a diagnostic failure threshold, not a C++ timed-join
+      primitive.
+- [ ] With torque disabled, both edge profiles run for at least 30 minutes
+      without sequence, deadline, lease, or reconnect anomalies.
+- [ ] With the UME fixed to a stand, each joint is enabled independently at a
+      low torque limit and its watchdog disable path is measured.
+- [ ] Both UME arms are tested together on stands; CAN and CPU deadline margins
+      are recorded.
+- [ ] The robot backend's group `servoJ` semantics and worst-case call duration
+      are measured. Sequential per-joint dispatch is not accepted as an
+      atomic group backend without a documented skew bound.
+- [ ] gRPC writer backpressure cannot stall the independent robot watchdog,
+      and an expired lease cannot be regranted until safe stop is confirmed.
+- [ ] TouchScreenTask and direct MotorService commands are either disabled by
+      the deployment profile or participate in the same resource authority.
+- [ ] Robot-only low-speed setpoint execution is validated before connecting
+      the leader-side algorithm.
+- [ ] Wired-network cable removal, peer process kill, delayed packets, stale
+      commands, duplicate sequences, lease theft, and robot fault injection
+      all lead to a bounded safe stop.
+- [ ] The original UME gravity/friction/feedback controller is commissioned on
+      a stand with force feedback initially clamped to zero, then increased in
+      reviewed steps.
+- [ ] Only after all previous evidence is archived may a supervised human test
+      be considered under a separate risk assessment.
+
+## Evidence record
+
+For each completed physical gate, archive the exact Git revision, installed
+`output/` checksum, configuration files, model and calibration SHA-256 values,
+test operator, hardware serial numbers, raw logs, and pass/fail decision. A
+successful build or simulator run must not be recorded as physical validation.

protos/cmvr/api/agv_command.proto

@@ -3,7 +3,7 @@ syntax = "proto3";
 package cmvr.api;
 
 import "cmvr/api/common.proto";
-import "cmvr/msgs/agv.proto";
+import "cmvr/api/agv_utils.proto";
 
 // 查询 AGV 运行状态命令。
 message AgvRuntimeStateCommand {
@@ -62,7 +62,7 @@ message AgvNavigateToPoseCommand {
     CommandHeader.Request header = 1;
     // 目标位姿。x/y 单位：米，theta 单位：弧度。
     cmvr.msgs.AgvPose2d pose = 2;
-    // 通用运动约束和执行选项。
+    // 通用运动约束和执行选项；默认同步阻塞至任务终态并确认停车。
     cmvr.msgs.AgvMotionOptions options = 3;
     // AGV 适配器扩展参数，用于传递厂商特有选项。
     cmvr.msgs.AgvAdapterParams adapter_params = 4;
@@ -82,7 +82,7 @@ message AgvNavigateToStationCommand {
     CommandHeader.Request header = 1;
     // 目标站点 id。
     string station_id = 2;
-    // 通用运动约束和执行选项。
+    // 通用运动约束和执行选项；默认同步阻塞至任务终态并确认停车。
     cmvr.msgs.AgvMotionOptions options = 3;
     // AGV 适配器扩展参数，用于传递厂商特有选项。
     cmvr.msgs.AgvAdapterParams adapter_params = 4;
@@ -102,6 +102,8 @@ message AgvFollowPathCommand {
     CommandHeader.Request header = 1;
     // 路径段列表。每段包含起点站点 id 和终点站点 id。
     repeated cmvr.msgs.AgvPathSegment path = 2;
+    // 通用执行选项；默认同步阻塞至整条路径终态并确认停车。
+    cmvr.msgs.AgvMotionOptions options = 3;
   }
   // 反馈体。
   message Feedback {
@@ -110,6 +112,23 @@ message AgvFollowPathCommand {
   }
 }
 
+
+// 按指定速度执行固定距离平移命令。
+message AgvTranslateCommand {
+  message Request {
+    // 通用请求头。header.device_id 指定目标 AGV 设备。
+    CommandHeader.Request header = 1;
+    // 固定距离平移参数。
+    cmvr.msgs.AgvTranslation translation = 2;
+  }
+
+  message Feedback {
+    // 仅表示控制器是否接受命令，不表示平移已经完成。
+    CommandHeader.Feedback header = 1;
+  }
+}
+
+
 // 下发底盘速度命令。
 message AgvSetVelocityCommand {
   // 请求体。

protos/cmvr/api/agv_service.proto

@@ -72,4 +72,8 @@ service AgvService {
 
   // 停止当前建图/扫图会话。
   rpc stopMapping(CommandHeader.Request) returns (CommandHeader.Feedback);
+
+
+  // 按指定速度平移固定距离。成功返回仅表示控制器已接受命令。
+  rpc translate(AgvTranslateCommand.Request) returns (AgvTranslateCommand.Feedback);
 }

protos/cmvr/api/arm_service.proto

@@ -16,9 +16,11 @@ service ArmService {
   rpc servoJ(ServoJ.Request) returns (ServoJ.Response);
   rpc stopMotion(CommandHeader.Request) returns (CommandHeader.Feedback);
   rpc getJointState(JointRequest) returns (JointResponse);
-  rpc getRobotState(GetRobotState.Request) returns (GetRobotState.Response);
   rpc getPose(GetPose.Request) returns (GetPose.Response);
   rpc calibrateZeroQ(CalibrateZeroQ.Request) returns (CalibrateZeroQ.Response);
   rpc getPoseMatrix(GetPoseMatrix.Request) returns (GetPoseMatrix.Response);
   rpc computeForwardKinematics(ComputeForwardKinematics.Request) returns (ComputeForwardKinematics.Response);
+
+  // Vendor-specific arm extension currently used for AUBO cabinet IO.
+  rpc ExecuteJsonCommand(JsonDeviceCommand.Request) returns (JsonDeviceCommand.Feedback);
 }

protos/cmvr/api/arm_teleop_v1.proto

@@ -0,0 +1,133 @@
+syntax = "proto3";
+
+package cmvr.api.armteleop.v1;
+
+// Versioned, session-oriented protocol for wired arm teleoperation. Existing
+// unary ArmService RPCs intentionally remain unchanged.
+service ArmTeleopService {
+  rpc Teleoperate(stream ClientFrame) returns (stream ServerFrame);
+}
+
+enum SessionPhase {
+  SESSION_PHASE_UNSPECIFIED = 0;
+  SESSION_PHASE_OPENED = 1;
+  SESSION_PHASE_READY = 2;
+  SESSION_PHASE_ACTIVE = 3;
+  SESSION_PHASE_HOLDING = 4;
+  SESSION_PHASE_STOPPED = 5;
+  SESSION_PHASE_WATCHDOG_EXPIRED = 6;
+  SESSION_PHASE_LEASE_LOST = 7;
+  SESSION_PHASE_REJECTED = 8;
+  SESSION_PHASE_FAILED = 9;
+}
+
+enum StopReason {
+  STOP_REASON_UNSPECIFIED = 0;
+  STOP_REASON_OPERATOR_REQUEST = 1;
+  STOP_REASON_CLIENT_SHUTDOWN = 2;
+  STOP_REASON_WATCHDOG = 3;
+  STOP_REASON_LEASE_REVOKED = 4;
+  STOP_REASON_ROBOT_FAULT = 5;
+  STOP_REASON_EMERGENCY_STOP = 6;
+  STOP_REASON_PROTOCOL_ERROR = 7;
+}
+
+enum EffortSource {
+  EFFORT_SOURCE_UNSPECIFIED = 0;
+  EFFORT_SOURCE_MOTOR_ESTIMATE = 1;
+  EFFORT_SOURCE_JOINT_SENSOR = 2;
+  EFFORT_SOURCE_FORCE_TORQUE_SENSOR = 3;
+  EFFORT_SOURCE_OBSERVER = 4;
+}
+
+message RobotManifest {
+  string robot_id = 1;
+  string model_sha256 = 2;
+  string calibration_sha256 = 3;
+  repeated string joint_names = 4;
+  string position_unit = 5;
+  string velocity_unit = 6;
+  string effort_unit = 7;
+  string base_frame = 8;
+  string tool_frame = 9;
+}
+
+message OpenSession {
+  uint32 protocol_major = 1;
+  uint32 protocol_minor = 2;
+  string client_instance_id = 3;
+  RobotManifest expected_robot = 4;
+  uint32 requested_command_rate_hz = 5;
+  uint32 requested_state_rate_hz = 6;
+  uint32 watchdog_timeout_ms = 7;
+  uint32 requested_lease_ms = 8;
+  bool request_force_feedback = 9;
+}
+
+message JointSetpoint {
+  // Strictly increasing and non-zero within a session.
+  uint64 sequence = 1;
+  repeated double position_rad = 2;
+  repeated double velocity_rad_s = 3;
+  // The receiver computes its deadline from local arrival time plus this
+  // duration. Zero is invalid for an active setpoint.
+  uint32 valid_for_us = 4;
+}
+
+message ClientHeartbeat {
+  uint64 sequence = 1;
+}
+
+message StopSession {
+  StopReason reason = 1;
+  string detail = 2;
+}
+
+message ClientFrame {
+  oneof payload {
+    OpenSession open = 1;
+    JointSetpoint setpoint = 2;
+    ClientHeartbeat heartbeat = 3;
+    StopSession stop = 4;
+  }
+}
+
+message JointState {
+  uint64 sample_sequence = 1;
+  repeated double position_rad = 2;
+  repeated double velocity_rad_s = 3;
+  repeated double effort_nm = 4;
+  bool position_valid = 5;
+  bool velocity_valid = 6;
+  bool effort_valid = 7;
+  EffortSource effort_source = 8;
+  uint64 sample_age_us = 9;
+}
+
+message SessionStatus {
+  string session_id = 1;
+  SessionPhase phase = 2;
+  uint64 received_sequence = 3;
+  uint64 applied_sequence = 4;
+  uint64 dropped_setpoints = 5;
+  uint64 rejected_setpoints = 6;
+  uint32 negotiated_watchdog_ms = 7;
+  uint32 lease_remaining_ms = 8;
+  StopReason stop_reason = 9;
+  string detail = 10;
+}
+
+message RobotSafetyState {
+  bool connected = 1;
+  bool powered_on = 2;
+  bool protective_stopped = 3;
+  bool emergency_stopped = 4;
+  bool fault = 5;
+  string fault_detail = 6;
+}
+
+message ServerFrame {
+  SessionStatus status = 1;
+  JointState joint_state = 2;
+  RobotSafetyState safety = 3;
+}

protos/cmvr/api/motor_command.proto

@@ -0,0 +1,151 @@
+syntax = "proto3";
+
+package cmvr.api;
+
+import "cmvr/api/common.proto";
+import "cmvr/msgs/motor.proto";
+
+// Selects exactly one motor inside the MotorManager named by header.device_id.
+message MotorTarget {
+  CommandHeader.Request header = 1;
+  oneof selector {
+    uint32 motor_id = 2;
+    string joint_name = 3;
+  }
+}
+
+message MotorWaitOptions {
+  // Zero selects the server default (30 seconds).
+  uint32 timeout_ms = 1;
+  // Zero selects the server default (10 milliseconds).
+  uint32 poll_period_ms = 2;
+  // Zero selects the server default.
+  double position_tolerance_rad = 3;
+  double velocity_tolerance_rad_s = 4;
+  // Number of consecutive in-tolerance samples. Zero selects the default (3).
+  uint32 settle_sample_count = 5;
+}
+
+enum MotorControlType {
+  MOTOR_CONTROL_NONE = 0;
+  MOTOR_CONTROL_SET_ZERO = 1;
+  MOTOR_CONTROL_PROFILE_POSITION = 2;
+  MOTOR_CONTROL_PROFILE_VELOCITY = 3;
+  MOTOR_CONTROL_CYCLIC_POSITION = 4;
+  MOTOR_CONTROL_CYCLIC_VELOCITY = 5;
+  MOTOR_CONTROL_SET_ENABLED = 6;
+}
+
+message MotorStatus {
+  uint32 motor_id = 1;
+  string joint_name = 2;
+  cmvr.msgs.RunMode run_mode = 3;
+  double position_rad = 4;
+  double velocity_rad_s = 5;
+  bool target_reached = 6;
+  bool service_busy = 7;
+  MotorControlType active_control = 8;
+  bool emergency_stopped = 9;
+  string last_error = 10;
+}
+
+message MotorCommandResponse {
+  CommandHeader.Feedback header = 1;
+  MotorStatus status = 2;
+  uint64 elapsed_ms = 3;
+}
+
+message SetMotorZeroRequest {
+  MotorTarget target = 1;
+}
+
+message MoveMotorToZeroRequest {
+  MotorTarget target = 1;
+  double max_velocity_rad_s = 2;
+  double acceleration_rad_s2 = 3;
+  MotorWaitOptions wait = 4;
+}
+
+message ProfilePositionRequest {
+  MotorTarget target = 1;
+  double target_position_rad = 2;
+  double max_velocity_rad_s = 3;
+  double acceleration_rad_s2 = 4;
+  MotorWaitOptions wait = 5;
+}
+
+message ProfileVelocityRequest {
+  MotorTarget target = 1;
+  double target_velocity_rad_s = 2;
+  double acceleration_rad_s2 = 3;
+  MotorWaitOptions wait = 4;
+}
+
+message EmergencyStopRequest {
+  MotorTarget target = 1;
+}
+
+message GetMotorStatusRequest {
+  MotorTarget target = 1;
+}
+
+message GetMotorStatusResponse {
+  CommandHeader.Feedback header = 1;
+  MotorStatus status = 2;
+}
+
+message SetMotorEnabledRequest {
+  MotorTarget target = 1;
+  bool enabled = 2;
+}
+
+message CyclicStreamOpen {
+  MotorTarget target = 1;
+  // The device/driver watchdog is authoritative. This service watchdog prevents a
+  // stalled gRPC client from retaining control indefinitely.
+  uint32 watchdog_timeout_ms = 2;
+}
+
+message CyclicPositionSetpoint {
+  uint64 sequence = 1;
+  double target_position_rad = 2;
+  optional double target_velocity_rad_s = 3;
+}
+
+message CyclicVelocitySetpoint {
+  uint64 sequence = 1;
+  double target_velocity_rad_s = 2;
+}
+
+message CyclicPositionRequest {
+  oneof payload {
+    CyclicStreamOpen open = 1;
+    CyclicPositionSetpoint setpoint = 2;
+  }
+}
+
+message CyclicVelocityRequest {
+  oneof payload {
+    CyclicStreamOpen open = 1;
+    CyclicVelocitySetpoint setpoint = 2;
+  }
+}
+
+enum CyclicStreamPhase {
+  CYCLIC_STREAM_PHASE_UNSPECIFIED = 0;
+  CYCLIC_STREAM_OPENED = 1;
+  CYCLIC_STREAM_APPLIED = 2;
+  CYCLIC_STREAM_STOPPED = 3;
+  CYCLIC_STREAM_WATCHDOG_EXPIRED = 4;
+  CYCLIC_STREAM_FAILED = 5;
+}
+
+message CyclicControlResponse {
+  CommandHeader.Feedback header = 1;
+  CyclicStreamPhase phase = 2;
+  uint64 sequence = 3;
+  uint64 dropped_setpoints = 4;
+  // Present for OPENED and terminal responses. APPLIED deliberately omits
+  // live status so one cyclic sample does not trigger extra fieldbus reads.
+  MotorStatus status = 5;
+}

protos/cmvr/api/motor_service.proto

@@ -0,0 +1,21 @@
+syntax = "proto3";
+
+package cmvr.api;
+
+import "cmvr/api/motor_command.proto";
+
+service MotorService {
+  rpc setZero(SetMotorZeroRequest) returns (MotorCommandResponse);
+  rpc moveToZero(MoveMotorToZeroRequest) returns (MotorCommandResponse);
+  rpc profilePosition(ProfilePositionRequest) returns (MotorCommandResponse);
+  rpc profileVelocity(ProfileVelocityRequest) returns (MotorCommandResponse);
+
+  rpc streamCyclicPosition(stream CyclicPositionRequest)
+      returns (stream CyclicControlResponse);
+  rpc streamCyclicVelocity(stream CyclicVelocityRequest)
+      returns (stream CyclicControlResponse);
+
+  rpc emergencyStop(EmergencyStopRequest) returns (MotorCommandResponse);
+  rpc getStatus(GetMotorStatusRequest) returns (GetMotorStatusResponse);
+  rpc setEnabled(SetMotorEnabledRequest) returns (MotorCommandResponse);
+}

protos/cmvr/api/system_service.proto

@@ -1,6 +1,5 @@
 syntax = "proto3";
 
-import "cmvr/api/common.proto";
 import "cmvr/api/system_command.proto";
 import "cmvr/api/safety_command.proto";
 
@@ -13,7 +12,6 @@ service SystemService {
     rpc GetDeviceList(GetDeviceListCommand.Request) returns (GetDeviceListCommand.Feedback) {}
 
     rpc UpdateParams(UpdateParamsCommand.Request) returns (UpdateParamsCommand.Feedback) {}
-    rpc ExecuteJsonCommand(JsonDeviceCommand.Request) returns (JsonDeviceCommand.Feedback) {}
 
     rpc StopAll(StopAllCommand.Request) returns (StopAllCommand.Feedback) {}
 

protos/cmvr/config/device_manager_config/device_manager_config.proto

@@ -1,6 +1,25 @@
 syntax = "proto3";
 package cmvr.config;
 
+message SafetyManagerConfig {
+  enum EnforcementMode {
+    ENFORCEMENT_MODE_UNSPECIFIED = 0;
+    LEGACY = 1;
+    SHADOW = 2;
+    ENFORCE_SELECTED = 3;
+    ENFORCE_ALL = 4;
+  }
+
+  EnforcementMode mode = 1;
+  repeated string enforced_device_ids = 2;
+  uint32 stop_all_timeout_ms = 3;
+  uint32 recovery_timeout_ms = 4;
+  uint32 command_ledger_result_capacity = 5;
+  uint32 command_ledger_total_id_capacity = 6;
+  uint32 event_history_capacity = 7;
+  bool fail_startup_on_missing_control_capability = 8;
+}
+
 message DeviceConfigEntry {
   enum DeviceType {
     reserved 1, 2, 3, 4, 5, 6, 7, 10, 11;
@@ -27,6 +46,10 @@ message DeviceConfigEntry {
   DeviceType type = 2;
   string config_file = 3;
   bool enable = 4;
+  bool safety_enforce = 5;
+  uint32 maximum_safety_snapshot_age_ms = 6;
+  uint32 safety_stop_timeout_ms = 7;
+  bool required_control_device = 8;
 }
 
 message DeviceManagerConfig {
@@ -35,6 +58,7 @@ message DeviceManagerConfig {
   string description = 3;
   repeated DeviceConfigEntry devices = 4;
   bool init_all_motors_when_no_active_joints = 20;
+  SafetyManagerConfig safety = 21;
 }
 message DeviceManagerRootConfig {
   DeviceManagerConfig device_manager = 1;

protos/cmvr/config/grpc_server_config/grpc_server_config.proto

@@ -1,6 +1,68 @@
 syntax = "proto3";
 package cmvr.config;
 
+message ArmTeleopBackendConfig {
+  // Two independent gates are required: this service-level switch and the
+  // RobotArm implementation's teleop group-servo capability.
+  bool enable = 1;
+  // Also becomes RobotManifest.robot_id and the process-wide control lease
+  // resource. It must exactly match RobotArm.id().
+  string device_id = 2;
+  string model_sha256 = 3;
+  string calibration_sha256 = 4;
+  string base_frame = 5;
+  string tool_frame = 6;
+  double servo_period_s = 7;
+  // Maximum wall time allowed for one RobotArm::servoJ call.
+  uint32 max_apply_duration_us = 8;
+  // Omission is interpreted as true by the backend. Explicit false is intended
+  // only for simulation and independently supervised commissioning.
+  optional bool require_powered = 9;
+  // Bounds the first target relative to the cached measured position.
+  double max_initial_position_step_rad = 10;
+  // Bounds every later target relative to the last accepted target.
+  double max_position_step_rad = 11;
+}
+
+message GRPCSecurityConfig {
+  enum TransportMode {
+    TRANSPORT_MODE_UNSPECIFIED = 0;
+    INSECURE = 1;
+    SERVER_TLS = 2;
+    MUTUAL_TLS = 3;
+  }
+
+  enum AuthenticationMode {
+    AUTHENTICATION_MODE_UNSPECIFIED = 0;
+    DISABLED = 1;
+    STATIC_TOKEN = 2;
+    JWT = 3;
+    TLS_CLIENT_CERTIFICATE = 4;
+  }
+
+  enum RecoveryExposure {
+    RECOVERY_EXPOSURE_UNSPECIFIED = 0;
+    RECOVERY_DISABLED = 1;
+    RECOVERY_LOCAL_ONLY = 2;
+    RECOVERY_AUTHORIZED = 3;
+  }
+
+  TransportMode transport_mode = 1;
+  AuthenticationMode authentication_mode = 2;
+  RecoveryExposure recovery_exposure = 3;
+  bool allow_insecure_non_loopback = 4;
+
+  // Reserved for optional providers. Selecting an unsupported provider causes
+  // startup to fail; it never falls back to DISABLED.
+  string server_certificate_file = 5;
+  string server_private_key_file = 6;
+  string client_ca_file = 7;
+  string static_token_file = 8;
+  string jwt_issuer = 9;
+  string jwt_audience = 10;
+  string audit_file = 11;
+}
+
 message GRPCServerConfig {
   string host = 1;
   string port = 2;
@@ -12,6 +74,8 @@ message GRPCServerConfig {
   // Frames older than this monotonic age are not sent. Zero uses the service
   // default so configurations written before these fields remain low-latency.
   uint32 camera_stream_max_frame_age_ms = 6;
+  ArmTeleopBackendConfig arm_teleop_backend = 7;
+  GRPCSecurityConfig security = 8;
 }
 message GRPCServerRootConfig {
   GRPCServerConfig grpc_server = 1;

protos/cmvr/config/task_manager_config/task_manager_config.proto

@@ -8,6 +8,7 @@ message TaskConfigEntry {
     TASK_TYPE_GRPC_SERVER = 3;
     TASK_TYPE_SELF_COLLISION = 4;
     TASK_TYPE_QUIC_EDGE = 5;
+    TASK_TYPE_UME_TELEOP = 6;
   }
 
   enum TaskRunMode {

test/e2e/CMakeLists.txt

@@ -9,9 +9,9 @@ if(NOT TARGET cmvr_es::quic_test_gateway)
 endif()
 
 if(NOT TARGET cmvr_es::quic_edge_service OR
-   NOT TARGET cmvr_es::media_source_hub)
+   NOT TARGET cmvr_es::media_source_manager)
     message(FATAL_ERROR
-            "Real QUIC E2E requires the production QUIC service and MediaSourceHub")
+            "Real QUIC E2E requires the production QUIC service and MediaSourceManager")
 endif()
 
 add_executable(cmvr_quic_msquic_e2e_test
@@ -22,7 +22,7 @@ target_link_libraries(cmvr_quic_msquic_e2e_test
         PRIVATE
         cmvr_es::quic_test_gateway
         cmvr_es::quic_edge_service
-        cmvr_es::media_source_hub
+        cmvr_es::media_source_manager
 )
 
 if(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang")

test/e2e/README.md

@@ -11,7 +11,7 @@ fake transport，也不要求连接物理设备。
 | `cmvr_quic_msquic_e2e_test` | 测试进程内同时运行 Gateway 和生产 `QuicEdgeService` | TLS/ALPN、注册、DeviceManager 合成快照、至少两次心跳 ACK、H.264/AAC descriptor 精确字段、真实 DATAGRAM、分片、序列号、flags、长度与载荷哈希 |
 | `cmvr_es_quic_process_smoke_test` | 分别启动测试 Gateway 和真实 `cmvr_es` 子进程 | 临时配置加载、`QuicEdgeTask` 工厂和生命周期、节点注册、IP、禁用设备过滤、已启用设备创建失败上报、本地心跳周期、至少两次心跳 ACK、SIGTERM 安全退出 |
 
-第一项向生产 `MediaSourceHub` 注册两个有界 synthetic source：
+第一项向生产 `MediaSourceManager` 注册两个有界 synthetic source：
 
 - 2500 字节的 H.264 Annex B IDR 视频帧，用于覆盖 DATAGRAM 分片；
 - 带 ADTS header 的 AAC-LC 48 kHz 双声道音频帧。