Merge remote-tracking branch 'origin/xtkuang_dev' into linbo_dev

# Conflicts:
#	cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp
#	cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp
#	cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h
#	cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp
#	cmvr-es/devices/motor/manager/src/motor_manager.cpp
#	cmvr-es/service/grpc/server/src/grpc_arm_service.cpp
#	cmvr-es/service/grpc/server/src/grpc_camera_service.cpp

assets/toppra

@@ -1 +0,0 @@
-Subproject commit 3089c7897a5711aceb39d25919aca8c57b5c5948

cmvr-es/CMakeLists.txt

@@ -6,13 +6,15 @@ add_subdirectory(hardware)
 add_subdirectory(algorithms)
 add_subdirectory(simulate)
 add_subdirectory(devices)
-add_subdirectory(manager/control_authority)
+add_subdirectory(manager/control_authority_manager)
+add_subdirectory(manager/safety_manager)
 add_subdirectory(manager/device_manager)
-add_subdirectory(manager/media_source_hub)
+add_subdirectory(service/grpc/stop_all)
+add_subdirectory(manager/media_source_manager)
 add_subdirectory(service/quic_edge)
-add_subdirectory(service/arm_teleop_client)
 add_subdirectory(task)
 add_subdirectory(task/quic_edge_task)
+add_subdirectory(service/grpc/client)
 add_subdirectory(task/ume_teleop_task)
 add_subdirectory(manager/task_manager)
 add_subdirectory(service)

cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h

@@ -52,8 +52,16 @@ public:
 
 private:
     void ensureWorkerStarted_();
-    void workerLoop_();
-    void sendZero_();
+    void workerLoop_(std::uint64_t worker_generation);
+    bool workerGenerationCurrent_(std::uint64_t worker_generation) const;
+    std::optional<Result> sendVelocityIfCurrent_(
+        const JointVelocityCommand& velocity,
+        double acceleration,
+        std::uint64_t worker_generation);
+    void finishCommandIfCurrent_(std::uint64_t command_version,
+                                 std::uint64_t worker_generation);
+    void sendZeroIfCurrent_(std::uint64_t worker_generation);
+    void sendZeroNow_();
 
     static double velocityNorm_(const std::vector<double>& velocity);
     static double twistNorm_(const CartesianVelocity& velocity);
@@ -65,15 +73,25 @@ private:
     ReadStateCallback read_state_;
     SendVelocityCallback send_velocity_;
 
+    // lifecycle_mutex_ serializes worker creation, join, and reset. It is held
+    // across join so a new command cannot start until the retired worker exits.
+    mutable std::mutex lifecycle_mutex_;
     std::unique_ptr<std::thread> worker_;
+    std::atomic<bool> worker_running_{false};
+
     mutable std::mutex mutex_;
     std::condition_variable cv_;
-    std::atomic<bool> stop_requested_{false};
+    bool stop_requested_{false};
     bool command_active_{false};
     CartesianVelocity target_twist_{};
     FrameType target_frame_{FrameType::Base};
     double target_acceleration_{0.25};
     std::uint64_t command_version_{0};
+
+    // Every worker output is checked while holding output_mutex_. shutdown()
+    // advances the generation before sending zero, fencing stale worker writes.
+    mutable std::mutex output_mutex_;
+    std::atomic<std::uint64_t> worker_generation_{0};
     std::atomic<bool> busy_{false};
 };
 

cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp

@@ -61,20 +61,23 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
     if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || acceleration <= 0.0) {
         return Result::failure(ArmErrorCode::InvalidArgument, "speedL invalid input");
     }
-    if ((!worker_ || !worker_->joinable()) && busy_.exchange(true)) {
-        return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
-    }
-
-    ensureWorkerStarted_();
-
     std::uint64_t command_version = 0;
     {
-        std::lock_guard<std::mutex> lock(mutex_);
-        target_twist_ = velocity;
-        target_acceleration_ = acceleration;
-        target_frame_ = frame;
-        command_active_ = true;
-        command_version = ++command_version_;
+        std::lock_guard<std::mutex> lifecycle_lock(lifecycle_mutex_);
+        if (worker_ && worker_->joinable() && !worker_running_.load()) {
+            worker_->join();
+            worker_.reset();
+        }
+        ensureWorkerStarted_();
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            target_twist_ = velocity;
+            target_acceleration_ = acceleration;
+            target_frame_ = frame;
+            command_active_ = true;
+            command_version = ++command_version_;
+        }
+        busy_.store(true);
     }
     cv_.notify_all();
 
@@ -100,16 +103,21 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
 
 Result CartesianVelocityController::stop(const std::optional<double> acceleration)
 {
-    if (!worker_ || !worker_->joinable()) {
-        return Result::success();
-    }
     {
-        std::lock_guard<std::mutex> lock(mutex_);
-        target_twist_ = {};
-        target_frame_ = FrameType::Base;
-        target_acceleration_ = acceleration.has_value() ? *acceleration : config_.stop_acceleration;
-        command_active_ = true;
-        ++command_version_;
+        std::lock_guard<std::mutex> lifecycle_lock(lifecycle_mutex_);
+        if (!worker_ || !worker_->joinable() || !worker_running_.load()) {
+            return Result::success();
+        }
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            target_twist_ = {};
+            target_frame_ = FrameType::Base;
+            target_acceleration_ =
+                acceleration.has_value() ? *acceleration
+                                         : config_.stop_acceleration;
+            command_active_ = true;
+            ++command_version_;
+        }
     }
     cv_.notify_all();
     return Result::success();
@@ -117,22 +125,35 @@ Result CartesianVelocityController::stop(const std::optional<double> acceleratio
 
 void CartesianVelocityController::shutdown()
 {
+    std::lock_guard<std::mutex> lifecycle_lock(lifecycle_mutex_);
     if (!worker_ || !worker_->joinable()) {
         busy_.store(false);
         return;
     }
+
+    // Revoke the worker before issuing zero. All worker outputs perform their
+    // final generation check under output_mutex_, so none can follow this zero.
+    worker_generation_.fetch_add(1, std::memory_order_acq_rel);
     {
         std::lock_guard<std::mutex> lock(mutex_);
-        stop_requested_.store(true);
+        stop_requested_ = true;
         command_active_ = false;
         target_twist_ = {};
         target_frame_ = FrameType::Base;
+        ++command_version_;
     }
     cv_.notify_all();
+
+    sendZeroNow_();
+    busy_.store(false);
+
     worker_->join();
     worker_.reset();
-    stop_requested_.store(false);
-    busy_.store(false);
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        stop_requested_ = false;
+        command_active_ = false;
+    }
 }
 
 CartesianVelocity CartesianVelocityController::getCommandTwistBase() const
@@ -148,12 +169,26 @@ void CartesianVelocityController::ensureWorkerStarted_()
     if (worker_ && worker_->joinable()) {
         return;
     }
-    stop_requested_.store(false);
-    worker_ = std::make_unique<std::thread>(&CartesianVelocityController::workerLoop_, this);
+    const auto worker_generation =
+        worker_generation_.fetch_add(1, std::memory_order_acq_rel) + 1;
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        stop_requested_ = false;
+        command_active_ = false;
+    }
+    worker_running_.store(true);
+    worker_ = std::make_unique<std::thread>(
+        &CartesianVelocityController::workerLoop_, this, worker_generation);
 }
 
-void CartesianVelocityController::workerLoop_()
+void CartesianVelocityController::workerLoop_(
+    const std::uint64_t worker_generation)
 {
+    struct RunningGuard {
+        std::atomic<bool>& running;
+        ~RunningGuard() { running.store(false); }
+    } running_guard{worker_running_};
+
     const double dt = config_.control_period_s;
     auto next_tick = std::chrono::steady_clock::now();
 
@@ -164,9 +199,10 @@ void CartesianVelocityController::workerLoop_()
         {
             std::unique_lock<std::mutex> lock(mutex_);
             cv_.wait(lock, [&]() {
-                return stop_requested_.load() || command_active_;
+                return stop_requested_ || command_active_;
             });
-            if (stop_requested_.load()) {
+            if (stop_requested_ ||
+                !workerGenerationCurrent_(worker_generation)) {
                 break;
             }
             target_twist = target_twist_;
@@ -176,43 +212,44 @@ void CartesianVelocityController::workerLoop_()
 
         next_tick = std::chrono::steady_clock::now();
         while (true) {
+            bool stopping = false;
+            std::uint64_t active_command_version = 0;
             {
                 std::lock_guard<std::mutex> lock(mutex_);
-                if (stop_requested_.load()) {
-                    sendZero_();
-                    busy_.store(false);
-                    return;
-                }
+                stopping = stop_requested_;
                 if (!command_active_) {
                     break;
                 }
                 target_twist = target_twist_;
                 acceleration = target_acceleration_;
                 target_frame = target_frame_;
+                active_command_version = command_version_;
+            }
+            if (stopping ||
+                !workerGenerationCurrent_(worker_generation)) {
+                return;
             }
 
             if (!planner_->updateSpeedLAcceleration(acceleration)) {
                 if (twistNorm_(target_twist) < config_.stop_twist_norm && acceleration <= 0.0) {
-                    std::lock_guard<std::mutex> lock(mutex_);
-                    command_active_ = false;
-                    sendZero_();
-                    busy_.store(false);
+                    finishCommandIfCurrent_(
+                        active_command_version, worker_generation);
                     break;
                 }
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration="
                                 << acceleration;
-                sendZero_();
-                busy_.store(false);
-                return;
+                finishCommandIfCurrent_(
+                    active_command_version, worker_generation);
+                break;
             }
 
             std::vector<double> q_now;
             std::vector<double> qd_now;
             if (!read_state_(q_now, qd_now)) {
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] read_state failed";
-                sendZero_();
-                busy_.store(false);
-                return;
+                finishCommandIfCurrent_(
+                    active_command_version, worker_generation);
+                break;
             }
 
             std::vector<double> qd_cmd;
@@ -222,31 +259,31 @@ void CartesianVelocityController::workerLoop_()
                                 << target_twist.vz << ", " << target_twist.wx << ", "
                                 << target_twist.wy << ", " << target_twist.wz
                                 << "], frame=" << (target_frame == FrameType::Tool ? "Tool" : "Base");
-                sendZero_();
-                busy_.store(false);
-                return;
+                finishCommandIfCurrent_(
+                    active_command_version, worker_generation);
+                break;
             }
 
             JointVelocityCommand velocity_command;
             velocity_command.velocity = qd_cmd;
-            const auto send_result = send_velocity_(velocity_command, acceleration);
-            if (!send_result.ok()) {
-                CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] send_velocity failed: "
-                                << send_result.message;
-                sendZero_();
-                busy_.store(false);
+            const auto send_result = sendVelocityIfCurrent_(
+                velocity_command, acceleration, worker_generation);
+            if (!send_result.has_value()) {
                 return;
             }
+            if (!send_result->ok()) {
+                CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] send_velocity failed: "
+                                << send_result->message;
+                finishCommandIfCurrent_(
+                    active_command_version, worker_generation);
+                break;
+            }
 
             if (twistNorm_(target_twist) < config_.stop_twist_norm &&
                 velocityNorm_(qd_cmd) < config_.stop_command_velocity_norm &&
                 velocityNorm_(qd_now) < config_.stop_measured_velocity_norm) {
-                {
-                    std::lock_guard<std::mutex> lock(mutex_);
-                    command_active_ = false;
-                }
-                sendZero_();
-                busy_.store(false);
+                finishCommandIfCurrent_(
+                    active_command_version, worker_generation);
                 break;
             }
 
@@ -256,12 +293,67 @@ void CartesianVelocityController::workerLoop_()
         }
     }
 
-    sendZero_();
-    busy_.store(false);
+    sendZeroIfCurrent_(worker_generation);
+    if (workerGenerationCurrent_(worker_generation)) {
+        busy_.store(false);
+    }
 }
 
-void CartesianVelocityController::sendZero_()
+bool CartesianVelocityController::workerGenerationCurrent_(
+    const std::uint64_t worker_generation) const
 {
+    return worker_generation_.load(std::memory_order_acquire) ==
+           worker_generation;
+}
+
+std::optional<Result> CartesianVelocityController::sendVelocityIfCurrent_(
+    const JointVelocityCommand& velocity,
+    const double acceleration,
+    const std::uint64_t worker_generation)
+{
+    std::lock_guard<std::mutex> lock(output_mutex_);
+    if (!workerGenerationCurrent_(worker_generation)) {
+        return std::nullopt;
+    }
+    return send_velocity_(velocity, acceleration);
+}
+
+void CartesianVelocityController::finishCommandIfCurrent_(
+    const std::uint64_t command_version,
+    const std::uint64_t worker_generation)
+{
+    std::lock_guard<std::mutex> output_lock(output_mutex_);
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        if (command_version_ != command_version) {
+            return;
+        }
+        command_active_ = false;
+        busy_.store(false);
+    }
+    if (!workerGenerationCurrent_(worker_generation)) {
+        return;
+    }
+    JointVelocityCommand zero;
+    zero.velocity.assign(dof_, 0.0);
+    (void)send_velocity_(zero, 0.0);
+}
+
+void CartesianVelocityController::sendZeroIfCurrent_(
+    const std::uint64_t worker_generation)
+{
+    std::lock_guard<std::mutex> lock(output_mutex_);
+    if (!workerGenerationCurrent_(worker_generation) || !send_velocity_) {
+        return;
+    }
+    JointVelocityCommand zero;
+    zero.velocity.assign(dof_, 0.0);
+    (void)send_velocity_(zero, 0.0);
+}
+
+void CartesianVelocityController::sendZeroNow_()
+{
+    std::lock_guard<std::mutex> lock(output_mutex_);
     if (!send_velocity_) {
         return;
     }

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/CMakeLists.txt

@@ -13,3 +13,26 @@ target_link_libraries(logging PUBLIC
 add_library(cmvr_es::logging ALIAS logging)
 
 install(TARGETS logging ARCHIVE DESTINATION lib)
+
+if(BUILD_TESTING)
+    add_executable(logger_test
+            tests/logger_test.cpp
+    )
+    target_link_libraries(logger_test PRIVATE
+            cmvr_es::logging
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(NAME logger_test COMMAND logger_test)
+    set(_logger_test_environment
+            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}")
+    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+        list(APPEND _logger_test_environment
+                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+    endif()
+    set_tests_properties(logger_test PROPERTIES
+            TIMEOUT 10
+            ENVIRONMENT "${_logger_test_environment}"
+    )
+endif()

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/common/base/logging/tests/logger_test.cpp

@@ -0,0 +1,93 @@
+#include "common/base/logging/logger.h"
+
+#include <algorithm>
+#include <array>
+#include <filesystem>
+#include <fstream>
+#include <iterator>
+#include <string>
+
+#include <gtest/gtest.h>
+#include <unistd.h>
+
+namespace cmvr::logging {
+namespace {
+
+class LoggerTest : public testing::Test {
+protected:
+    void SetUp() override
+    {
+        std::array<char, 64> pattern{};
+        const std::string value = "/tmp/cmvr-logger-test-XXXXXX";
+        std::copy(value.begin(), value.end(), pattern.begin());
+        if (char* created = ::mkdtemp(pattern.data())) {
+            directory_ = created;
+        }
+        ASSERT_FALSE(directory_.empty());
+    }
+
+    void TearDown() override
+    {
+        shutdownLogging();
+        std::error_code error;
+        std::filesystem::remove_all(directory_, error);
+    }
+
+    config::LoggerConfig warningFileConfig() const
+    {
+        config::LoggerConfig config;
+        config.set_minimum_level(config::LOG_LEVEL_DEBUG);
+        config.set_directory(directory_.string());
+        config.set_flush_interval_seconds(3600);
+        auto* route = config.add_routes();
+        route->set_level(config::LOG_LEVEL_WARNING);
+        route->set_terminal(false);
+        route->set_file(true);
+        return config;
+    }
+
+    std::string fileContents() const
+    {
+        std::ifstream input(directory_ / "logger_test.log");
+        return {std::istreambuf_iterator<char>(input),
+                std::istreambuf_iterator<char>()};
+    }
+
+    std::filesystem::path directory_;
+};
+
+TEST_F(LoggerTest, ExplicitFlushMakesWarningVisibleInFile)
+{
+    ASSERT_TRUE(initLogging(
+        warningFileConfig(), "logger_test", directory_));
+
+    Logger::instance().write(
+        Level::WARNING, __FILE__, __LINE__, "warning sentinel");
+    Logger::instance().flush();
+
+    EXPECT_NE(fileContents().find("warning sentinel"), std::string::npos);
+}
+
+TEST_F(LoggerTest, ErrorIsVisibleInFileImmediately)
+{
+    auto config = warningFileConfig();
+    config.mutable_routes(0)->set_level(config::LOG_LEVEL_ERROR);
+    ASSERT_TRUE(initLogging(config, "logger_test", directory_));
+
+    Logger::instance().write(
+        Level::ERROR, __FILE__, __LINE__, "error sentinel");
+
+    EXPECT_NE(fileContents().find("error sentinel"), std::string::npos);
+}
+
+TEST_F(LoggerTest, FlushIsSafeOutsideInitializedLifetime)
+{
+    Logger::instance().flush();
+    ASSERT_TRUE(initLogging(
+        warningFileConfig(), "logger_test", directory_));
+    shutdownLogging();
+    Logger::instance().flush();
+}
+
+} // namespace
+} // namespace cmvr::logging

cmvr-es/config/README.md

@@ -73,9 +73,45 @@ cmvr_es.pb.txt
 - 新增 loader 对不认识的 enum 和未设置的 oneof 必须明确失败；当前个别历史路径仍有退化默认行为，不应复制；
 - 设备端口、坐标系、速度和单位写入注释；
 - `enable` 应由 manager 层控制，后端内部的 enable 字段不能替代 manager 开关；
-- QUIC 需要 TaskManager 与 `QuicEdgeConfig.enable` 同时开启；
+- QUIC 任务需要在 TaskManager 中显式开启；
 - QUIC 零媒体轨道是合法配置。
 
+### QUIC 多平台
+
+`QuicEdgeTask` 可以同时连接多个平台。原有顶层
+`server_host`、`server_port`、`tls` 继续表示主平台；每个 `platforms` 条目会与主平台
+并行运行。也可以不配置顶层目标，只使用一个或多个 `platforms` 条目：
+
+```protobuf
+platforms {
+  id: "operations"
+  server_host: "192.168.0.222"
+  server_port: 4433
+  enable_media: false
+  tls {
+    ca_file: "certs/cmvr-quic-ca.crt"
+    server_name: "192.168.0.222"
+  }
+}
+platforms {
+  id: "analytics"
+  server_host: "192.168.0.223"
+  server_port: 4433
+  enable_media: true
+  tls {
+    ca_file: "certs/cmvr-quic-ca.crt"
+    server_name: "192.168.0.223"
+  }
+}
+```
+
+平台 ID 和 `host:port` 必须分别唯一；配置兼容主平台时，其平台 ID 使用任务
+`QuicEdgeConfig.id`，新增条目也不能与它重名。每个平台拥有独立的 QUIC 连接、
+注册会话、心跳序号、ACK 超时和重连退避，一个平台断线不会阻塞其他平台。
+`enable_media` 默认为 `false`，此时仍发送注册、心跳、网络接口和设备状态，但不会
+复制音视频；设为 `true` 才会把全局 `tracks` 转发到该平台。兼容的顶层主平台保持
+原有媒体行为。
+
 ### gRPC 相机实时流
 
 [`tasks/grpc_server_task/grpc_server_task.pb.txt`](tasks/grpc_server_task/grpc_server_task.pb.txt)

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

@@ -17,6 +17,7 @@ arm {
       tool_frame: "tool0"
       username: "aubo"
       password: "123456"
+      auto_power_on_after_hardware_estop_release: true
     }
   }
 }

cmvr-es/config/logger/logger.pb.txt

@@ -13,17 +13,17 @@ logger {
   routes {
     level: LOG_LEVEL_WARNING
     terminal: true
-    file: false
+    file: true
   }
   routes {
     level: LOG_LEVEL_ERROR
     terminal: true
-    file: false
+    file: true
   }
   routes {
     level: LOG_LEVEL_FATAL
     terminal: true
-    file: false
+    file: true
   }
 
   directory: "../log"

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

@@ -4,6 +4,18 @@ device_manager {
   description: "cmvr edge system version 0.1"
   init_all_motors_when_no_active_joints: true
 
+  # The unified safety coordinator observes all decisions while the legacy
+  # gates remain authoritative during staged hardware migration.
+  safety {
+    mode: SHADOW
+    stop_all_timeout_ms: 15000
+    recovery_timeout_ms: 10000
+    command_ledger_result_capacity: 4096
+    command_ledger_total_id_capacity: 262144
+    event_history_capacity: 2048
+    fail_startup_on_missing_control_capability: false
+  }
+
   devices {
     id: "mujoco_world"
     type: DEVICE_TYPE_MUJOCO_WORLD

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

@@ -6,6 +6,15 @@ grpc_server {
   camera_stream_max_pending_frames: 2
   camera_stream_max_frame_age_ms: 250
 
+  # Current small-scope deployment intentionally keeps the existing clients
+  # certificate-free. Recovery remains unavailable over the network.
+  security {
+    transport_mode: INSECURE
+    authentication_mode: DISABLED
+    recovery_exposure: RECOVERY_DISABLED
+    allow_insecure_non_loopback: true
+  }
+
   # The RobotArm adapter is implemented, but remains explicitly closed until
   # the device itself enables teleop group servo, real hashes are provisioned,
   # and group-write timing and independent stop behavior pass hardware review.

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

@@ -29,6 +29,21 @@ quic_edge {
     allow_insecure: false
   }
 
+  # Additional platforms run concurrently with the primary endpoint above.
+  # Each one has an independent connection, registration, heartbeat ACK state
+  # and reconnect loop. Media forwarding is opt-in for additional platforms.
+  # platforms {
+  #   id: "operations_backup"
+  #   server_host: "192.168.0.223"
+  #   server_port: 4433
+  #   enable_media: false
+  #   tls {
+  #     ca_file: "certs/cmvr-quic-ca.crt"
+  #     server_name: "192.168.0.223"
+  #     allow_insecure: false
+  #   }
+  # }
+
   reconnect {
     initial_delay_ms: 500
     maximum_delay_ms: 30000

cmvr-es/devices/README.md

@@ -222,7 +222,7 @@ CameraDeviceConfig / AGVDeviceConfig / ... 的外层 id
 
 ## 摄像头与麦克风实时流
 
-设备实现抽象流接口后，由 [`../manager/media_source_hub/`](../manager/media_source_hub/) 适配给 gRPC 和 QUIC，不应在设备后端实现两套协议代码。
+设备实现抽象流接口后，由 [`../manager/media_source_manager/`](../manager/media_source_manager/) 适配给 gRPC 和 QUIC，不应在设备后端实现两套协议代码。
 
 当前 Hub 轨道：
 
@@ -312,7 +312,7 @@ adapter 检测到描述变化后创建新 descriptor，设备后端不要自行
 - 满队列覆盖旧数据是实时媒体的预期行为；
 - `waitEncodedFrame()` 必须有有限 timeout，不能永久阻塞。
 
-MediaSourceHub Subscription 同样是单消费者对象，不同协议或客户端必须各自订阅。
+MediaSourceManager Subscription 同样是单消费者对象，不同协议或客户端必须各自订阅。
 
 发布后的 `MediaFrame`、`TrackDescriptor` 和 payload 不可再修改。
 
@@ -334,19 +334,19 @@ MediaSourceHub Subscription 同样是单消费者对象，不同协议或客户
 无硬件参考测试：
 
 - [`camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp`](camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp)
-- [`../manager/media_source_hub/tests/media_source_hub_test.cpp`](../manager/media_source_hub/tests/media_source_hub_test.cpp)
+- [`../manager/media_source_manager/tests/media_source_manager_test.cpp`](../manager/media_source_manager/tests/media_source_manager_test.cpp)
 
 ```bash
 cmake -S . -B build \
   -DCMVR_ARCH=x86 \
   -DBUILD_TESTING=ON \
-  -DCMVR_MEDIA_SOURCE_HUB_BUILD_TESTS=ON
+  -DCMVR_MEDIA_SOURCE_MANAGER_BUILD_TESTS=ON
 
 cmake --build build -j"$(nproc)"
 
 ctest \
   --test-dir build \
-  -R 'hikvision_camera_callback_test|media_source_hub_test' \
+  -R 'hikvision_camera_callback_test|media_source_manager_test' \
   --output-on-failure
 ```
 

cmvr-es/devices/arm/aubo_arm/README.md

@@ -17,7 +17,7 @@
 - DeviceManager 配置：
   [`../../../config/manager/device_manager.pb.txt`](../../../config/manager/device_manager.pb.txt)
 - ArmService 实现：
-  [`../../../service/grpc/src/grpc_arm_service.cpp`](../../../service/grpc/src/grpc_arm_service.cpp)
+  [`../../../service/grpc/server/src/grpc_arm_service.cpp`](../../../service/grpc/server/src/grpc_arm_service.cpp)
 - Proto：[`../../../../protos/cmvr/api/arm_service.proto`](../../../../protos/cmvr/api/arm_service.proto)
 
 仓库配置使用 SDK RPC 端口 `30004`。现场部署必须填写真实控制器地址和凭据，
@@ -106,16 +106,27 @@ cmake --install build
 - 后端使用独立 SDK RPC 会话持续读取控制器的 `SafetyModeType`、
   `RobotModeType` 和硬件急停来源；首次有效样本前、监控断线或样本过期时，
   所有 Move、Speed、Servo 和程序启动请求均按不安全状态拒绝；
-- 硬件急停、防护停机、Safety Fault/Violation 会锁存安全事件，并使当前运动
-  generation 失效。控制器重新报告 `Normal`/`ReducedMode` 不会自动解除锁存；
-- 锁存后会终止直接运动与程序、关闭 servo 模式并清理控制器轨迹。只有确认
-  `ExecId == -1`、普通队列和轨迹队列均为空、运行时已停止且机械臂稳定后，
-  显式 `torqueOn`/`clearFault`/`unlockProtectiveStop` 才可能恢复运动权限；
-- 恢复流程不会调用 `resume`、`arbitraryResume`、`startMove`，也不会重新提交
-  急停前的目标、速度、servo 指令或程序；
+- 硬件急停会立即使当前运动 generation 失效，并在急停输入有效期间保持锁存。
+  检测到硬件急停输入消失且控制器重新报告 `Normal`/`ReducedMode` 后，后端应
+  自动执行 `poweron()` 和 `startup()`，恢复到 `Running` 后再完成安全确认并开放新的
+  gRPC 控制指令；防护停机和 Safety Fault/Violation 仍保持显式恢复语义；
+- `emergencyStop()` 使用独立的 `SoftwareEmergencyStop` 锁存。即使软件急停在真实
+  硬件急停有效期间触发，后续硬件采样也不能覆盖该锁存，释放硬件急停开关不会
+  自动清除软件急停；它只能通过显式安全恢复流程解除；
+- 锁存后会终止直接运动与程序、关闭 servo 模式并清理控制器轨迹。硬件急停
+  自动恢复先上电到 `Idle`，在刹车释放前清理 runtime、servo 和轨迹队列，再执行
+  `startup()`；到达 `Running` 后还会再次确认 `ExecId == -1`、普通队列和轨迹队列
+  均为空、运行时已停止且机械臂稳定，全部成立后才能解除锁存；
+- 当前 AUBO 配置通过 `auto_power_on_after_hardware_estop_release: true` 显式启用自动
+  上电。自动确认失败时继续保持 fail-closed，并允许通过 `torqueOn`/`clearFault`/
+  `unlockProtectiveStop` 显式重试；本轮释放期间收到 `stopMotion()` 或 `torqueOff()`
+  会取消自动上电，显式停止始终优先；
+- 恢复流程只调用 `poweron()` 和 `startup()`，不会调用 `resume`、`arbitraryResume`、
+  `startMove`，也不会重新提交急停前的目标、速度、servo 指令或程序；
 - AUBO SDK 未在本地文档中保证急停期间 `clearPath` 的可用性，也未说明释放
-  急停开关后的控制器恢复时序。因此本实现保持 fail-closed 并在释放后再次清队列，
-  但“释放开关后零位移”的最终保证仍需真机验证及控制器侧安全配置配合；
+  急停开关后的控制器恢复时序。因此自动恢复必须在释放后再次清队列并完成上述
+  安全确认；无法确认时不得解除锁存。“释放开关后零位移”的最终保证仍需真机
+  验证及控制器侧安全配置配合；
 - 只访问控制柜 Standard 数字 IO，不访问工具端 IO、可配置 IO 或安全 IO；
 - `set_do` 不修改输出 runstate；
 - 只有 `StandardOutputRunState::None` 的通道允许写入，否则返回

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

@@ -3,6 +3,7 @@
 #include "devices/arm/aubo_arm/aubo_motion_result.h"
 #include "devices/arm/aubo_arm/aubo_motion_state.h"
 #include "devices/arm/aubo_arm/aubo_safety_state.h"
+#include "devices/arm/aubo_arm/aubo_torque_on_result.h"
 
 #include <algorithm>
 #include <cctype>
@@ -173,6 +174,20 @@ public:
         return true;
     }
 
+    bool completeHardwareEmergencyStop(
+        const bool robot_running,
+        const bool controller_idle,
+        const bool cancellation_confirmed)
+    {
+        if (!state_->completeHardwareEmergencyStopRecovery(
+                token_, robot_running, controller_idle,
+                cancellation_confirmed)) {
+            return false;
+        }
+        completed_ = true;
+        return true;
+    }
+
 private:
     std::shared_ptr<aubo_internal::SafetyState> state_;
     aubo_internal::RecoveryToken token_;
@@ -303,6 +318,8 @@ const char* safetyConditionName(
         return "SystemEmergencyStop";
     case Condition::RobotEmergencyStop:
         return "RobotEmergencyStop";
+    case Condition::SoftwareEmergencyStop:
+        return "SoftwareEmergencyStop";
     case Condition::Fault:
         return "Fault";
     case Condition::Unknown:
@@ -327,6 +344,7 @@ SafetyMode publicSafetyMode(
     case Condition::SystemEmergencyStop:
         return SafetyMode::SystemEmergencyStop;
     case Condition::RobotEmergencyStop:
+    case Condition::SoftwareEmergencyStop:
         return SafetyMode::EmergencyStop;
     case Condition::Violation:
     case Condition::Fault:
@@ -375,6 +393,8 @@ struct AuboSafetyMonitor final {
     std::atomic<int> runtime_state{
         static_cast<int>(RuntimeState::Stopped)};
     std::atomic<int> emergency_stop_source{-1};
+    std::atomic<bool> hardware_emergency_stop_latched{false};
+    std::atomic<bool> automatic_recovery_suppressed{false};
     std::atomic<int> servo_mode_select{0};
     std::atomic<std::int64_t> last_sample_ns{0};
     std::atomic<bool> cancellation_confirmed{true};
@@ -386,6 +406,8 @@ struct AuboSafetyMonitor final {
     std::condition_variable wait_cv;
     std::mutex termination_mutex;
     std::recursive_mutex command_rpc_mutex;
+    bool auto_power_on_after_hardware_estop_release{false};
+    std::function<void()> on_hardware_estop_auto_recovered;
     std::string arm_id;
 };
 
@@ -448,6 +470,16 @@ void publishSafetySample(
     monitor->servo_mode_select.store(servo_mode_select);
     monitor->last_sample_ns.store(monotonicNowNs());
 
+    if (emergency_stop_source != 0) {
+        const bool first_sample_for_event =
+            !monitor->hardware_emergency_stop_latched.exchange(true);
+        if (first_sample_for_event) {
+            monitor->automatic_recovery_suppressed.store(false);
+        }
+    } else if (!current.latched) {
+        monitor->hardware_emergency_stop_latched.store(false);
+    }
+
     if (previous.observed != condition ||
         (!previous.latched && current.latched)) {
         if (current.latched) {
@@ -545,11 +577,34 @@ bool enforceControllerTermination(
     const std::shared_ptr<arcs::aubo_sdk::RpcClient>& rpc_client,
     const std::shared_ptr<AuboSafetyMonitor>& monitor)
 {
+    std::unique_lock<std::recursive_mutex> command_rpc_lock(
+        monitor->command_rpc_mutex);
     std::unique_lock termination_lock(monitor->termination_mutex);
     monitor->motion_state->cancelActiveForSafety();
-    const auto stop_request = monitor->motion_state->beginStop();
+    auto stop_request = monitor->motion_state->beginStop();
     if (!stop_request.started()) {
-        return monitor->cancellation_confirmed.load();
+        constexpr auto kExistingStopTimeout = std::chrono::seconds(6);
+        const auto existing_result =
+            monitor->motion_state->waitForStopCompletion(
+                stop_request,
+                std::chrono::duration_cast<std::chrono::milliseconds>(
+                    kExistingStopTimeout));
+        if (existing_result == aubo_internal::StopWaitStatus::Timeout) {
+            monitor->cancellation_confirmed.store(false);
+            return false;
+        }
+
+        // A regular stopMotion confirms direct-motion idle, while safety
+        // termination additionally verifies runtime, servo and controller
+        // queues. Re-enter the stop state and perform that stronger check. A
+        // failed existing stop is retried here as well, while MotionState
+        // remains fail-closed between the two attempts.
+        monitor->motion_state->cancelActiveForSafety();
+        stop_request = monitor->motion_state->beginStop();
+        if (!stop_request.started()) {
+            monitor->cancellation_confirmed.store(false);
+            return false;
+        }
     }
 
     const auto fail = [&monitor]() {
@@ -810,6 +865,185 @@ bool controllerStillQuiescent(
                RuntimeState::Stopped;
 }
 
+bool hardwareEmergencyStopRecoveryCurrent(
+    const std::shared_ptr<AuboSafetyMonitor>& monitor,
+    const aubo_internal::RecoveryToken token)
+{
+    const auto snapshot = monitor->safety_state->snapshot();
+    return token.valid() && !monitor->stop_requested.load() &&
+           !monitor->automatic_recovery_suppressed.load() &&
+           monitor->emergency_stop_source.load() == 0 && snapshot.latched &&
+           snapshot.recovery_in_progress && snapshot.epoch == token.epoch &&
+           !snapshot.software_emergency_stop_latched &&
+           snapshot.latched_reason ==
+               aubo_internal::SafetyCondition::RobotEmergencyStop &&
+           aubo_internal::isMotionSafe(snapshot.observed);
+}
+
+bool waitForHardwareEmergencyStopRecoveryMode(
+    const RobotInterfacePtr& robot_interface,
+    const std::shared_ptr<AuboSafetyMonitor>& monitor,
+    const aubo_internal::RecoveryToken token,
+    const RobotModeType target_mode)
+{
+    const auto deadline =
+        std::chrono::steady_clock::now() + std::chrono::seconds(20);
+    while (std::chrono::steady_clock::now() < deadline) {
+        if (!hardwareEmergencyStopRecoveryCurrent(monitor, token)) {
+            return false;
+        }
+        if (robot_interface->getRobotState()->getRobotModeType() ==
+            target_mode) {
+            return true;
+        }
+        if (monitorWait(monitor, std::chrono::milliseconds(100))) {
+            return false;
+        }
+    }
+    return false;
+}
+
+bool autoPowerOnAfterHardwareEmergencyStop(
+    const std::shared_ptr<arcs::aubo_sdk::RpcClient>& rpc_client,
+    const std::shared_ptr<AuboSafetyMonitor>& monitor,
+    const RobotInterfacePtr& robot_interface)
+{
+    std::unique_lock<std::recursive_mutex> command_rpc_lock(
+        monitor->command_rpc_mutex);
+    refreshSafetySample(rpc_client, monitor, robot_interface);
+    const auto snapshot = monitor->safety_state->snapshot();
+    if (!aubo_internal::shouldAutoRecoverHardwareEmergencyStop(
+            snapshot,
+            monitor->hardware_emergency_stop_latched.load(),
+            monitor->emergency_stop_source.load(),
+            monitor->auto_power_on_after_hardware_estop_release,
+            monitor->automatic_recovery_suppressed.load())) {
+        return false;
+    }
+
+    const auto token = monitor->safety_state->beginRecovery(snapshot.epoch);
+    if (!token.has_value()) {
+        return false;
+    }
+    SafetyRecoveryGuard recovery{monitor->safety_state, *token};
+    const auto fail = [&monitor](const std::string& detail) {
+        CMVR_LOG(WARNING)
+            << "[AuboArm] hardware emergency-stop automatic power-on "
+               "failed, id="
+            << monitor->arm_id << ", detail=" << detail;
+        return false;
+    };
+
+    try {
+        cancelForSafetyTransition(monitor);
+        if (!hardwareEmergencyStopRecoveryCurrent(monitor, *token)) {
+            return fail("recovery was cancelled before controller setup");
+        }
+
+        double mass = 0.0;
+        std::vector<double> cog(3, 0.0);
+        std::vector<double> aom(3, 0.0);
+        std::vector<double> inertia(6, 0.0);
+        const int payload_ret = robot_interface->getRobotConfig()->setPayload(
+            mass, cog, aom, inertia);
+        if (payload_ret != arcs::common_interface::AUBO_OK) {
+            return fail("setPayload ret=" + std::to_string(payload_ret));
+        }
+        if (!hardwareEmergencyStopRecoveryCurrent(monitor, *token)) {
+            return fail("recovery was cancelled after payload setup");
+        }
+
+        auto current_mode =
+            robot_interface->getRobotState()->getRobotModeType();
+        if (current_mode != RobotModeType::Running &&
+            current_mode != RobotModeType::Idle) {
+            const int power_on_ret =
+                robot_interface->getRobotManage()->poweron();
+            if (power_on_ret != arcs::common_interface::AUBO_OK) {
+                return fail("poweron ret=" + std::to_string(power_on_ret));
+            }
+            if (!waitForHardwareEmergencyStopRecoveryMode(
+                    robot_interface, monitor, *token,
+                    RobotModeType::Idle)) {
+                return fail("Idle was not reached after poweron");
+            }
+            current_mode = RobotModeType::Idle;
+        }
+
+        refreshSafetySample(rpc_client, monitor, robot_interface);
+        if (!hardwareEmergencyStopRecoveryCurrent(monitor, *token)) {
+            return fail("safety state changed before brake release");
+        }
+
+        const bool cleanup_ok = current_mode == RobotModeType::Running
+            ? enforceControllerTermination(rpc_client, monitor)
+            : prepareControllerForStartup(rpc_client, monitor);
+        if (!cleanup_ok) {
+            return fail("old runtime, servo, or path state could not be cleared");
+        }
+        if (!hardwareEmergencyStopRecoveryCurrent(monitor, *token)) {
+            return fail("safety state changed during pre-startup cleanup");
+        }
+
+        if (current_mode != RobotModeType::Running) {
+            const int startup_ret =
+                robot_interface->getRobotManage()->startup();
+            if (startup_ret != arcs::common_interface::AUBO_OK) {
+                return fail("startup ret=" + std::to_string(startup_ret));
+            }
+            if (!waitForHardwareEmergencyStopRecoveryMode(
+                    robot_interface, monitor, *token,
+                    RobotModeType::Running)) {
+                return fail("Running was not reached after startup");
+            }
+        }
+
+        refreshSafetySample(rpc_client, monitor, robot_interface);
+        if (!hardwareEmergencyStopRecoveryCurrent(monitor, *token) ||
+            monitor->robot_mode.load() !=
+                static_cast<int>(RobotModeType::Running)) {
+            return fail("controller safety changed during startup");
+        }
+
+        // Startup is allowed to energize the arm, but it must not revive an
+        // old controller operation. Terminate once more in Running mode and
+        // require a fresh empty/steady observation before reopening commands.
+        cancelForSafetyTransition(monitor);
+        if (!enforceControllerTermination(rpc_client, monitor)) {
+            return fail("post-startup controller quiescence was not confirmed");
+        }
+        refreshSafetySample(rpc_client, monitor, robot_interface);
+        const bool robot_running =
+            monitor->robot_mode.load() ==
+            static_cast<int>(RobotModeType::Running);
+        const bool controller_idle =
+            robot_running &&
+            hardwareEmergencyStopRecoveryCurrent(monitor, *token) &&
+            controllerStillQuiescent(rpc_client, robot_interface);
+        if (!recovery.completeHardwareEmergencyStop(
+                robot_running,
+                controller_idle,
+                monitor->cancellation_confirmed.load())) {
+            return fail("safety epoch changed before recovery commit");
+        }
+
+        monitor->hardware_emergency_stop_latched.store(false);
+        monitor->automatic_recovery_suppressed.store(false);
+        if (monitor->on_hardware_estop_auto_recovered) {
+            monitor->on_hardware_estop_auto_recovered();
+        }
+        CMVR_LOG(INFO)
+            << "[AuboArm] hardware emergency-stop release automatically "
+               "powered on and enabled, id="
+            << monitor->arm_id;
+        return true;
+    } catch (const std::exception& error) {
+        return fail(error.what());
+    } catch (...) {
+        return fail("unknown exception");
+    }
+}
+
 void runSafetyMonitor(
     const std::shared_ptr<AuboSafetyMonitor>& monitor,
     const std::string& ip,
@@ -844,7 +1078,27 @@ void runSafetyMonitor(
                             refreshSafetySample(
                                 rpc_client, monitor, robot_interface);
 
-                            if (monitor->safety_state->snapshot().latched) {
+                            auto safety = monitor->safety_state->snapshot();
+                            const bool hardware_estop_released =
+                                aubo_internal::
+                                    shouldAutoRecoverHardwareEmergencyStop(
+                                        safety,
+                                        monitor
+                                            ->hardware_emergency_stop_latched
+                                            .load(),
+                                        monitor->emergency_stop_source.load(),
+                                        monitor
+                                            ->auto_power_on_after_hardware_estop_release,
+                                        monitor
+                                            ->automatic_recovery_suppressed
+                                            .load());
+                            if (hardware_estop_released) {
+                                (void)autoPowerOnAfterHardwareEmergencyStop(
+                                    rpc_client, monitor, robot_interface);
+                                safety = monitor->safety_state->snapshot();
+                            }
+
+                            if (safety.latched) {
                                 if (monitor->cancellation_confirmed.load() &&
                                     !controllerStillQuiescent(
                                         rpc_client, robot_interface)) {
@@ -1010,18 +1264,38 @@ RobotInterfacePtr getPrimaryRobotInterface(const std::shared_ptr<arcs::aubo_sdk:
     return robot_interface;
 }
 
-bool waitForRobotMode(const RobotInterfacePtr& robot_interface,
-                      const RobotModeType& target_mode)
+enum class RobotModeWaitResult {
+    Reached,
+    Cancelled,
+    Timeout,
+};
+
+RobotModeWaitResult waitForRobotMode(
+    const RobotInterfacePtr& robot_interface,
+    const RobotModeType& target_mode,
+    const std::function<bool()>& cancellation_requested)
 {
     const auto start_time = std::chrono::steady_clock::now();
     while (std::chrono::steady_clock::now() - start_time < std::chrono::seconds(20)) {
+        if (cancellationRequested(cancellation_requested)) {
+            return RobotModeWaitResult::Cancelled;
+        }
         const auto current_mode = robot_interface->getRobotState()->getRobotModeType();
         if (current_mode == target_mode) {
-            return true;
+            return RobotModeWaitResult::Reached;
         }
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
     }
-    return false;
+    return cancellationRequested(cancellation_requested)
+        ? RobotModeWaitResult::Cancelled
+        : RobotModeWaitResult::Timeout;
+}
+
+bool waitForRobotMode(const RobotInterfacePtr& robot_interface,
+                      const RobotModeType& target_mode)
+{
+    return waitForRobotMode(robot_interface, target_mode, {}) ==
+           RobotModeWaitResult::Reached;
 }
 
 template <typename IsCancelled>
@@ -1487,6 +1761,12 @@ bool AuboArm::busy() const
 }
 
 Result AuboArm::torqueOn()
+{
+    return torqueOn({});
+}
+
+Result AuboArm::torqueOn(
+    const std::function<bool()>& cancellation_requested)
 {
     std::shared_ptr<arcs::aubo_sdk::RpcClient> rpc_client;
     std::shared_ptr<AuboSafetyMonitor> monitor;
@@ -1500,22 +1780,82 @@ Result AuboArm::torqueOn()
         monitor = sdk_->safety_monitor;
     }
 
+    bool cancellation_latched = false;
+    bool controller_mutated = false;
+    const std::function<bool()> cancellation_check =
+        [&cancellation_requested, &cancellation_latched]() {
+            if (!cancellation_latched) {
+                cancellation_latched = cancellationRequested(
+                    cancellation_requested);
+            }
+            return cancellation_latched;
+        };
+    const auto cancelled_before_startup = []() {
+        return Result::failure(
+            ArmErrorCode::CommandRejected,
+            "[AuboArm] torqueOn cancelled before controller startup");
+    };
+    const auto cancellation_result = [&]() -> std::optional<Result> {
+        if (!cancellation_check()) {
+            return std::nullopt;
+        }
+        if (!controller_mutated) {
+            return cancelled_before_startup();
+        }
+
+        try {
+            std::unique_lock<std::recursive_mutex> cleanup_rpc_lock(
+                monitor->command_rpc_mutex);
+            cancelForSafetyTransition(monitor);
+            if (!enforceControllerTermination(rpc_client, monitor)) {
+                return Result::failure(
+                    ArmErrorCode::CommandFailed,
+                    "[AuboArm] torqueOn cancelled: control ownership was revoked, but controller safety termination could not be confirmed");
+            }
+            return Result::failure(
+                ArmErrorCode::CommandRejected,
+                "[AuboArm] torqueOn cancelled: control ownership was revoked; controller safety termination confirmed");
+        } catch (const std::exception& e) {
+            return Result::failure(
+                ArmErrorCode::CommandFailed,
+                "[AuboArm] torqueOn cancelled: controller safety termination raised an exception: " +
+                    std::string(e.what()));
+        } catch (...) {
+            return Result::failure(
+                ArmErrorCode::CommandFailed,
+                "[AuboArm] torqueOn cancelled: controller safety termination raised an unknown exception");
+        }
+    };
+
     try {
         if (!monitor) {
             return Result::failure(
                 ArmErrorCode::RobotNotReady,
                 "[AuboArm] torqueOn failed: hardware safety monitor is unavailable");
         }
+
+        if (cancellation_check()) {
+            return cancelled_before_startup();
+        }
         std::unique_lock<std::recursive_mutex> command_rpc_lock(
             monitor->command_rpc_mutex);
+        if (cancellation_check()) {
+            return cancelled_before_startup();
+        }
         Result interface_result;
         auto robot_interface = getPrimaryRobotInterface(
             rpc_client, "torqueOn", interface_result);
         if (!interface_result.ok()) {
             return interface_result;
         }
+        if (const auto cancelled = cancellation_result()) {
+            return *cancelled;
+        }
 
         refreshSafetySample(rpc_client, monitor, robot_interface);
+        if (const auto cancelled = cancellation_result()) {
+            return *cancelled;
+        }
         auto safety_snapshot = monitor->safety_state->snapshot();
         const std::uint64_t entry_safety_epoch = safety_snapshot.epoch;
         if (monitor->emergency_stop_source.load() != 0) {
@@ -1554,21 +1894,37 @@ Result AuboArm::torqueOn()
                         std::string(safetyConditionName(condition)));
             }
 
-            const int restart_ret =
-                robot_interface->getRobotManage()->restartInterfaceBoard();
-            if (restart_ret != arcs::common_interface::AUBO_OK) {
-                return Result::failure(
-                    ArmErrorCode::CommandFailed,
-                    "[AuboArm] torqueOn recovery failed: restartInterfaceBoard ret=" +
-                        std::to_string(restart_ret));
+            controller_mutated = true;
+            if (const auto mutation_result =
+                    aubo_internal::runTorqueOnControllerMutation(
+                        arcs::common_interface::AUBO_OK,
+                        [&robot_interface] {
+                            return robot_interface->getRobotManage()
+                                ->restartInterfaceBoard();
+                        },
+                        [](const int return_code) {
+                            return Result::failure(
+                                ArmErrorCode::CommandFailed,
+                                "[AuboArm] torqueOn recovery failed: "
+                                "restartInterfaceBoard ret=" +
+                                    std::to_string(return_code));
+                        },
+                        cancellation_result)) {
+                return *mutation_result;
             }
 
             const auto safety_deadline =
                 std::chrono::steady_clock::now() +
                 std::chrono::seconds(10);
             do {
+                if (const auto cancelled = cancellation_result()) {
+                    return *cancelled;
+                }
                 std::this_thread::sleep_for(
                     std::chrono::milliseconds(100));
+                if (const auto cancelled = cancellation_result()) {
+                    return *cancelled;
+                }
                 refreshSafetySample(
                     rpc_client, monitor, robot_interface);
                 safety_snapshot = monitor->safety_state->snapshot();
@@ -1580,6 +1936,10 @@ Result AuboArm::torqueOn()
                      safety_deadline);
         }
 
+        if (const auto cancelled = cancellation_result()) {
+            return *cancelled;
+        }
+
         safety_snapshot = monitor->safety_state->snapshot();
         if (safety_snapshot.epoch != entry_safety_epoch) {
             return Result::failure(
@@ -1595,6 +1955,9 @@ Result AuboArm::torqueOn()
         }
 
         if (recovering) {
+            if (const auto cancelled = cancellation_result()) {
+                return *cancelled;
+            }
             const auto token = monitor->safety_state->beginRecovery(
                 entry_safety_epoch);
             if (!token.has_value()) {
@@ -1607,30 +1970,71 @@ Result AuboArm::torqueOn()
                 monitor->safety_state, recovery_token);
         }
 
+        if (const auto cancelled = cancellation_result()) {
+            return *cancelled;
+        }
+
         double mass = 0.0;
         std::vector<double> cog(3, 0.0);
         std::vector<double> aom(3, 0.0);
         std::vector<double> inertia(6, 0.0);
-        robot_interface->getRobotConfig()->setPayload(mass, cog, aom, inertia);
+        controller_mutated = true;
+        if (const auto mutation_result =
+                aubo_internal::runTorqueOnControllerMutation(
+                    arcs::common_interface::AUBO_OK,
+                    [&robot_interface, mass, &cog, &aom, &inertia] {
+                        return robot_interface->getRobotConfig()->setPayload(
+                            mass, cog, aom, inertia);
+                    },
+                    [](const int return_code) {
+                        return Result::failure(
+                            ArmErrorCode::CommandFailed,
+                            "[AuboArm] torqueOn failed: setPayload ret=" +
+                                std::to_string(return_code));
+                    },
+                    cancellation_result)) {
+            return *mutation_result;
+        }
 
         auto current_mode =
             robot_interface->getRobotState()->getRobotModeType();
         if (current_mode != RobotModeType::Running &&
             current_mode != RobotModeType::Idle) {
-            const int poweron_ret =
-                robot_interface->getRobotManage()->poweron();
-            if (poweron_ret != arcs::common_interface::AUBO_OK) {
-                return Result::failure(
-                    ArmErrorCode::CommandFailed,
-                    "[AuboArm] torqueOn failed: poweron ret=" +
-                        std::to_string(poweron_ret));
+            if (const auto cancelled = cancellation_result()) {
+                return *cancelled;
             }
-            if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Idle)) {
+            controller_mutated = true;
+            if (const auto mutation_result =
+                    aubo_internal::runTorqueOnControllerMutation(
+                        arcs::common_interface::AUBO_OK,
+                        [&robot_interface] {
+                            return robot_interface->getRobotManage()->poweron();
+                        },
+                        [](const int return_code) {
+                            return Result::failure(
+                                ArmErrorCode::CommandFailed,
+                                "[AuboArm] torqueOn failed: poweron ret=" +
+                                    std::to_string(return_code));
+                        },
+                        cancellation_result)) {
+                return *mutation_result;
+            }
+            const auto idle_wait = waitForRobotMode(
+                robot_interface,
+                arcs::common_interface::RobotModeType::Idle,
+                cancellation_check);
+            if (idle_wait != RobotModeWaitResult::Reached) {
+                if (const auto cancelled = cancellation_result()) {
+                    return *cancelled;
+                }
                 return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Idle");
             }
             current_mode = RobotModeType::Idle;
         }
 
+        if (const auto cancelled = cancellation_result()) {
+            return *cancelled;
+        }
         refreshSafetySample(rpc_client, monitor, robot_interface);
         auto before_brake_release =
             monitor->safety_state->snapshot();
@@ -1651,6 +2055,10 @@ Result AuboArm::torqueOn()
         }
 
         if (recovering) {
+            if (const auto cancelled = cancellation_result()) {
+                return *cancelled;
+            }
+            controller_mutated = true;
             cancelForSafetyTransition(monitor);
             const bool cleanup_ok = current_mode == RobotModeType::Running
                 ? enforceControllerTermination(rpc_client, monitor)
@@ -1660,23 +2068,50 @@ Result AuboArm::torqueOn()
                     ArmErrorCode::CommandFailed,
                     "[AuboArm] torqueOn recovery failed: old controller queue could not be acknowledged and cleared before brake release");
             }
+            if (const auto cancelled = cancellation_result()) {
+                return *cancelled;
+            }
         }
 
         if (current_mode != RobotModeType::Running) {
-            const int startup_ret =
-                robot_interface->getRobotManage()->startup();
-            if (startup_ret != arcs::common_interface::AUBO_OK) {
-                return Result::failure(
-                    ArmErrorCode::CommandFailed,
-                    "[AuboArm] torqueOn failed: startup ret=" +
-                        std::to_string(startup_ret));
+            if (const auto cancelled = cancellation_result()) {
+                return *cancelled;
             }
-            if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Running)) {
+            controller_mutated = true;
+            if (const auto mutation_result =
+                    aubo_internal::runTorqueOnControllerMutation(
+                        arcs::common_interface::AUBO_OK,
+                        [&robot_interface] {
+                            return robot_interface->getRobotManage()->startup();
+                        },
+                        [](const int return_code) {
+                            return Result::failure(
+                                ArmErrorCode::CommandFailed,
+                                "[AuboArm] torqueOn failed: startup ret=" +
+                                    std::to_string(return_code));
+                        },
+                        cancellation_result)) {
+                return *mutation_result;
+            }
+            const auto running_wait = waitForRobotMode(
+                robot_interface,
+                arcs::common_interface::RobotModeType::Running,
+                cancellation_check);
+            if (running_wait != RobotModeWaitResult::Reached) {
+                if (const auto cancelled = cancellation_result()) {
+                    return *cancelled;
+                }
                 return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Running");
             }
         }
 
+        if (const auto cancelled = cancellation_result()) {
+            return *cancelled;
+        }
         refreshSafetySample(rpc_client, monitor, robot_interface);
+        if (const auto cancelled = cancellation_result()) {
+            return *cancelled;
+        }
         const auto after_startup = monitor->safety_state->snapshot();
         const bool post_recovery_token_current = !recovering ||
             (after_startup.recovery_in_progress &&
@@ -1692,12 +2127,18 @@ Result AuboArm::torqueOn()
                 "[AuboArm] torqueOn rejected: safety state changed during startup; the new event remains latched");
         }
         if (recovering) {
+            if (const auto cancelled = cancellation_result()) {
+                return *cancelled;
+            }
             cancelForSafetyTransition(monitor);
             if (!enforceControllerTermination(rpc_client, monitor)) {
                 return Result::failure(
                     ArmErrorCode::CommandFailed,
                     "[AuboArm] torqueOn recovery failed: controller did not reach an empty, steady state after startup");
             }
+            if (const auto cancelled = cancellation_result()) {
+                return *cancelled;
+            }
             refreshSafetySample(rpc_client, monitor, robot_interface);
             const bool robot_running =
                 monitor->robot_mode.load() ==
@@ -1711,32 +2152,76 @@ Result AuboArm::torqueOn()
                     "[AuboArm] torqueOn recovery failed: safety state changed during recovery");
             }
         }
+        if (const auto cancelled = cancellation_result()) {
+            return *cancelled;
+        }
         emergency_stopped_.store(false);
         servo_mode_.store(false);
+        monitor->hardware_emergency_stop_latched.store(false);
+        monitor->automatic_recovery_suppressed.store(false);
+        if (const auto cancelled = cancellation_result()) {
+            return *cancelled;
+        }
         return Result::success();
     } catch (const std::exception& e) {
-        return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOn failed: ") + e.what());
+        auto primary_failure = Result::failure(
+            ArmErrorCode::CommandFailed,
+            std::string("[AuboArm] torqueOn failed: ") + e.what());
+        return controller_mutated
+            ? aubo_internal::preservePrimaryTorqueOnFailure(
+                  std::move(primary_failure), cancellation_result())
+            : primary_failure;
+    } catch (...) {
+        auto primary_failure = Result::failure(
+            ArmErrorCode::CommandFailed,
+            "[AuboArm] torqueOn failed: unknown exception");
+        return controller_mutated
+            ? aubo_internal::preservePrimaryTorqueOnFailure(
+                  std::move(primary_failure), cancellation_result())
+            : primary_failure;
     }
 }
 
 Result AuboArm::torqueOff()
 {
-    const auto ready = ensureConnected_("torqueOff");
-    if (!ready.ok()) {
-        return ready;
+    std::shared_ptr<arcs::aubo_sdk::RpcClient> rpc_client;
+    std::shared_ptr<AuboSafetyMonitor> monitor;
+    {
+        std::lock_guard lock(mutex_);
+        const auto ready = ensureConnected_("torqueOff");
+        if (!ready.ok()) {
+            return ready;
+        }
+        rpc_client = sdk_->rpc_client;
+        monitor = sdk_->safety_monitor;
+        if (monitor) {
+            monitor->automatic_recovery_suppressed.store(true);
+        }
     }
 
     try {
-        const auto robot_names = sdk_->rpc_client->getRobotNames();
-        if (robot_names.empty()) {
-            return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
+        std::unique_lock<std::recursive_mutex> command_rpc_lock;
+        if (monitor) {
+            command_rpc_lock = std::unique_lock<std::recursive_mutex>(
+                monitor->command_rpc_mutex);
         }
-        auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
-        if (!robot_interface) {
-            return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
+        Result interface_result;
+        auto robot_interface = getPrimaryRobotInterface(
+            rpc_client, "torqueOff", interface_result);
+        if (!interface_result.ok()) {
+            return interface_result;
         }
-        robot_interface->getRobotManage()->poweroff();
-        if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::PowerOff)) {
+        const int power_off_ret =
+            robot_interface->getRobotManage()->poweroff();
+        if (power_off_ret != arcs::common_interface::AUBO_OK) {
+            return Result::failure(
+                ArmErrorCode::CommandFailed,
+                "[AuboArm] torqueOff failed: poweroff ret=" +
+                    std::to_string(power_off_ret));
+        }
+        if (!waitForRobotMode(
+                robot_interface,
+                arcs::common_interface::RobotModeType::PowerOff)) {
             return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOff failed: timeout waiting for PowerOff");
         }
         return Result::success();
@@ -1758,7 +2243,7 @@ Result AuboArm::emergencyStop()
         std::lock_guard lock(mutex_);
         if (sdk_ && sdk_->safety_monitor) {
             sdk_->safety_monitor->safety_state->observe(
-                aubo_internal::SafetyCondition::RobotEmergencyStop);
+                aubo_internal::SafetyCondition::SoftwareEmergencyStop);
             cancelForSafetyTransition(sdk_->safety_monitor);
         }
     }
@@ -2282,6 +2767,13 @@ Result AuboArm::stopMotion_(
         }
 
         const auto motion_state = sdk_->motion_state;
+        const auto monitor = sdk_->safety_monitor;
+        std::unique_lock<std::recursive_mutex> command_rpc_lock;
+        if (monitor) {
+            monitor->automatic_recovery_suppressed.store(true);
+            command_rpc_lock = std::unique_lock<std::recursive_mutex>(
+                monitor->command_rpc_mutex);
+        }
         aubo_internal::MotionKind forced_kind =
             aubo_internal::MotionKind::None;
         if (requested_kind == MotionStopKind::Joint) {
@@ -2293,9 +2785,28 @@ Result AuboArm::stopMotion_(
         const auto stop_request =
             motion_state->beginStop(forced_kind);
         if (!stop_request.started()) {
+            busy_.store(true);
+            constexpr auto kExistingStopTimeout =
+                std::chrono::seconds(6);
+            const auto existing_result =
+                motion_state->waitForStopCompletion(
+                    stop_request,
+                    std::chrono::duration_cast<std::chrono::milliseconds>(
+                        kExistingStopTimeout));
+            busy_.store(motion_state->busy());
+            if (existing_result ==
+                aubo_internal::StopWaitStatus::Completed) {
+                return Result::success();
+            }
+            if (existing_result ==
+                aubo_internal::StopWaitStatus::Timeout) {
+                return Result::failure(
+                    ArmErrorCode::Timeout,
+                    "[AuboArm] stopMotion failed: timeout waiting for the existing stop operation to complete");
+            }
             return Result::failure(
-                ArmErrorCode::RobotNotReady,
-                "[AuboArm] stopMotion rejected: another stop operation is in progress");
+                ArmErrorCode::CommandFailed,
+                "[AuboArm] stopMotion failed: the existing stop operation could not confirm controller idle");
         }
         busy_.store(true);
         StopStateGuard stop_state_guard{
@@ -2706,6 +3217,14 @@ Result AuboArm::connect(const std::string& ip, const int port)
         sdk_state->safety_monitor->motion_state =
             sdk_state->motion_state;
         sdk_state->safety_monitor->arm_id = id_;
+        sdk_state->safety_monitor
+            ->auto_power_on_after_hardware_estop_release =
+            vendor_cfg_.auto_power_on_after_hardware_estop_release();
+        sdk_state->safety_monitor->on_hardware_estop_auto_recovered =
+            [this] {
+                busy_.store(false);
+                servo_mode_.store(false);
+            };
         publishSafetySample(
             sdk_state->safety_monitor,
             robot_interface->getRobotState()->getSafetyModeType(),
@@ -3377,7 +3896,9 @@ Result AuboArm::ensureMotionReady_(
         if (condition ==
                 aubo_internal::SafetyCondition::RobotEmergencyStop ||
             condition ==
-                aubo_internal::SafetyCondition::SystemEmergencyStop) {
+                aubo_internal::SafetyCondition::SystemEmergencyStop ||
+            condition ==
+                aubo_internal::SafetyCondition::SoftwareEmergencyStop) {
             code = ArmErrorCode::RobotInEmergencyStop;
         } else if (
             condition == aubo_internal::SafetyCondition::ProtectiveStop ||
@@ -3388,12 +3909,21 @@ Result AuboArm::ensureMotionReady_(
             condition == aubo_internal::SafetyCondition::Violation) {
             code = ArmErrorCode::RobotInFault;
         }
+        std::string recovery_instruction =
+            "; clear the hardware condition and perform explicit recovery";
+        if (condition ==
+                aubo_internal::SafetyCondition::RobotEmergencyStop &&
+            monitor->auto_power_on_after_hardware_estop_release &&
+            !monitor->automatic_recovery_suppressed.load()) {
+            recovery_instruction =
+                "; release the physical emergency stop and wait for "
+                "automatic power-on recovery";
+        }
         return Result::failure(
             code,
             "[AuboArm] " + context +
                 " rejected: hardware safety latch is " +
-                safetyConditionName(condition) +
-                "; clear the hardware condition and perform explicit recovery");
+                safetyConditionName(condition) + recovery_instruction);
     }
 
     if (monitor->robot_mode.load() !=

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

@@ -3,6 +3,7 @@
 
 #include <atomic>
 #include <cstdint>
+#include <functional>
 #include <memory>
 #include <mutex>
 #include <optional>
@@ -36,6 +37,8 @@ public:
     bool supportsActionQueueMotion() const noexcept override { return true; }
 
     Result torqueOn() override;
+    Result torqueOn(
+        const std::function<bool()>& cancellation_requested) override;
     Result torqueOff() override;
     Result calibrateZeroQ(const std::string& joint_name) override;
     Result emergencyStop() override;

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

@@ -5,6 +5,7 @@
 #include <chrono>
 #include <condition_variable>
 #include <cstdint>
+#include <memory>
 #include <mutex>
 
 namespace cmvr::device::aubo_internal {
@@ -54,11 +55,50 @@ enum class StopStartStatus {
     AlreadyStopping,
 };
 
+enum class StopWaitStatus {
+    Completed,
+    Failed,
+    Timeout,
+};
+
+class StopCompletion final {
+public:
+    StopWaitStatus waitFor(const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(mutex_);
+        if (!cv_.wait_for(lock, timeout, [this]() { return completed_; })) {
+            return StopWaitStatus::Timeout;
+        }
+        return succeeded_
+            ? StopWaitStatus::Completed
+            : StopWaitStatus::Failed;
+    }
+
+private:
+    friend class MotionState;
+
+    void finish(const bool succeeded)
+    {
+        {
+            std::lock_guard lock(mutex_);
+            succeeded_ = succeeded;
+            completed_ = true;
+        }
+        cv_.notify_all();
+    }
+
+    std::mutex mutex_;
+    std::condition_variable cv_;
+    bool completed_{false};
+    bool succeeded_{false};
+};
+
 struct StopRequest {
     StopStartStatus status{StopStartStatus::AlreadyStopping};
     MotionKind kind{MotionKind::None};
     MotionToken active_token;
     bool tracked_motion{false};
+    std::shared_ptr<StopCompletion> completion;
 
     bool started() const noexcept
     {
@@ -152,10 +192,17 @@ public:
     {
         std::lock_guard lock(mutex_);
         if (stop_in_progress_) {
-            return {};
+            return {
+                StopStartStatus::AlreadyStopping,
+                MotionKind::None,
+                {},
+                false,
+                active_stop_completion_};
         }
 
+        auto completion = std::make_shared<StopCompletion>();
         stop_in_progress_ = true;
+        active_stop_completion_ = completion;
         const MotionToken active = owner_active_
             ? active_token_
             : MotionToken{};
@@ -186,7 +233,8 @@ public:
             StopStartStatus::Started,
             kind,
             active,
-            tracked_motion};
+            tracked_motion,
+            completion};
     }
 
     SafetyCancelResult cancelActiveForSafety()
@@ -248,27 +296,51 @@ public:
                active_token_.generation == token.generation;
     }
 
+    StopWaitStatus waitForStopCompletion(
+        const StopRequest& request,
+        const std::chrono::milliseconds timeout) const
+    {
+        if (!request.completion) {
+            return StopWaitStatus::Failed;
+        }
+        return request.completion->waitFor(timeout);
+    }
+
     bool completeStop()
     {
-        std::lock_guard lock(mutex_);
-        if (owner_active_) {
-            return false;
+        std::shared_ptr<StopCompletion> completion;
+        {
+            std::lock_guard lock(mutex_);
+            if (owner_active_) {
+                return false;
+            }
+            stop_in_progress_ = false;
+            blocked_ = false;
+            active_token_ = {};
+            last_kind_ = MotionKind::None;
+            previous_kind_ = MotionKind::None;
+            completion = std::move(active_stop_completion_);
+            owner_finished_cv_.notify_all();
+        }
+        if (completion) {
+            completion->finish(true);
         }
-        stop_in_progress_ = false;
-        blocked_ = false;
-        active_token_ = {};
-        last_kind_ = MotionKind::None;
-        previous_kind_ = MotionKind::None;
-        owner_finished_cv_.notify_all();
         return true;
     }
 
     void failStop()
     {
-        std::lock_guard lock(mutex_);
-        stop_in_progress_ = false;
-        blocked_ = true;
-        owner_finished_cv_.notify_all();
+        std::shared_ptr<StopCompletion> completion;
+        {
+            std::lock_guard lock(mutex_);
+            stop_in_progress_ = false;
+            blocked_ = true;
+            completion = std::move(active_stop_completion_);
+            owner_finished_cv_.notify_all();
+        }
+        if (completion) {
+            completion->finish(false);
+        }
     }
 
     bool busy() const
@@ -286,6 +358,7 @@ private:
     MotionToken active_token_;
     MotionKind last_kind_{MotionKind::None};
     MotionKind previous_kind_{MotionKind::None};
+    std::shared_ptr<StopCompletion> active_stop_completion_;
     bool owner_active_{false};
     bool stop_in_progress_{false};
     bool blocked_{false};

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

@@ -19,6 +19,7 @@ enum class SafetyCondition {
     SafeguardStop,
     SystemEmergencyStop,
     RobotEmergencyStop,
+    SoftwareEmergencyStop,
     Fault,
 };
 
@@ -74,8 +75,25 @@ struct SafetySnapshot {
     std::uint64_t epoch{0};
     bool latched{false};
     bool recovery_in_progress{false};
+    bool software_emergency_stop_latched{false};
 };
 
+inline bool shouldAutoRecoverHardwareEmergencyStop(
+    const SafetySnapshot& snapshot,
+    const bool hardware_emergency_stop_was_observed,
+    const int current_emergency_stop_source,
+    const bool auto_power_on_enabled,
+    const bool automatic_recovery_suppressed) noexcept
+{
+    return auto_power_on_enabled && !automatic_recovery_suppressed &&
+           hardware_emergency_stop_was_observed && snapshot.latched &&
+           !snapshot.recovery_in_progress &&
+           !snapshot.software_emergency_stop_latched &&
+           snapshot.latched_reason == SafetyCondition::RobotEmergencyStop &&
+           isMotionSafe(snapshot.observed) &&
+           current_emergency_stop_source == 0;
+}
+
 // Hardware safety is an event, not a level. Once an unsafe state has been
 // observed, returning to Normal only changes the observed level. A separate,
 // explicit recovery must prove that the old controller operation has been
@@ -89,6 +107,9 @@ public:
         std::lock_guard lock(mutex_);
         const bool changed = observed_ != condition;
         observed_ = condition;
+        if (condition == SafetyCondition::SoftwareEmergencyStop) {
+            software_emergency_stop_latched_ = true;
+        }
         if (isMotionSafe(condition)) {
             return;
         }
@@ -98,7 +119,12 @@ public:
         }
         latched_ = true;
         recovery_in_progress_ = false;
-        latched_reason_ = condition;
+        // A physical E-stop sample can continue arriving after a software
+        // E-stop request. Keep the software stop independently latched so a
+        // later physical-input release can never clear it automatically.
+        latched_reason_ = software_emergency_stop_latched_
+            ? SafetyCondition::SoftwareEmergencyStop
+            : condition;
     }
 
     std::optional<SafetyPermit> tryPermit() const
@@ -144,6 +170,34 @@ public:
             return false;
         }
 
+        latched_ = false;
+        recovery_in_progress_ = false;
+        latched_reason_ = SafetyCondition::Unknown;
+        software_emergency_stop_latched_ = false;
+        ++epoch_;
+        return true;
+    }
+
+    // The caller may clear the physical E-stop latch only after it has powered
+    // the controller, released the brakes, and then re-confirmed an empty,
+    // steady controller in Running mode. This never authorizes replaying the
+    // old target, runtime program, or servo session.
+    bool completeHardwareEmergencyStopRecovery(
+        const RecoveryToken token,
+        const bool robot_running,
+        const bool controller_idle,
+        const bool cancellation_confirmed)
+    {
+        std::lock_guard lock(mutex_);
+        if (!token.valid() || token.epoch != epoch_ || !latched_ ||
+            !recovery_in_progress_ ||
+            software_emergency_stop_latched_ ||
+            latched_reason_ != SafetyCondition::RobotEmergencyStop ||
+            !isMotionSafe(observed_) || !robot_running || !controller_idle ||
+            !cancellation_confirmed) {
+            return false;
+        }
+
         latched_ = false;
         recovery_in_progress_ = false;
         latched_reason_ = SafetyCondition::Unknown;
@@ -167,7 +221,8 @@ public:
             latched_reason_,
             epoch_,
             latched_,
-            recovery_in_progress_};
+            recovery_in_progress_,
+            software_emergency_stop_latched_};
     }
 
 private:
@@ -177,6 +232,7 @@ private:
     std::uint64_t epoch_{1};
     bool latched_{false};
     bool recovery_in_progress_{false};
+    bool software_emergency_stop_latched_{false};
 };
 
 } // namespace cmvr::device::aubo_internal

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

@@ -0,0 +1,50 @@
+#ifndef CMVR_ES_AUBO_TORQUE_ON_RESULT_H
+#define CMVR_ES_AUBO_TORQUE_ON_RESULT_H
+
+#include <optional>
+#include <string>
+#include <utility>
+
+#include "common/types/arm/arm_types.h"
+
+namespace cmvr::device::aubo_internal {
+
+inline Result preservePrimaryTorqueOnFailure(
+    Result primary_failure,
+    const std::optional<Result>& cancellation_outcome)
+{
+    if (!cancellation_outcome.has_value() ||
+        cancellation_outcome->message.empty()) {
+        return primary_failure;
+    }
+
+    if (!primary_failure.message.empty()) {
+        primary_failure.message += "; ";
+    }
+    primary_failure.message +=
+        "cancellation handling: " + cancellation_outcome->message;
+    return primary_failure;
+}
+
+template <typename Mutation, typename FailureResult,
+          typename CancellationOutcome>
+std::optional<Result> runTorqueOnControllerMutation(
+    const int success_code,
+    Mutation&& mutation,
+    FailureResult&& failure_result,
+    CancellationOutcome&& cancellation_outcome)
+{
+    const int return_code = std::forward<Mutation>(mutation)();
+    if (return_code != success_code) {
+        auto primary_failure =
+            std::forward<FailureResult>(failure_result)(return_code);
+        return preservePrimaryTorqueOnFailure(
+            std::move(primary_failure),
+            std::forward<CancellationOutcome>(cancellation_outcome)());
+    }
+    return std::forward<CancellationOutcome>(cancellation_outcome)();
+}
+
+} // namespace cmvr::device::aubo_internal
+
+#endif // CMVR_ES_AUBO_TORQUE_ON_RESULT_H

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

@@ -1,6 +1,10 @@
 #include "devices/arm/aubo_arm/aubo_motion_result.h"
+#include "devices/arm/aubo_arm/aubo_torque_on_result.h"
 
 #include <iostream>
+#include <optional>
+#include <string>
+#include <vector>
 
 namespace {
 
@@ -19,7 +23,9 @@ int main()
 {
     using cmvr::device::aubo_internal::MotionCommandOutcome;
     using cmvr::device::aubo_internal::MotionWaitResult;
+    using cmvr::device::aubo_internal::preservePrimaryTorqueOnFailure;
     using cmvr::device::aubo_internal::resolveMotionCommand;
+    using cmvr::device::aubo_internal::runTorqueOnControllerMutation;
 
     constexpr int success_code = 0;
     constexpr int request_ignore_code = 13;
@@ -79,5 +85,74 @@ int main()
                    wait_cancelled) == MotionCommandOutcome::Cancelled);
     CHECK_TRUE(wait_calls == 1);
 
+    using cmvr::device::ArmErrorCode;
+    using cmvr::device::Result;
+    std::vector<std::string> mutation_trace;
+    const auto failed_mutation = runTorqueOnControllerMutation(
+        success_code,
+        [&mutation_trace] {
+            mutation_trace.push_back("mutation");
+            return 42;
+        },
+        [&mutation_trace](const int return_code) {
+            mutation_trace.push_back("primary-failure");
+            return Result::failure(
+                ArmErrorCode::CommandFailed,
+                "vendor failure ret=" + std::to_string(return_code));
+        },
+        [&mutation_trace]() -> std::optional<Result> {
+            mutation_trace.push_back("cancellation-cleanup");
+            return Result::failure(
+                ArmErrorCode::CommandRejected,
+                "controller safety termination confirmed");
+        });
+    CHECK_TRUE(failed_mutation.has_value());
+    CHECK_TRUE(failed_mutation->code == ArmErrorCode::CommandFailed);
+    CHECK_TRUE(
+        failed_mutation->message.find("vendor failure ret=42") !=
+        std::string::npos);
+    CHECK_TRUE(
+        failed_mutation->message.find(
+            "controller safety termination confirmed") !=
+        std::string::npos);
+    CHECK_TRUE(mutation_trace.size() == 3);
+    CHECK_TRUE(mutation_trace[0] == "mutation");
+    CHECK_TRUE(mutation_trace[1] == "primary-failure");
+    CHECK_TRUE(mutation_trace[2] == "cancellation-cleanup");
+
+    const auto cleanup_failed = preservePrimaryTorqueOnFailure(
+        Result::failure(
+            ArmErrorCode::CommandFailed,
+            "vendor exception"),
+        std::optional<Result>{Result::failure(
+            ArmErrorCode::CommandFailed,
+            "safety termination failed")});
+    CHECK_TRUE(cleanup_failed.code == ArmErrorCode::CommandFailed);
+    CHECK_TRUE(
+        cleanup_failed.message.find("vendor exception") !=
+        std::string::npos);
+    CHECK_TRUE(
+        cleanup_failed.message.find("safety termination failed") !=
+        std::string::npos);
+
+    int successful_cancellation_checks = 0;
+    const auto cancelled_after_success = runTorqueOnControllerMutation(
+        success_code,
+        [] { return 0; },
+        [](const int) {
+            return Result::failure(
+                ArmErrorCode::CommandFailed, "must not be used");
+        },
+        [&successful_cancellation_checks]() -> std::optional<Result> {
+            ++successful_cancellation_checks;
+            return Result::failure(
+                ArmErrorCode::CommandRejected,
+                "cancelled after successful mutation");
+        });
+    CHECK_TRUE(cancelled_after_success.has_value());
+    CHECK_TRUE(
+        cancelled_after_success->code == ArmErrorCode::CommandRejected);
+    CHECK_TRUE(successful_cancellation_checks == 1);
+
     return 0;
 }

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

@@ -1,7 +1,9 @@
 #include "devices/arm/aubo_arm/aubo_motion_state.h"
 
+#include <atomic>
 #include <chrono>
 #include <iostream>
+#include <thread>
 
 namespace {
 
@@ -36,8 +38,13 @@ int main()
                joint.token.generation);
     CHECK_TRUE(stop_joint.tracked_motion);
     CHECK_TRUE(state.cancelled(joint.token));
-    CHECK_TRUE(state.beginStop().status ==
+    const auto joined_stop_joint = state.beginStop();
+    CHECK_TRUE(joined_stop_joint.status ==
                StopStartStatus::AlreadyStopping);
+    CHECK_TRUE(
+        state.waitForStopCompletion(
+            joined_stop_joint, std::chrono::milliseconds(1)) ==
+        StopWaitStatus::Timeout);
     CHECK_TRUE(state.begin(MotionKind::Linear).status ==
                MotionStartStatus::Stopping);
     CHECK_TRUE(!state.waitForOwnerExit(
@@ -48,6 +55,10 @@ int main()
     CHECK_TRUE(state.waitForOwnerExit(
         joint.token, std::chrono::milliseconds(1)));
     CHECK_TRUE(state.completeStop());
+    CHECK_TRUE(
+        state.waitForStopCompletion(
+            joined_stop_joint, std::chrono::milliseconds(1)) ==
+        StopWaitStatus::Completed);
     CHECK_TRUE(!state.busy());
 
     const auto linear = state.begin(MotionKind::Linear);
@@ -81,7 +92,14 @@ int main()
     const auto idle_stop = state.beginStop();
     CHECK_TRUE(idle_stop.kind == MotionKind::None);
     CHECK_TRUE(!idle_stop.tracked_motion);
+    const auto joined_idle_stop = state.beginStop();
+    CHECK_TRUE(joined_idle_stop.status ==
+               StopStartStatus::AlreadyStopping);
     state.failStop();
+    CHECK_TRUE(
+        state.waitForStopCompletion(
+            joined_idle_stop, std::chrono::milliseconds(1)) ==
+        StopWaitStatus::Failed);
     const auto retry_idle_stop = state.beginStop();
     CHECK_TRUE(retry_idle_stop.kind == MotionKind::None);
     CHECK_TRUE(!retry_idle_stop.tracked_motion);
@@ -152,5 +170,44 @@ int main()
     CHECK_TRUE(retained_stop.tracked_motion);
     CHECK_TRUE(state.completeStop());
 
+    // A waiter keeps the completion for the stop it joined even when another
+    // stop starts and finishes before the waiter is scheduled again.
+    const auto concurrent_first_stop = state.beginStop();
+    CHECK_TRUE(concurrent_first_stop.started());
+    const auto concurrent_first_join = state.beginStop();
+    CHECK_TRUE(concurrent_first_join.status ==
+               StopStartStatus::AlreadyStopping);
+    std::atomic<bool> waiter_entered{false};
+    std::atomic<StopWaitStatus> first_wait_result{
+        StopWaitStatus::Timeout};
+    std::thread first_waiter([&]() {
+        waiter_entered.store(true, std::memory_order_release);
+        first_wait_result.store(
+            state.waitForStopCompletion(
+                concurrent_first_join,
+                std::chrono::milliseconds(500)),
+            std::memory_order_release);
+    });
+    while (!waiter_entered.load(std::memory_order_acquire)) {
+        std::this_thread::yield();
+    }
+
+    const bool concurrent_first_completed = state.completeStop();
+    const auto concurrent_second_stop = state.beginStop();
+    const auto concurrent_second_join = state.beginStop();
+    state.failStop();
+    first_waiter.join();
+
+    CHECK_TRUE(concurrent_first_completed);
+    CHECK_TRUE(concurrent_second_stop.started());
+    CHECK_TRUE(concurrent_second_join.status ==
+               StopStartStatus::AlreadyStopping);
+    CHECK_TRUE(first_wait_result.load(std::memory_order_acquire) ==
+               StopWaitStatus::Completed);
+    CHECK_TRUE(
+        state.waitForStopCompletion(
+            concurrent_second_join, std::chrono::milliseconds(1)) ==
+        StopWaitStatus::Failed);
+
     return 0;
 }

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

@@ -48,10 +48,19 @@ int main()
     CHECK_TRUE(state.snapshot().latched);
     CHECK_TRUE(!state.beginRecovery(state.snapshot().epoch).has_value());
 
-    // Releasing the hardware switch must not unlock motion by itself.
+    // The observed level alone does not unlock motion. The monitor must first
+    // prove the old controller operation is fully quiescent.
     state.observe(SafetyCondition::Normal);
     CHECK_TRUE(state.snapshot().latched);
     CHECK_TRUE(!state.tryPermit().has_value());
+    CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop(
+        state.snapshot(), false, 0, true, false));
+    CHECK_TRUE(shouldAutoRecoverHardwareEmergencyStop(
+        state.snapshot(), true, 0, true, false));
+    CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop(
+        state.snapshot(), true, 0, false, false));
+    CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop(
+        state.snapshot(), true, 0, true, true));
 
     const auto recovery = state.beginRecovery(state.snapshot().epoch);
     CHECK_TRUE(recovery.has_value());
@@ -60,21 +69,60 @@ int main()
 
     const auto retry = state.beginRecovery(state.snapshot().epoch);
     CHECK_TRUE(retry.has_value());
-    CHECK_TRUE(state.completeRecovery(*retry, true, true, true));
+    // Automatic release is not committed at Idle/PowerOn. The controller
+    // must have completed startup and reached Running first.
+    CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery(
+        *retry, false, true, true));
+    CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery(
+        *retry, true, false, true));
+    CHECK_TRUE(state.completeHardwareEmergencyStopRecovery(
+        *retry, true, true, true));
     const auto recovered_permit = state.tryPermit();
     CHECK_TRUE(recovered_permit.has_value());
     CHECK_TRUE(state.validate(*recovered_permit));
 
+    // Releasing a real E-stop must never clear a software-triggered stop that
+    // was latched while the hardware input was active.
+    state.observe(SafetyCondition::RobotEmergencyStop);
+    state.observe(SafetyCondition::SoftwareEmergencyStop);
+    // The hardware monitor continues publishing the physical E-stop level
+    // until the switch is released. It must not overwrite the software latch.
+    state.observe(SafetyCondition::RobotEmergencyStop);
+    state.observe(SafetyCondition::Normal);
+    CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop(
+        state.snapshot(), true, 0, true, false));
+    CHECK_TRUE(state.snapshot().software_emergency_stop_latched);
+    CHECK_TRUE(state.snapshot().latched_reason ==
+               SafetyCondition::SoftwareEmergencyStop);
+    const auto software_recovery = state.beginRecovery(
+        state.snapshot().epoch);
+    CHECK_TRUE(software_recovery.has_value());
+    CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery(
+        *software_recovery, true, true, true));
+    state.failRecovery(*software_recovery);
+    const auto explicit_software_recovery = state.beginRecovery(
+        state.snapshot().epoch);
+    CHECK_TRUE(explicit_software_recovery.has_value());
+    CHECK_TRUE(state.completeRecovery(
+        *explicit_software_recovery, true, true, true));
+    CHECK_TRUE(!state.snapshot().software_emergency_stop_latched);
+
     // A new safety event invalidates an in-flight recovery token.
     state.observe(SafetyCondition::ProtectiveStop);
     state.observe(SafetyCondition::Reduced);
     const auto stale_recovery = state.beginRecovery(
         state.snapshot().epoch);
     CHECK_TRUE(stale_recovery.has_value());
+    CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery(
+        *stale_recovery, true, true, true));
+    state.failRecovery(*stale_recovery);
+    const auto explicit_recovery = state.beginRecovery(
+        state.snapshot().epoch);
+    CHECK_TRUE(explicit_recovery.has_value());
     state.observe(SafetyCondition::SafeguardStop);
     state.observe(SafetyCondition::Normal);
     CHECK_TRUE(!state.completeRecovery(
-        *stale_recovery, true, true, true));
+        *explicit_recovery, true, true, true));
     CHECK_TRUE(state.snapshot().latched);
 
     // An old API call must not begin recovery for a newer safety event.

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

@@ -26,6 +26,7 @@ add_executable(motor_robot_arm_mujoco_test
 target_link_libraries(motor_robot_arm_mujoco_test
         PRIVATE
         cmvr_es::device::motor_robot_arm
+        cmvr_es::algorithms::arm_control
         cmvr_es::device::motor_manager
         cmvr_es::device::mujoco_motor_driver
         cmvr_es::mujoco_viewer

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

@@ -103,6 +103,12 @@ public:
     bool busy() const override;
 
 private:
+    enum class TrajectoryExecutionResult {
+        Completed,
+        Canceled,
+        Failed,
+    };
+
     bool containsJoint_(const std::string& joint_name) const;
     bool safetyStopRequested_() const;
     std::optional<Result> safetyStopResult_(const std::string& command,
@@ -115,7 +121,10 @@ private:
     std::vector<double> readJointPosition_() const;
 
     bool configureAlgorithms_();
-    bool executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory);
+    TrajectoryExecutionResult executeMoveLTrajectory_(
+        const CartesianJointTrajectory& trajectory,
+        const std::function<bool()>& cancellation_requested,
+        std::uint64_t motion_generation);
 
     static CartesianVelocityController::Config toCartesianVelocityControllerConfig_(
         const config::CartesianVelocityControllerConfig& config);
@@ -131,6 +140,7 @@ private:
     std::unordered_set<std::string> joint_set_;
     std::string motor_system_id_;
     std::shared_ptr<MotorManager> motor_manager_{nullptr};
+    std::uint64_t motor_control_claim_id_{0};
 
     std::shared_ptr<cmvr::IKSolver> ik_solver_{nullptr};
     std::shared_ptr<JointMotionPlanner> joint_planner_{nullptr};
@@ -138,6 +148,7 @@ private:
     std::unique_ptr<CartesianVelocityController> cartesian_velocity_controller_{nullptr};
 
     mutable std::mutex mutex_;
+    std::atomic<std::uint64_t> motion_generation_{0};
     std::atomic<bool> busy_{false};
     std::atomic<bool> powered_on_{false};
     mutable std::atomic<std::uint64_t> joint_state_sequence_{0};

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

@@ -35,6 +35,19 @@ struct AtomicFlagGuard {
     ~AtomicFlagGuard() { flag.store(false); }
 };
 
+bool cancellationRequested(
+    const std::function<bool()>& cancellation_requested) noexcept
+{
+    if (!cancellation_requested) {
+        return false;
+    }
+    try {
+        return cancellation_requested();
+    } catch (...) {
+        return true;
+    }
+}
+
 const config::JointLimitsConfig* configuredJointLimits(
     const config::ArmKinematicsConfig& kinematics)
 {
@@ -130,6 +143,9 @@ MotorRobotArm::~MotorRobotArm()
     if (cartesian_velocity_controller_) {
         cartesian_velocity_controller_->shutdown();
     }
+    if (motor_manager_ && motor_control_claim_id_ != 0U) {
+        motor_manager_->releaseArmJoints(motor_control_claim_id_);
+    }
 }
 
 bool MotorRobotArm::init()
@@ -178,6 +194,16 @@ bool MotorRobotArm::init()
         CMVR_LOG(ERROR) << "[MotorRobotArm] failed to configure algorithms: " << id_;
         return false;
     }
+    if (motor_control_claim_id_ == 0U) {
+        std::string claim_error;
+        if (!motor_manager_->claimArmJoints(
+                id_, joint_names_, motor_control_claim_id_, &claim_error)) {
+            CMVR_LOG(ERROR)
+                << "[MotorRobotArm] failed to claim direct motor control: "
+                << id_ << ", detail=" << claim_error;
+            return false;
+        }
+    }
     CMVR_LOG(INFO) << "[MotorRobotArm] (init): Arm '" << id_ << "' init success";
     return true;
 }
@@ -342,6 +368,7 @@ Result MotorRobotArm::calibrateZeroQ(const std::string& joint_name)
 
 Result MotorRobotArm::emergencyStop()
 {
+    motion_generation_.fetch_add(1, std::memory_order_acq_rel);
     if (cartesian_velocity_controller_) {
         cartesian_velocity_controller_->shutdown();
     }
@@ -583,6 +610,8 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
     if (const auto stopped = safetyStopResult_("moveJ")) {
         return *stopped;
     }
+    const auto motion_generation =
+        motion_generation_.load(std::memory_order_acquire);
     std::string error;
     if (!validatePositionCommand_(target, error)) {
         return Result::failure(ArmErrorCode::InvalidArgument, error);
@@ -594,7 +623,12 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
         return Result::failure(ArmErrorCode::RobotNotReady, "[MotorRobotArm] arm is busy: " + id_);
     }
     BusyGuard busy_guard{busy_};
-    std::lock_guard<std::mutex> lock(mutex_);
+
+    if (cancellationRequested(options.cancellation_requested)) {
+        return Result::failure(
+            ArmErrorCode::CommandRejected,
+            "[MotorRobotArm] moveJ canceled before planning: " + id_);
+    }
 
     JointTrajectory samples;
     if (!joint_planner_->planMoveJ(readJointPosition_(), target, options, speed_scaling_, samples)) {
@@ -606,19 +640,36 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
 
     std::vector<std::shared_ptr<AbstractMotor>> motors;
     motors.reserve(joint_names_.size());
-    for (const auto& joint_name : joint_names_) {
-        auto motor = getMotor_(joint_name);
-        if (!motor) {
-            return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name);
+    if (cancellationRequested(options.cancellation_requested)) {
+        return Result::failure(
+            ArmErrorCode::CommandRejected,
+            "[MotorRobotArm] moveJ canceled before dispatch: " + id_);
+    }
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        if (motion_generation_.load(std::memory_order_acquire) !=
+            motion_generation) {
+            return Result::failure(
+                ArmErrorCode::CommandRejected,
+                "[MotorRobotArm] moveJ canceled before dispatch: " + id_);
         }
-        if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
-            if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
-                return Result::failure(ArmErrorCode::CommandFailed,
-                                       "failed to set cyclic position mode for joint: " +
-                                           joint_name);
+        for (const auto& joint_name : joint_names_) {
+            auto motor = getMotor_(joint_name);
+            if (!motor) {
+                return Result::failure(
+                    ArmErrorCode::RobotNotReady,
+                    "motor not found for joint: " + joint_name);
             }
+            if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
+                if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
+                    return Result::failure(
+                        ArmErrorCode::CommandFailed,
+                        "failed to set cyclic position mode for joint: " +
+                            joint_name);
+                }
+            }
+            motors.push_back(std::move(motor));
         }
-        motors.push_back(std::move(motor));
     }
 
     const auto t0 = std::chrono::steady_clock::now();
@@ -636,10 +687,28 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
         std::copy_n(sample.velocity.begin(),
                     std::min(sample.velocity.size(), command_velocity.size()),
                     command_velocity.begin());
-        if (!motor_manager_->commandCyclicPositionsAtomic(
-                motors, sample.position, command_velocity)) {
-            return Result::failure(ArmErrorCode::CommandFailed,
-                                   "failed to submit atomic cyclic position command");
+        if (cancellationRequested(options.cancellation_requested)) {
+            return Result::failure(
+                ArmErrorCode::CommandRejected,
+                "[MotorRobotArm] moveJ canceled during execution: " + id_);
+        }
+        {
+            // The local generation and one complete joint frame are ordered
+            // against stopMotion(). External cancellation is intentionally
+            // evaluated before taking the device mutex because it is caller code.
+            std::lock_guard<std::mutex> lock(mutex_);
+            if (motion_generation_.load(std::memory_order_acquire) !=
+                motion_generation) {
+                return Result::failure(
+                    ArmErrorCode::CommandRejected,
+                    "[MotorRobotArm] moveJ canceled during execution: " + id_);
+            }
+            if (!motor_manager_->commandCyclicPositionsAtomic(
+                    motors, sample.position, command_velocity)) {
+                return Result::failure(
+                    ArmErrorCode::CommandFailed,
+                    "failed to submit atomic cyclic position command");
+            }
         }
         if (k + 1 < samples.size()) {
             const double next_t = samples[k + 1].time_s > 0.0
@@ -697,6 +766,7 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity,
 
 Result MotorRobotArm::stopJ(const double acceleration)
 {
+    motion_generation_.fetch_add(1, std::memory_order_acq_rel);
     if (safetyStopRequested_()) {
         return Result::success();
     }
@@ -712,6 +782,8 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
     if (const auto stopped = safetyStopResult_("moveL")) {
         return *stopped;
     }
+    const auto motion_generation =
+        motion_generation_.load(std::memory_order_acquire);
     if (cartesian_velocity_controller_) {
         cartesian_velocity_controller_->shutdown();
     }
@@ -729,6 +801,12 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
     }
     BusyGuard busy_guard{busy_};
 
+    if (cancellationRequested(options.cancellation_requested)) {
+        return Result::failure(
+            ArmErrorCode::CommandRejected,
+            "[MotorRobotArm] moveL canceled before planning: " + id_);
+    }
+
     std::vector<double> q_start;
     std::vector<double> qd_now;
     if (!readArmState_(q_start, qd_now)) {
@@ -753,13 +831,25 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
                           << ", executable_path_m=" << trajectory.executable_path_length;
     }
 
-    if (executeMoveLTrajectory_(trajectory)) {
-        return Result::success();
+    switch (executeMoveLTrajectory_(
+        trajectory,
+        options.cancellation_requested,
+        motion_generation)) {
+        case TrajectoryExecutionResult::Completed:
+            return Result::success();
+        case TrajectoryExecutionResult::Canceled:
+            return Result::failure(
+                ArmErrorCode::CommandRejected,
+                "[MotorRobotArm] moveL canceled during execution: " + id_);
+        case TrajectoryExecutionResult::Failed:
+            if (const auto stopped = safetyStopResult_("moveL", true)) {
+                return *stopped;
+            }
+            return Result::failure(
+                ArmErrorCode::CommandFailed, "moveL execution failed");
     }
-    if (const auto stopped = safetyStopResult_("moveL", true)) {
-        return *stopped;
-    }
-    return Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed");
+    return Result::failure(ArmErrorCode::CommandFailed,
+                           "moveL execution failed");
 }
 
 Result MotorRobotArm::speedL(const CartesianVelocity& velocity,
@@ -789,16 +879,26 @@ Result MotorRobotArm::stopL(const std::optional<double> acceleration)
 
 Result MotorRobotArm::stopMotion()
 {
-    stopL(0.0);
-    return stopJ(0.0);
+    // Revoke position trajectories before stopping the velocity worker. The
+    // controller fences its old worker and sends zero before joining it.
+    motion_generation_.fetch_add(1, std::memory_order_acq_rel);
+    if (cartesian_velocity_controller_) {
+        cartesian_velocity_controller_->shutdown();
+    }
+    JointVelocityCommand zero;
+    zero.velocity.assign(joint_names_.size(), 0.0);
+    return speedJ(zero, 0.0, 0.0);
 }
 
 Result MotorRobotArm::shutdown()
 {
+    motion_generation_.fetch_add(1, std::memory_order_acq_rel);
     if (cartesian_velocity_controller_) {
         cartesian_velocity_controller_->shutdown();
     }
-    return stopJ(0.0);
+    JointVelocityCommand zero;
+    zero.velocity.assign(joint_names_.size(), 0.0);
+    return speedJ(zero, 0.0, 0.0);
 }
 
 Result MotorRobotArm::startServoMode(const ServoOptions& options)
@@ -1085,29 +1185,44 @@ bool MotorRobotArm::configureAlgorithms_()
     return true;
 }
 
-bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory)
+MotorRobotArm::TrajectoryExecutionResult
+MotorRobotArm::executeMoveLTrajectory_(
+    const CartesianJointTrajectory& trajectory,
+    const std::function<bool()>& cancellation_requested,
+    const std::uint64_t motion_generation)
 {
     if (trajectory.position.empty() ||
         trajectory.velocity.size() != trajectory.position.size() ||
         trajectory.time.size() != trajectory.position.size()) {
-        return false;
+        return TrajectoryExecutionResult::Failed;
     }
     if (trajectory.position.size() == 1) {
-        return true;
+        return cancellationRequested(cancellation_requested) ||
+                motion_generation_.load(std::memory_order_acquire) !=
+                    motion_generation
+            ? TrajectoryExecutionResult::Canceled
+            : TrajectoryExecutionResult::Completed;
     }
 
     std::vector<std::shared_ptr<AbstractMotor>> motors;
     motors.reserve(joint_names_.size());
+    if (cancellationRequested(cancellation_requested)) {
+        return TrajectoryExecutionResult::Canceled;
+    }
     {
         std::lock_guard<std::mutex> lock(mutex_);
+        if (motion_generation_.load(std::memory_order_acquire) !=
+            motion_generation) {
+            return TrajectoryExecutionResult::Canceled;
+        }
         for (const auto& joint_name : joint_names_) {
             auto motor = getMotor_(joint_name);
             if (!motor) {
-                return false;
+                return TrajectoryExecutionResult::Failed;
             }
             if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
                 if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
-                    return false;
+                    return TrajectoryExecutionResult::Failed;
                 }
             }
             motors.push_back(std::move(motor));
@@ -1117,23 +1232,40 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj
     auto next_deadline = std::chrono::steady_clock::now();
     for (std::size_t i = 1; i < trajectory.position.size(); ++i) {
         if (safetyStopRequested_()) {
-            return false;
+            return TrajectoryExecutionResult::Failed;
         }
         const double dt_segment = std::max(1e-4, trajectory.time[i] - trajectory.time[i - 1]);
         const auto& position = trajectory.position[i];
         const auto& velocity = trajectory.velocity[i];
         if (position.size() != motors.size() || velocity.size() != motors.size()) {
-            return false;
+            return TrajectoryExecutionResult::Failed;
         }
-        if (!motor_manager_->commandCyclicPositionsAtomic(motors, position, velocity)) {
-            return false;
+        if (cancellationRequested(cancellation_requested)) {
+            return TrajectoryExecutionResult::Canceled;
+        }
+        {
+            // Keep the local stop decision and the complete joint frame in the
+            // same critical section as stopMotion()/stopJ().
+            std::lock_guard<std::mutex> lock(mutex_);
+            if (motion_generation_.load(std::memory_order_acquire) !=
+                motion_generation) {
+                return TrajectoryExecutionResult::Canceled;
+            }
+            if (!motor_manager_->commandCyclicPositionsAtomic(
+                    motors, position, velocity)) {
+                return TrajectoryExecutionResult::Failed;
+            }
         }
         next_deadline += std::chrono::duration_cast<std::chrono::steady_clock::duration>(
             std::chrono::duration<double>(dt_segment));
         std::this_thread::sleep_until(next_deadline);
     }
 
-    return true;
+    return cancellationRequested(cancellation_requested) ||
+            motion_generation_.load(std::memory_order_acquire) !=
+                motion_generation
+        ? TrajectoryExecutionResult::Canceled
+        : TrajectoryExecutionResult::Completed;
 }
 
 CartesianVelocityController::Config MotorRobotArm::toCartesianVelocityControllerConfig_(

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

@@ -2,12 +2,17 @@
 
 #include <algorithm>
 #include <array>
+#include <atomic>
 #include <chrono>
 #include <cmath>
+#include <condition_variable>
 #include <filesystem>
+#include <functional>
+#include <future>
 #include <iostream>
 #include <limits>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <thread>
 #include <unordered_set>
@@ -99,6 +104,142 @@ struct ArmMujocoConfigCase {
     const char* config_file;
 };
 
+class BlockingCartesianMotionPlanner final : public CartesianMotionPlanner {
+public:
+    bool configureSpeedL(const config::SpeedLPlannerConfig&, std::size_t) override
+    {
+        return true;
+    }
+
+    bool configureMoveL(const config::MoveLPlannerConfig&) override
+    {
+        return true;
+    }
+
+    bool planMoveL(const CartesianPose&,
+                   const std::vector<double>&,
+                   const std::vector<double>&,
+                   double,
+                   double,
+                   double,
+                   FrameType,
+                   CartesianJointTrajectory&) override
+    {
+        return false;
+    }
+
+    bool speedLStep(const CartesianVelocity&,
+                    double,
+                    const std::vector<double>& q_measured,
+                    const std::vector<double>&,
+                    std::vector<double>& qd_command,
+                    FrameType) override
+    {
+        std::unique_lock lock(mutex_);
+        if (step_count_++ == 0) {
+            first_step_entered_ = true;
+            condition_.notify_all();
+            condition_.wait(lock, [&] { return release_first_step_; });
+        }
+        qd_command.assign(q_measured.size(), 0.4);
+        return true;
+    }
+
+    bool updateSpeedLAcceleration(double) override
+    {
+        return true;
+    }
+
+    CartesianVelocity getSpeedLCommandTwistBase() const override
+    {
+        return {};
+    }
+
+    bool waitForFirstStep(const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(mutex_);
+        return condition_.wait_for(
+            lock, timeout, [&] { return first_step_entered_; });
+    }
+
+    void releaseFirstStep()
+    {
+        {
+            std::lock_guard lock(mutex_);
+            release_first_step_ = true;
+        }
+        condition_.notify_all();
+    }
+
+private:
+    mutable std::mutex mutex_;
+    std::condition_variable condition_;
+    std::size_t step_count_{0};
+    bool first_step_entered_{false};
+    bool release_first_step_{false};
+};
+
+class VelocityCommandRecorder {
+public:
+    Result record(const JointVelocityCommand& velocity, double)
+    {
+        {
+            std::lock_guard lock(mutex_);
+            commands_.push_back(velocity.velocity);
+        }
+        condition_.notify_all();
+        return Result::success();
+    }
+
+    bool waitForZero(const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(mutex_);
+        return condition_.wait_for(lock, timeout, [&] {
+            return std::any_of(commands_.begin(), commands_.end(), isZero_);
+        });
+    }
+
+    bool waitForNonZeroAfter(const std::size_t index,
+                             const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(mutex_);
+        return condition_.wait_for(lock, timeout, [&] {
+            return index < commands_.size() &&
+                   std::any_of(commands_.begin() + index,
+                               commands_.end(),
+                               [](const auto& command) {
+                                   return !isZero_(command);
+                               });
+        });
+    }
+
+    std::size_t size() const
+    {
+        std::lock_guard lock(mutex_);
+        return commands_.size();
+    }
+
+    bool allZeroFrom(const std::size_t index) const
+    {
+        std::lock_guard lock(mutex_);
+        return index <= commands_.size() &&
+               std::all_of(commands_.begin() + index,
+                           commands_.end(), isZero_);
+    }
+
+private:
+    static bool isZero_(const std::vector<double>& command)
+    {
+        return std::all_of(command.begin(), command.end(), [](const double value) {
+            return std::abs(value) < 1e-12;
+        });
+    }
+
+    mutable std::mutex mutex_;
+    std::condition_variable condition_;
+    std::vector<std::vector<double>> commands_;
+};
+
 void PrintTo(const ArmMujocoConfigCase& value, std::ostream* os)
 {
     *os << value.name << " (" << value.config_file << ")";
@@ -314,6 +455,240 @@ TEST_P(MotorRobotArmMujocoTest, MoveL)
     EXPECT_LT(outcome.move_l_error, 0.04);
 }
 
+TEST_P(MotorRobotArmMujocoTest, StopMotionDoesNotWaitForMoveJCancellationCallback)
+{
+    MotionOptions options;
+    options.velocity = 0.4;
+    options.acceleration = 2.0;
+
+    std::mutex cancellation_mutex;
+    std::condition_variable cancellation_condition;
+    int cancellation_checks = 0;
+    bool release_dispatch_check = false;
+    options.cancellation_requested = [&] {
+        std::unique_lock lock(cancellation_mutex);
+        ++cancellation_checks;
+        cancellation_condition.notify_all();
+        if (cancellation_checks == 3) {
+            cancellation_condition.wait(lock, [&] {
+                return release_dispatch_check;
+            });
+        }
+        return false;
+    };
+
+    std::vector<double> target(kDof, 0.0);
+    target[0] = 0.2;
+    auto motion = std::async(std::launch::async, [&] {
+        return arm_->moveJ(JointPositionCommand{target}, options);
+    });
+
+    bool cancellation_blocked = false;
+    {
+        std::unique_lock lock(cancellation_mutex);
+        cancellation_blocked = cancellation_condition.wait_for(
+            lock, std::chrono::seconds(2), [&] {
+                return cancellation_checks >= 3;
+            });
+    }
+
+    auto stop = std::async(std::launch::async, [&] {
+        return arm_->stopMotion();
+    });
+    EXPECT_TRUE(cancellation_blocked);
+    EXPECT_EQ(stop.wait_for(std::chrono::seconds(1)),
+              std::future_status::ready);
+
+    {
+        std::lock_guard lock(cancellation_mutex);
+        release_dispatch_check = true;
+    }
+    cancellation_condition.notify_all();
+
+    const auto motion_result = motion.get();
+    const auto stop_result = stop.get();
+    EXPECT_EQ(motion_result.code, ArmErrorCode::CommandRejected)
+        << motion_result.message;
+    EXPECT_TRUE(stop_result.ok()) << stop_result.message;
+}
+
+TEST_P(MotorRobotArmMujocoTest, StopMotionDoesNotWaitForMoveLCancellationCallback)
+{
+    const std::vector<double> initial{
+        0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0.0, 0.0};
+    MotionOptions joint_options;
+    joint_options.velocity = 2.8;
+    joint_options.acceleration = 20.0;
+    const auto setup = arm_->moveJ(
+        JointPositionCommand{initial}, joint_options);
+    ASSERT_TRUE(setup.ok()) << setup.message;
+
+    CartesianPose target = arm_->getTcpPose();
+    target.x += 0.05;
+    MotionOptions options;
+    options.velocity = 0.4;
+    options.acceleration = 5.0;
+    options.jerk = 20.0;
+
+    std::mutex cancellation_mutex;
+    std::condition_variable cancellation_condition;
+    int cancellation_checks = 0;
+    bool release_dispatch_check = false;
+    options.cancellation_requested = [&] {
+        std::unique_lock lock(cancellation_mutex);
+        ++cancellation_checks;
+        cancellation_condition.notify_all();
+        if (cancellation_checks == 3) {
+            cancellation_condition.wait(lock, [&] {
+                return release_dispatch_check;
+            });
+        }
+        return false;
+    };
+
+    auto motion = std::async(std::launch::async, [&] {
+        return arm_->moveL(target, options, FrameType::Base);
+    });
+
+    bool cancellation_blocked = false;
+    {
+        std::unique_lock lock(cancellation_mutex);
+        cancellation_blocked = cancellation_condition.wait_for(
+            lock, std::chrono::seconds(2), [&] {
+                return cancellation_checks >= 3;
+            });
+    }
+
+    auto stop = std::async(std::launch::async, [&] {
+        return arm_->stopMotion();
+    });
+    EXPECT_TRUE(cancellation_blocked);
+    EXPECT_EQ(stop.wait_for(std::chrono::seconds(1)),
+              std::future_status::ready);
+
+    {
+        std::lock_guard lock(cancellation_mutex);
+        release_dispatch_check = true;
+    }
+    cancellation_condition.notify_all();
+
+    const auto motion_result = motion.get();
+    const auto stop_result = stop.get();
+    EXPECT_EQ(motion_result.code, ArmErrorCode::CommandRejected)
+        << motion_result.message;
+    EXPECT_TRUE(stop_result.ok()) << stop_result.message;
+}
+
+TEST_P(MotorRobotArmMujocoTest, StopMotionCancelsMoveJWithoutExternalCallback)
+{
+    MotionOptions options;
+    options.velocity = 0.1;
+    options.acceleration = 0.5;
+
+    std::vector<double> target(kDof, 0.0);
+    target[0] = 0.4;
+    auto motion = std::async(std::launch::async, [&] {
+        return arm_->moveJ(JointPositionCommand{target}, options);
+    });
+
+    waitFor([&] { return arm_->busy(); }, std::chrono::seconds(1));
+    const auto stop_result = arm_->stopMotion();
+    const auto motion_result = motion.get();
+
+    EXPECT_TRUE(stop_result.ok()) << stop_result.message;
+    EXPECT_EQ(motion_result.code, ArmErrorCode::CommandRejected)
+        << motion_result.message;
+    EXPECT_FALSE(arm_->busy());
+}
+
+TEST_P(MotorRobotArmMujocoTest, StopMotionCancelsMoveLWithoutExternalCallback)
+{
+    const std::vector<double> initial{
+        0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0.0, 0.0};
+    MotionOptions joint_options;
+    joint_options.velocity = 2.8;
+    joint_options.acceleration = 20.0;
+    const auto setup = arm_->moveJ(
+        JointPositionCommand{initial}, joint_options);
+    ASSERT_TRUE(setup.ok()) << setup.message;
+
+    CartesianPose target = arm_->getTcpPose();
+    target.x += 0.08;
+    MotionOptions options;
+    options.velocity = 0.1;
+    options.acceleration = 1.0;
+    options.jerk = 5.0;
+    auto motion = std::async(std::launch::async, [&] {
+        return arm_->moveL(target, options, FrameType::Base);
+    });
+
+    waitFor([&] { return arm_->busy(); }, std::chrono::seconds(1));
+    const auto stop_result = arm_->stopMotion();
+    const auto motion_result = motion.get();
+
+    EXPECT_TRUE(stop_result.ok()) << stop_result.message;
+    EXPECT_EQ(motion_result.code, ArmErrorCode::CommandRejected)
+        << motion_result.message;
+    EXPECT_FALSE(arm_->busy());
+}
+
+TEST(CartesianVelocityControllerTest,
+     ShutdownFencesStaleWriteAndLaterSpeedLRestartsWorker)
+{
+    constexpr std::size_t dof = 2;
+    auto planner = std::make_shared<BlockingCartesianMotionPlanner>();
+    VelocityCommandRecorder recorder;
+    CartesianVelocityController controller(
+        CartesianVelocityController::Config{},
+        planner,
+        dof,
+        [](std::vector<double>& q, std::vector<double>& qd) {
+            q.assign(dof, 0.0);
+            qd.assign(dof, 0.0);
+            return true;
+        },
+        [&](const JointVelocityCommand& command, const double acceleration) {
+            return recorder.record(command, acceleration);
+        });
+
+    CartesianVelocity velocity;
+    velocity.vx = 0.1;
+    const auto first = controller.speedL(
+        velocity, 0.5, 0.0, FrameType::Base);
+    ASSERT_TRUE(first.ok()) << first.message;
+    const bool first_step_entered =
+        planner->waitForFirstStep(std::chrono::seconds(1));
+    if (!first_step_entered) {
+        planner->releaseFirstStep();
+        controller.shutdown();
+        FAIL() << "velocity worker did not enter the blocking planner step";
+    }
+
+    auto shutdown = std::async(std::launch::async, [&] {
+        controller.shutdown();
+    });
+    EXPECT_TRUE(recorder.waitForZero(std::chrono::seconds(1)));
+    EXPECT_EQ(shutdown.wait_for(std::chrono::milliseconds(20)),
+              std::future_status::timeout);
+
+    const auto zero_index = recorder.size();
+    planner->releaseFirstStep();
+    EXPECT_EQ(shutdown.wait_for(std::chrono::seconds(1)),
+              std::future_status::ready);
+    shutdown.get();
+    EXPECT_TRUE(recorder.allZeroFrom(zero_index));
+    EXPECT_FALSE(controller.busy());
+
+    const auto restart_index = recorder.size();
+    const auto restarted = controller.speedL(
+        velocity, 0.5, 0.0, FrameType::Base);
+    EXPECT_TRUE(restarted.ok()) << restarted.message;
+    EXPECT_TRUE(recorder.waitForNonZeroAfter(
+        restart_index, std::chrono::seconds(1)));
+    controller.shutdown();
+    EXPECT_FALSE(controller.busy());
+}
+
 TEST_P(MotorRobotArmMujocoTest, SpeedL)
 {
     MotorRobotArm& arm = *arm_;

cmvr-es/devices/arm/robot_arm.h

@@ -2,6 +2,7 @@
 #define CMVR_ES_ROBOT_ARM_H
 
 #include <cstddef>
+#include <functional>
 #include <memory>
 #include <optional>
 #include <string>
@@ -44,6 +45,28 @@ public:
     }
 
     virtual Result torqueOn() = 0;
+    // Long-running startup implementations may cooperatively observe loss of
+    // their control lease. Backends which have not adopted cancellation retain
+    // the legacy behavior, while still rejecting an already-cancelled request
+    // before entering the vendor API.
+    virtual Result torqueOn(
+        const std::function<bool()>& cancellation_requested)
+    {
+        if (cancellation_requested) {
+            try {
+                if (cancellation_requested()) {
+                    return Result::failure(
+                        ArmErrorCode::CommandRejected,
+                        "torqueOn cancelled before execution");
+                }
+            } catch (...) {
+                return Result::failure(
+                    ArmErrorCode::CommandRejected,
+                    "torqueOn cancellation check failed");
+            }
+        }
+        return torqueOn();
+    }
     virtual Result torqueOff() = 0;
     virtual Result calibrateZeroQ(const std::string& joint_name) = 0;
 

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/camera/abstract_camera.h

@@ -131,6 +131,12 @@ namespace cmvr::device {
 
         virtual bool startStreaming() {return true;}
         virtual void stopStreaming() {}
+        virtual bool startOperationalActivity() { return start(); }
+        // Stops activity started by CameraService::StartCamera without
+        // tearing down the device lifecycle. Implementations must return true
+        // only after the camera is quiescent and a later start() can resume it
+        // without another init(). Unsupported backends fail closed.
+        virtual bool stopOperationalActivity() { return false; }
         virtual bool controlPtz(PtzCommand command, bool stop, int speed) {
             (void)command;
             (void)stop;

cmvr-es/devices/camera/hikvision_camera/include/hikvision_camera.h

@@ -36,6 +36,7 @@ public:
     bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) override;
     bool startStreaming() override;
     void stopStreaming() override;
+    bool stopOperationalActivity() override;
     bool controlPtz(PtzCommand command, bool stop, int speed) override;
     bool executeJsonCommand(const std::string& request_json, std::string& response_json) override;
     bool requestKeyFrame() override;
@@ -56,7 +57,7 @@ private:
     void releaseSdk_();
     bool login_();
     bool startPreview_();
-    void stopPreview_();
+    bool stopPreview_();
     bool requestKeyFrame_();
     void stopRecordingUnlocked_();
     void fillIntrinsics_(Rs2Intrinsics& intrinsics) const;

cmvr-es/devices/camera/hikvision_camera/src/hikvision_camera.cpp

@@ -323,7 +323,7 @@ bool HikvisionCamera::start()
     if (state_.is_opened) {
         return true;
     }
-    if (!login_()) {
+    if (user_id_ < 0 && !login_()) {
         return false;
     }
     if (!startPreview_()) {
@@ -348,7 +348,7 @@ bool HikvisionCamera::stop()
     state_.is_streaming = false;
     stream_count_ = 0;
     resetStreamState_();
-    stopPreview_();
+    (void)stopPreview_();
     if (user_id_ >= 0) {
         NET_DVR_Logout(user_id_);
         user_id_ = -1;
@@ -544,6 +544,23 @@ void HikvisionCamera::stopStreaming()
     }
 }
 
+bool HikvisionCamera::stopOperationalActivity()
+{
+    std::lock_guard lock(ctrl_mtx_);
+    clear_error_();
+
+    if (stream_count_ != 0 || state_.is_streaming || state_.is_recording) {
+        return false;
+    }
+    if (!stopPreview_()) {
+        setError_(sdkError_("NET_DVR_StopRealPlay"));
+        return false;
+    }
+    state_.is_opened = false;
+    return real_handle_ < 0 && !state_.is_streaming &&
+        !state_.is_recording;
+}
+
 bool HikvisionCamera::controlPtz(PtzCommand command, bool stop, int speed)
 {
     std::lock_guard lock(ctrl_mtx_);
@@ -917,7 +934,7 @@ bool HikvisionCamera::startPreview_()
     return true;
 }
 
-void HikvisionCamera::stopPreview_()
+bool HikvisionCamera::stopPreview_()
 {
     const int preview_handle = real_handle_;
     {
@@ -930,9 +947,12 @@ void HikvisionCamera::stopPreview_()
         awaiting_key_frame_ = false;
     }
     if (preview_handle >= 0) {
-        NET_DVR_StopRealPlay(preview_handle);
+        if (!NET_DVR_StopRealPlay(preview_handle)) {
+            return false;
+        }
         real_handle_ = -1;
     }
+    return true;
 }
 
 void HikvisionCamera::fillIntrinsics_(Rs2Intrinsics& intrinsics) const

cmvr-es/devices/camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp

@@ -269,11 +269,30 @@ bool testCallbackPublicationLifecycle()
     CHECK_TRUE(frame.sequence == 0);
     CHECK_TRUE(frame.codec_config_generation == 3);
 
+    camera.stopStreaming();
+    CHECK_TRUE(camera.stopOperationalActivity());
+    CHECK_TRUE(g_stop_callback_count.load() == 1);
+    cmvr::device::CameraState stopped_state{};
+    camera.getState(stopped_state);
+    CHECK_TRUE(stopped_state.is_initialized);
+    CHECK_TRUE(!stopped_state.is_opened);
+    CHECK_TRUE(!stopped_state.is_streaming);
+
+    // Operational stop keeps the SDK/login lifecycle reusable. start() only
+    // recreates the preview pipeline and can stream again without init().
+    CHECK_TRUE(camera.startOperationalActivity());
+    CHECK_TRUE(camera.startStreaming());
+    emitIFrame();
+    CHECK_TRUE(camera.waitEncodedFrame(
+        frame, cursor, std::chrono::milliseconds(50)));
+    CHECK_TRUE(frame.stream_epoch == 3);
+    camera.stopStreaming();
+
     // The fake StopRealPlay invokes the SDK callback synchronously. stop()
     // owns ctrl_mtx_ here, proving the callback neither takes that mutex nor
     // publishes after the preview handle has been invalidated.
     CHECK_TRUE(camera.stop());
-    CHECK_TRUE(g_stop_callback_count.load() == 1);
+    CHECK_TRUE(g_stop_callback_count.load() == 2);
     CHECK_TRUE(!camera.waitEncodedFrame(
         frame, cursor, std::chrono::milliseconds(10)));
     return true;

cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h

@@ -50,6 +50,8 @@ public:
     void getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
     bool startStreaming() override;
     void stopStreaming() override;
+    bool startOperationalActivity() override;
+    bool stopOperationalActivity() override;
     bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) override;
 
 private:
@@ -91,6 +93,8 @@ private:
     uint64_t last_frame_id_{0};
     bool has_last_frame_id_{false};
     size_t stream_frame_index_{0};
+    std::size_t stream_count_{0};
+    bool operational_active_{false};
     bool streaming_{false};
     std::shared_ptr<FfmpegEncoderInfo> rgb_encoder_;
 };

cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp

@@ -150,6 +150,9 @@ bool MujocoCamera::start()
 bool MujocoCamera::stop()
 {
     std::lock_guard<std::mutex> lock(mtx_);
+    operational_active_ = false;
+    streaming_ = false;
+    stream_count_ = 0U;
     state_.is_streaming = false;
     state_.is_opened = false;
     destroyOffscreen_();
@@ -213,6 +216,7 @@ bool MujocoCamera::startStreaming()
         return false;
     }
     std::lock_guard<std::mutex> lock(mtx_);
+    ++stream_count_;
     streaming_ = true;
     state_.is_streaming = true;
     return true;
@@ -221,10 +225,41 @@ bool MujocoCamera::startStreaming()
 void MujocoCamera::stopStreaming()
 {
     std::lock_guard<std::mutex> lock(mtx_);
+    if (stream_count_ > 0U) {
+        --stream_count_;
+    }
+    if (stream_count_ == 0U) {
+        streaming_ = false;
+        state_.is_streaming = operational_active_;
+        stream_frame_index_ = 0;
+        rgb_encoder_.reset();
+    }
+}
+
+bool MujocoCamera::startOperationalActivity()
+{
+    if (!start()) {
+        return false;
+    }
+    std::lock_guard<std::mutex> lock(mtx_);
+    operational_active_ = true;
+    state_.is_streaming = true;
+    return true;
+}
+
+bool MujocoCamera::stopOperationalActivity()
+{
+    std::lock_guard<std::mutex> lock(mtx_);
+    if (stream_count_ != 0U || state_.is_recording) {
+        return false;
+    }
+    operational_active_ = false;
     streaming_ = false;
     state_.is_streaming = false;
+    state_.is_opened = false;
     stream_frame_index_ = 0;
     rgb_encoder_.reset();
+    return true;
 }
 
 bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index)

cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera_test.cpp

@@ -53,4 +53,40 @@ TEST(MujocoCameraTest, CapturesOffscreenRgbdFrame)
     EXPECT_GT(intrinsics.fy, 0.0f);
 }
 
+TEST(MujocoCameraTest, OperationalStopCanResumeWithoutReinitializing)
+{
+    std::uint64_t frame_id = 0;
+    cmvr::device::MujocoCamera camera(
+        [&frame_id](std::vector<unsigned char>& rgb,
+                    std::vector<float>& depth,
+                    int& width,
+                    int& height,
+                    std::uint64_t& returned_frame_id) {
+            width = 2;
+            height = 2;
+            rgb.assign(12U, 127U);
+            depth.assign(4U, 1.0F);
+            returned_frame_id = ++frame_id;
+            return true;
+        });
+
+    ASSERT_TRUE(camera.init());
+    ASSERT_TRUE(camera.startOperationalActivity());
+    ASSERT_TRUE(camera.startStreaming());
+    EXPECT_FALSE(camera.stopOperationalActivity());
+    camera.stopStreaming();
+    EXPECT_TRUE(camera.stopOperationalActivity());
+
+    cmvr::device::CameraState state{};
+    camera.getState(state);
+    EXPECT_TRUE(state.is_initialized);
+    EXPECT_FALSE(state.is_opened);
+    EXPECT_FALSE(state.is_streaming);
+
+    ASSERT_TRUE(camera.startOperationalActivity());
+    camera.getState(state);
+    EXPECT_TRUE(state.is_opened);
+    EXPECT_TRUE(state.is_streaming);
+}
+
 } // namespace

cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h

@@ -5,6 +5,10 @@
 #ifndef REALSENSE_CAMERA_H
 #define REALSENSE_CAMERA_H
 
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+
 #include "camera/abstract_camera.h"
 #include "common/base/ring_buffer.h"
 #include "devices/camera/common/include/camera_stream_encoder.h"
@@ -38,6 +42,7 @@ namespace cmvr::device{
 
         bool startStreaming() override;
         void stopStreaming() override;
+        bool stopOperationalActivity() override;
 
 
         Eigen::Vector3f get3DPointFromPixel(int u, int v) override;
@@ -45,6 +50,11 @@ namespace cmvr::device{
         rs2::frameset get_frameset(bool align);
         void streaming_worker_();
         void recording_worker_();
+        bool collectStreamingWorker_(std::chrono::milliseconds timeout);
+        bool collectRecordingWorker_(std::chrono::milliseconds timeout);
+        void markStreamingWorkerStopped_() noexcept;
+        void markRecordingWorkerStopped_() noexcept;
+        void setWorkerError_(const std::string& message) noexcept;
     private:
         int fps_;
         int width_;
@@ -79,6 +89,10 @@ namespace cmvr::device{
         std::string current_video_path_;
 
         std::mutex ctrl_mtx_{};
+        std::mutex stream_lifecycle_mtx_{};
+        std::mutex stream_stop_mtx_{};
+        std::condition_variable stream_stop_cv_{};
+        std::condition_variable recording_stop_cv_{};
         std::unique_ptr<cv::VideoWriter> video_writer_;
         std::shared_ptr<std::thread> stream_thread_;//采集线程
         std::shared_ptr<std::thread> encode_thread_;//采集线程
@@ -102,8 +116,12 @@ namespace cmvr::device{
         size_t recordingIndex_ = 0;
         size_t getImageIndex_ = 0;
 
-        bool is_streaming_running = false;
-        bool is_recording_running = false;
+        std::atomic<bool> stream_requested_{false};
+        std::atomic<bool> recording_requested_{false};
+        std::atomic<bool> stream_worker_exited_{true};
+        std::atomic<bool> recording_worker_exited_{true};
+        std::atomic<bool> is_streaming_running{false};
+        std::atomic<bool> is_recording_running{false};
         int stream_count_ = 0;
 
         cv::Mat latest_depth_;

cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp

@@ -8,6 +8,10 @@
 using namespace std;
 using namespace cmvr::device;
 
+namespace {
+constexpr auto kStreamStopTimeout = std::chrono::seconds(2);
+}
+
 
 // 检查系统中是否存在指定序列号的 RealSense 设备
 bool checkRealSenseCamera(const std::string& serialNumber = "") {
@@ -307,31 +311,39 @@ bool RealsenseCamera::start() {
 
 bool RealsenseCamera::stop() {
     //先停止录制再关闭摄像头
-    if (state_.is_recording) {
+    bool is_recording = false;
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        is_recording = state_.is_recording;
+    }
+    if (is_recording) {
         try {
             stopRecording();
         } catch (const std::exception& e) {
             CMVR_LOG(WARNING) << "[RealsenseCamera] (stop): stopRecording failed: " << e.what();
         }
     }
-    std::shared_ptr<std::thread> stream_thread_to_join;
+    std::lock_guard lifecycle_lock(stream_lifecycle_mtx_);
     {
         std::lock_guard lock(ctrl_mtx_);
         clear_error_();
-        stream_count_ = 0;
+        if (!state_.is_opened || !state_.is_initialized) {
+            state_.is_opened = false;
+            return true;
+        }
         state_.is_streaming = false;
-        state_.is_recording = false;
-        stream_thread_to_join = stream_thread_;
-        stream_thread_.reset();
+        stream_count_ = 0;
+        stream_requested_.store(false, std::memory_order_release);
     }
-    if (stream_thread_to_join && stream_thread_to_join->joinable()) {
-        stream_thread_to_join->join();
+
+    if (!collectStreamingWorker_(kStreamStopTimeout)) {
+        std::lock_guard lock(ctrl_mtx_);
+        state_.is_error = true;
+        state_.error_message = "timed out waiting for camera stream to stop";
+        return false;
     }
+
     std::lock_guard lock(ctrl_mtx_);
-    if (!state_.is_opened || !state_.is_initialized) {
-        state_.is_opened = false;
-        return true;
-    }
     try {
         pipe_.stop();
     } catch (const std::exception& e) {
@@ -399,6 +411,8 @@ void RealsenseCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) {
 }
 
 void RealsenseCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
+    std::lock_guard control_lock(ctrl_mtx_);
+    clear_error_();
     intrinsics.cx = intrinsics_.ppx;
     intrinsics.cy = intrinsics_.ppy;
     intrinsics.fx = intrinsics_.fx;
@@ -445,6 +459,8 @@ void RealsenseCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
 }
 
 void RealsenseCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) {
+    std::lock_guard control_lock(ctrl_mtx_);
+    clear_error_();
     intrinsics.cx = intrinsics_.ppx;
     intrinsics.cy = intrinsics_.ppy;
     intrinsics.fx = intrinsics_.fx;
@@ -497,24 +513,49 @@ void RealsenseCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsic
 
 
 void RealsenseCamera::startRecording(const std::string &video_path) {
-    std::lock_guard lock(ctrl_mtx_);
-    clear_error_();
-    if (mode_ != VIDEO_MODE) {
-        state_.is_error = true;
-        state_.error_message = "startRecording only supports VIDEO_MODE";
-        CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message;
+    std::lock_guard lifecycle_lock(stream_lifecycle_mtx_);
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        clear_error_();
+        if (mode_ != VIDEO_MODE) {
+            state_.is_error = true;
+            state_.error_message = "startRecording only supports VIDEO_MODE";
+            return;
+        }
+        if (!state_.is_opened) {
+            state_.is_error = true;
+            state_.error_message = "camera not opened";
+            return;
+        }
+        if (state_.is_recording ||
+            recording_requested_.load(std::memory_order_acquire)) {
+            state_.is_error = true;
+            state_.error_message = "already recording";
+            return;
+        }
+    }
+
+    if (!collectRecordingWorker_(kStreamStopTimeout)) {
+        setWorkerError_("previous camera recording did not stop");
         return;
     }
-    if (!state_.is_opened) {
-        state_.is_error = true;
-        state_.error_message = "camera not opened";
-        CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message;
+    bool collect_stale_stream = false;
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        collect_stale_stream = stream_thread_ &&
+            stream_worker_exited_.load(std::memory_order_acquire);
+    }
+    if (collect_stale_stream &&
+        !collectStreamingWorker_(kStreamStopTimeout)) {
+        setWorkerError_("previous camera stream did not stop");
         return;
     }
-    if (state_.is_recording) {
+
+    std::unique_lock lock(ctrl_mtx_);
+    if (!state_.is_opened || state_.is_recording) {
         state_.is_error = true;
-        state_.error_message = "already recording";
-        CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message;
+        state_.error_message = state_.is_recording
+            ? "already recording" : "camera not opened";
         return;
     }
 
@@ -528,8 +569,11 @@ void RealsenseCamera::startRecording(const std::string &video_path) {
         stream_ = nullptr;
         format_context_ = nullptr;
         state_.is_recording = false;
+        recording_requested_.store(false, std::memory_order_release);
+        current_video_path_.clear();
     };
 
+    bool created_stream_worker = false;
     try {
         current_video_path_ = video_path;
         std::string temp_path = current_video_path_ + ".temp"; // 临时文件
@@ -620,63 +664,67 @@ void RealsenseCamera::startRecording(const std::string &video_path) {
             cleanup_recording_resources();
             return;
         }
-        //不在录像也不在流传输，但是采集线程没有退出时。
-        if (!state_.is_streaming && !state_.is_recording) {
-            if (stream_thread_) {
-                if (stream_thread_->joinable()) {
-                    stream_thread_->join();
-                    is_streaming_running = false;
-                }
-                stream_thread_.reset();
-            }
-        }
         // 开启录像
         state_.is_recording = true;
+        recording_requested_.store(true, std::memory_order_release);
         //开启流采集线程
         if (!stream_thread_) {
+            stream_worker_exited_.store(false, std::memory_order_release);
             stream_thread_ = make_shared<thread>(&RealsenseCamera::streaming_worker_, this);
-            //延时100ms，等待流线程获取图像
-            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            created_stream_worker = true;
         }
         // 启动录像线程
         frame_count_ = 0;
-        if (recording_thread_) {
-            if (recording_thread_->joinable()) {
-                recording_thread_->join();
-                is_recording_running = false;
-            }
-            recording_thread_.reset();
-        }
+        recording_worker_exited_.store(false, std::memory_order_release);
         recording_thread_ = make_shared<thread>(&RealsenseCamera::recording_worker_, this);
 
     } catch (const std::exception& e) {
+        recording_requested_.store(false, std::memory_order_release);
+        recording_worker_exited_.store(true, std::memory_order_release);
+        recording_stop_cv_.notify_all();
+        if (!stream_thread_) {
+            stream_worker_exited_.store(true, std::memory_order_release);
+            stream_stop_cv_.notify_all();
+        }
         cleanup_recording_resources();
         state_.is_error = true;
         state_.error_message = "[RealsenseCamera] (startRecording): " + std::string(e.what());
         CMVR_LOG(ERROR) << state_.error_message;
+        lock.unlock();
+        if (created_stream_worker &&
+            !collectStreamingWorker_(kStreamStopTimeout)) {
+            setWorkerError_(
+                "failed to collect camera stream after recording start failure");
+        }
     }
 }
 
 void RealsenseCamera::stopRecording() {
-    std::lock_guard lock(ctrl_mtx_);
-    clear_error_();
-    if (mode_ != VIDEO_MODE) {
-        state_.is_error = true;
-        state_.error_message = "stopRecording only supports VIDEO_MODE";
-        CMVR_LOG(ERROR) << "[RealsenseCamera] (stopRecording): " << state_.error_message;
-        return;
+    std::lock_guard lifecycle_lock(stream_lifecycle_mtx_);
+    bool has_recording_worker = false;
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        clear_error_();
+        if (mode_ != VIDEO_MODE) {
+            state_.is_error = true;
+            state_.error_message = "stopRecording only supports VIDEO_MODE";
+            return;
+        }
+        has_recording_worker = static_cast<bool>(recording_thread_);
+        if (!state_.is_recording && !has_recording_worker) {
+            return;
+        }
+        recording_requested_.store(false, std::memory_order_release);
     }
-    if (!state_.is_recording) {
-        CMVR_LOG(WARNING) << "[RealsenseCamera] (stopRecording): not recording";
-        return;
+    if (has_recording_worker &&
+        !collectRecordingWorker_(kStreamStopTimeout)) {
+        setWorkerError_("timed out waiting for camera recording to stop");
+        throw std::runtime_error(
+            "timed out waiting for camera recording to stop");
     }
 
-    // 1. 停止录像线程
+    std::unique_lock lock(ctrl_mtx_);
     state_.is_recording = false;
-    if (recording_thread_ && recording_thread_->joinable()) {
-        recording_thread_->join();
-        recording_thread_.reset();
-    }
 
     // 2. 清理FFmpeg资源
     if (packet_) {
@@ -698,14 +746,26 @@ void RealsenseCamera::stopRecording() {
     stream_ = nullptr;
 
     // 3. 重命名临时文件为目标文件
-    std::string temp_path = current_video_path_ + ".temp";
-    if (rename(temp_path.c_str(), current_video_path_.c_str()) != 0) {
+    const std::string completed_video_path = current_video_path_;
+    const std::string temp_path = completed_video_path + ".temp";
+    if (!completed_video_path.empty() &&
+        rename(temp_path.c_str(), completed_video_path.c_str()) != 0) {
         state_.is_error = true;
-        state_.error_message = "failed to rename temp file: " + temp_path + " -> " + current_video_path_;
+        state_.error_message = "failed to rename temp file: " + temp_path +
+            " -> " + completed_video_path;
         CMVR_LOG(ERROR) << "[RealsenseCamera] (stopRecording): " << state_.error_message;
-        return;
     }
     current_video_path_.clear();
+
+    const bool collect_stream = stream_count_ == 0 &&
+        static_cast<bool>(stream_thread_);
+    lock.unlock();
+    if (collect_stream &&
+        !collectStreamingWorker_(kStreamStopTimeout)) {
+        setWorkerError_("timed out waiting for camera stream to stop");
+        throw std::runtime_error(
+            "timed out waiting for camera stream to stop");
+    }
 }
 
 void RealsenseCamera::pauseRecording() {
@@ -734,14 +794,15 @@ void RealsenseCamera::streaming_worker_() {
         const int frame_interval = 1000 / fps_;
 
         bool success = false;
-        is_streaming_running = true;
+        is_streaming_running.store(true, std::memory_order_release);
         uint64_t frame_sequence = 0;
         const uint64_t stream_epoch = static_cast<uint64_t>(
             std::chrono::duration_cast<std::chrono::nanoseconds>(
                 std::chrono::steady_clock::now().time_since_epoch()).count());
 
         // 处于流传输或者录像状态时就不退出线程
-        while (state_.is_streaming || state_.is_recording) {
+        while (stream_requested_.load(std::memory_order_acquire) ||
+               recording_requested_.load(std::memory_order_acquire)) {
             // 记录当前帧处理开始时间
             const auto frame_start_time = std::chrono::steady_clock::now();
 
@@ -750,15 +811,13 @@ void RealsenseCamera::streaming_worker_() {
             rs2::frame color_frame = frames.get_color_frame();
             rs2::frame depth_frame = frames.get_depth_frame();
             if (!color_frame) {
-                state_.is_error = true;
-                state_.error_message = "missing color frame";
-                CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_: " << state_.error_message;
+                setWorkerError_("missing color frame");
+                recording_requested_.store(false, std::memory_order_release);
                 break;
             }
             if (stream_mode_ == RGBD_MODE && !depth_frame) {
-                state_.is_error = true;
-                state_.error_message = "missing depth frame in RGBD mode";
-                CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_: " << state_.error_message;
+                setWorkerError_("missing depth frame in RGBD mode");
+                recording_requested_.store(false, std::memory_order_release);
                 break;
             }
 
@@ -843,7 +902,7 @@ void RealsenseCamera::streaming_worker_() {
             }
         }
 
-        is_streaming_running = false;
+        markStreamingWorkerStopped_();
         // 线程结束时清空队列
         stream_frame_buffer_->clear();
         recordingIndex_ = 0;
@@ -854,20 +913,21 @@ void RealsenseCamera::streaming_worker_() {
         // 线程结束时清空队列
         stream_frame_buffer_->clear();
         // 确保线程状态正确更新
-        is_streaming_running = false;
-        state_.is_error = true;
-        state_.error_message = e.what();
-        CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_ error:" << state_.error_message;
+        recording_requested_.store(false, std::memory_order_release);
+        setWorkerError_(e.what());
+        markStreamingWorkerStopped_();
+        CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_ error:"
+                        << e.what();
     }
 }
 
 void RealsenseCamera::recording_worker_() {
-    is_recording_running = true;
+    is_recording_running.store(true, std::memory_order_release);
     const int frame_interval = 1000 / fps_;
     bool is_first_key = false;
     try {
         //保证当前采集线程正常运行
-        while (state_.is_recording && is_streaming_running) {
+        while (recording_requested_.load(std::memory_order_acquire)) {
             // 等待缓冲区有数据
             if (stream_frame_buffer_->empty()) {
                 std::this_thread::sleep_for(std::chrono::milliseconds(frame_interval));
@@ -932,11 +992,12 @@ void RealsenseCamera::recording_worker_() {
         av_write_trailer(format_context_);
 
     } catch (const std::exception& e) {
+        setWorkerError_(e.what());
         CMVR_LOG(ERROR) << "Recording thread error: " << e.what();
     }
 
-    is_recording_running = false;
-    state_.is_recording = false;
+    recording_requested_.store(false, std::memory_order_release);
+    markRecordingWorkerStopped_();
 }
 
 void RealsenseCamera::getEncodedFrame(StreamFrameData& frame_data, size_t& index) {
@@ -970,49 +1031,222 @@ bool RealsenseCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t&
 
 bool RealsenseCamera::startStreaming()
 {
-    std::lock_guard lock(ctrl_mtx_);
-    //不在录像也不在流传输，但是采集线程没有退出时。
-    if (!state_.is_streaming && !state_.is_recording) {
-        if (stream_thread_) {
-            if (stream_thread_->joinable()) {
-                stream_thread_->join();
-                is_streaming_running = false;
+    std::lock_guard lifecycle_lock(stream_lifecycle_mtx_);
+
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        clear_error_();
+        if (!state_.is_opened) {
+            state_.is_error = true;
+            state_.error_message = "camera not opened";
+            return false;
+        }
+        if (stream_count_ > 0) {
+            if (!stream_thread_ ||
+                stream_worker_exited_.load(std::memory_order_acquire)) {
+                state_.is_error = true;
+                state_.error_message = "camera stream worker exited";
+                return false;
             }
-            stream_thread_.reset();
+            ++stream_count_;
+            state_.is_streaming = true;
+            stream_requested_.store(true, std::memory_order_release);
+            return true;
+        }
+        if (stream_thread_ &&
+            !stream_worker_exited_.load(std::memory_order_acquire)) {
+            ++stream_count_;
+            state_.is_streaming = true;
+            stream_requested_.store(true, std::memory_order_release);
+            return true;
         }
     }
-    //开启流采集线程
-    if (!stream_thread_) {
-        state_.is_streaming = true;
-        stream_thread_ = make_shared<thread>(&RealsenseCamera::streaming_worker_, this);
 
-        //延时100ms，等待流线程获取图像
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    if (!collectStreamingWorker_(kStreamStopTimeout)) {
+        setWorkerError_("previous camera stream did not stop");
+        return false;
     }
-    stream_count_++;
+
+    std::lock_guard lock(ctrl_mtx_);
+    if (!state_.is_opened) {
+        state_.is_error = true;
+        state_.error_message = "camera not opened";
+        return false;
+    }
+    state_.is_streaming = true;
+    stream_requested_.store(true, std::memory_order_release);
+    stream_worker_exited_.store(false, std::memory_order_release);
+    try {
+        stream_thread_ = make_shared<thread>(&RealsenseCamera::streaming_worker_, this);
+    } catch (const std::exception& error) {
+        stream_requested_.store(false, std::memory_order_release);
+        stream_worker_exited_.store(true, std::memory_order_release);
+        stream_stop_cv_.notify_all();
+        state_.is_streaming = false;
+        state_.is_error = true;
+        state_.error_message =
+            std::string("failed to start camera stream worker: ") +
+            error.what();
+        return false;
+    }
+    stream_count_ = 1;
     return true;
 }
 
 void RealsenseCamera::stopStreaming()
 {
-    std::shared_ptr<std::thread> stream_thread_to_join;
+    std::lock_guard lifecycle_lock(stream_lifecycle_mtx_);
     {
         std::lock_guard lock(ctrl_mtx_);
-        if (stream_count_ > 0) {
-            stream_count_--;
+        if (stream_count_ == 0) {
+            return;
         }
-        if (stream_count_ == 0)
-        {
-        // 当前已经没有正在使用的流了，编码采集线程状态修改
-            state_.is_streaming = false;
-            if (!state_.is_recording) {
-                stream_thread_to_join = stream_thread_;
-                stream_thread_.reset();
-            }
+        --stream_count_;
+        if (stream_count_ != 0) {
+            return;
+        }
+        state_.is_streaming = false;
+        stream_requested_.store(false, std::memory_order_release);
+        if (recording_requested_.load(std::memory_order_acquire)) {
+            return;
         }
     }
-    if (stream_thread_to_join && stream_thread_to_join->joinable()) {
-        stream_thread_to_join->join();
+    if (!collectStreamingWorker_(kStreamStopTimeout)) {
+        setWorkerError_("timed out waiting for camera stream to stop");
+        throw std::runtime_error("timed out waiting for camera stream to stop");
+    }
+}
+
+bool RealsenseCamera::stopOperationalActivity()
+{
+    std::lock_guard lifecycle_lock(stream_lifecycle_mtx_);
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        clear_error_();
+
+        if (stream_count_ != 0 || state_.is_streaming ||
+            state_.is_recording ||
+            recording_requested_.load(std::memory_order_acquire)) {
+            return false;
+        }
+        stream_requested_.store(false, std::memory_order_release);
+        recording_requested_.store(false, std::memory_order_release);
+    }
+
+    if (!collectRecordingWorker_(kStreamStopTimeout)) {
+        setWorkerError_("timed out waiting for camera recording to stop");
+        return false;
+    }
+    if (!collectStreamingWorker_(kStreamStopTimeout)) {
+        setWorkerError_("timed out waiting for camera stream to stop");
+        return false;
+    }
+
+    std::lock_guard lock(ctrl_mtx_);
+    if (state_.is_opened) {
+        try {
+            pipe_.stop();
+        } catch (const std::exception& error) {
+            state_.is_error = true;
+            state_.error_message =
+                std::string("failed to stop operational pipeline: ") +
+                error.what();
+            return false;
+        }
+    }
+    align_.reset();
+    pipe_ = rs2::pipeline();
+    state_.is_opened = false;
+    return !state_.is_streaming && !state_.is_recording;
+}
+
+bool RealsenseCamera::collectStreamingWorker_(
+    const std::chrono::milliseconds timeout)
+{
+    std::shared_ptr<std::thread> worker;
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        worker = stream_thread_;
+    }
+    if (!worker) {
+        return true;
+    }
+
+    {
+        std::unique_lock lock(stream_stop_mtx_);
+        if (!stream_stop_cv_.wait_for(lock, timeout, [this] {
+                return stream_worker_exited_.load(std::memory_order_acquire);
+            })) {
+            return false;
+        }
+    }
+    if (worker->joinable()) {
+        worker->join();
+    }
+    std::lock_guard lock(ctrl_mtx_);
+    if (stream_thread_ == worker) {
+        stream_thread_.reset();
+    }
+    return true;
+}
+
+void RealsenseCamera::markStreamingWorkerStopped_() noexcept
+{
+    is_streaming_running.store(false, std::memory_order_release);
+    stream_worker_exited_.store(true, std::memory_order_release);
+    stream_stop_cv_.notify_all();
+}
+
+bool RealsenseCamera::collectRecordingWorker_(
+    const std::chrono::milliseconds timeout)
+{
+    std::shared_ptr<std::thread> worker;
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        worker = recording_thread_;
+    }
+    if (!worker) {
+        return true;
+    }
+
+    {
+        std::unique_lock lock(stream_stop_mtx_);
+        if (!recording_stop_cv_.wait_for(lock, timeout, [this] {
+                return recording_worker_exited_.load(
+                    std::memory_order_acquire);
+            })) {
+            return false;
+        }
+    }
+    if (worker->joinable()) {
+        worker->join();
+    }
+    std::lock_guard lock(ctrl_mtx_);
+    if (recording_thread_ == worker) {
+        recording_thread_.reset();
+    }
+    return true;
+}
+
+void RealsenseCamera::markRecordingWorkerStopped_() noexcept
+{
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        state_.is_recording = false;
+    }
+    is_recording_running.store(false, std::memory_order_release);
+    recording_worker_exited_.store(true, std::memory_order_release);
+    recording_stop_cv_.notify_all();
+}
+
+void RealsenseCamera::setWorkerError_(const std::string& message) noexcept
+{
+    try {
+        std::lock_guard lock(ctrl_mtx_);
+        state_.is_error = true;
+        state_.error_message = message;
+    } catch (...) {
+        // Error reporting from a worker must never terminate the process.
     }
 }
 

cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h

@@ -6,6 +6,7 @@
 #define CMVR_ES_UVC_CAMERA_H
 
 #include <atomic>
+#include <chrono>
 #include <condition_variable>
 
 #include "common/base/ring_buffer.h"
@@ -45,6 +46,7 @@ namespace cmvr::device {
 
         bool startStreaming() override;
         void stopStreaming() override;
+        bool stopOperationalActivity() override;
     private:
         void streaming_worker_();
         void recording_worker_();
@@ -58,6 +60,11 @@ namespace cmvr::device {
                                      int64_t& capture_utc_ns);
         bool convert_capture_frame_to_bgr_(const AVFrame* frame, cv::Mat& bgr_frame);
         static int interrupt_capture_(void* opaque);
+        bool collectStreamingWorker_(std::chrono::milliseconds timeout);
+        bool collectRecordingWorker_(std::chrono::milliseconds timeout);
+        void markStreamingWorkerStopped_() noexcept;
+        void markRecordingWorkerStopped_() noexcept;
+        void setWorkerError_(const std::string& message) noexcept;
 
         int fps_;
         int width_;
@@ -75,6 +82,10 @@ namespace cmvr::device {
         std::string current_video_path_;
 
         std::mutex ctrl_mtx_{};
+        std::mutex stream_lifecycle_mtx_{};
+        std::mutex stream_stop_mtx_{};
+        std::condition_variable stream_stop_cv_{};
+        std::condition_variable recording_stop_cv_{};
         std::shared_ptr<std::thread> stream_thread_;
         std::shared_ptr<std::thread> recording_thread_;
         std::shared_ptr<std::thread> capture_thread_;
@@ -111,8 +122,12 @@ namespace cmvr::device {
         size_t recordingIndex_ = 0;
         size_t getImageIndex_ = 0;
 
-        bool is_streaming_running = false;
-        bool is_recording_running = false;
+        std::atomic<bool> stream_requested_{false};
+        std::atomic<bool> recording_requested_{false};
+        std::atomic<bool> stream_worker_exited_{true};
+        std::atomic<bool> recording_worker_exited_{true};
+        std::atomic<bool> is_streaming_running{false};
+        std::atomic<bool> is_recording_running{false};
         int stream_count_ = 0;
 
         config::UVCCameraConfig camera_;

cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp

@@ -20,6 +20,7 @@ using namespace cmvr::device;
 #define USE_LIST_IMAGE 1
 
 namespace {
+constexpr auto kStreamStopTimeout = std::chrono::seconds(2);
 
 std::string ffmpeg_error_string(const int error_code)
 {
@@ -114,7 +115,6 @@ bool is_complete_mjpeg_packet(const AVPacket* packet)
 }
 
 } // namespace
-
 UVCCamera::UVCCamera(const config::UVCCameraConfig& camera):camera_(camera)
 {
     id_ = camera_.id();
@@ -263,9 +263,18 @@ bool UVCCamera::start() {
 
 bool UVCCamera::stop() {
     //先停止录制再关闭摄像头
-    if (state_.is_recording || recording_thread_ || packet_ || format_context_ || stream_) {
+    bool is_recording = false;
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        is_recording = state_.is_recording || recording_thread_ || packet_ ||
+            format_context_ || stream_;
+    }
+    if (is_recording) {
         stopRecording();
     }
+    std::lock_guard lifecycle_lock(stream_lifecycle_mtx_);
+    bool collect_stream = false;
+    {
     std::lock_guard lock(ctrl_mtx_);
     clear_error_();
     try {
@@ -275,18 +284,10 @@ bool UVCCamera::stop() {
         }
         if (mode_ == VIDEO_MODE){
             state_.is_streaming = false;
-            if (stream_thread_) {
-                if (stream_thread_->joinable()) {
-                    stream_thread_->join();
-                }
-                stream_thread_.reset();
-            }
-            is_streaming_running = false;
+            stream_count_ = 0;
+            stream_requested_.store(false, std::memory_order_release);
+            collect_stream = static_cast<bool>(stream_thread_);
         }
-
-        close_capture_();
-        state_.is_opened = false;
-        return true;
     }
     catch (exception &e) {
         CMVR_LOG(ERROR) << "[UVCCamera] (stop): " << e.what();
@@ -294,6 +295,20 @@ bool UVCCamera::stop() {
         state_.error_message = e.what();
         return false;
     }
+    }
+
+    if (collect_stream && !collectStreamingWorker_(kStreamStopTimeout)) {
+        std::lock_guard lock(ctrl_mtx_);
+        state_.is_error = true;
+        state_.error_message = "timed out waiting for camera stream to stop";
+        return false;
+    }
+    close_capture_();
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        state_.is_opened = false;
+    }
+    return true;
 }
 
 void UVCCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics)
@@ -768,6 +783,7 @@ bool UVCCamera::convert_capture_frame_to_bgr_(const AVFrame* frame, cv::Mat& bgr
 }
 
 void UVCCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
+    std::lock_guard lock(ctrl_mtx_);
     state_.is_error = true;
     state_.error_message = "getDepthImage unsupported usage";
     CMVR_LOG(ERROR) << "[UVCCamera] (getDepthImage): " << state_.error_message;
@@ -775,6 +791,7 @@ void UVCCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
 }
 
 void UVCCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) {
+    std::lock_guard lock(ctrl_mtx_);
     state_.is_error = true;
     state_.error_message = "getRGBDImages unsupported usage";
     CMVR_LOG(ERROR) << "[UVCCamera] (getRGBDImages): " << state_.error_message;
@@ -799,27 +816,56 @@ void UVCCamera::cleanup_recording_resources_() {
 }
 
 void UVCCamera::startRecording(const std::string &video_path) {
-    std::lock_guard lock(ctrl_mtx_);
-    clear_error_();
-    if (mode_ != VIDEO_MODE) {
-        state_.is_error = true;
-        state_.error_message = "startRecording only supports VIDEO_MODE";
-        CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message;
+    std::lock_guard lifecycle_lock(stream_lifecycle_mtx_);
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        clear_error_();
+        if (mode_ != VIDEO_MODE) {
+            state_.is_error = true;
+            state_.error_message = "startRecording only supports VIDEO_MODE";
+            CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message;
+            return;
+        }
+        if (!state_.is_opened) {
+            state_.is_error = true;
+            state_.error_message = "camera not opened";
+            CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message;
+            return;
+        }
+        if (state_.is_recording ||
+            recording_requested_.load(std::memory_order_acquire)) {
+            state_.is_error = true;
+            state_.error_message = "already recording";
+            CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message;
+            return;
+        }
+    }
+
+    if (!collectRecordingWorker_(kStreamStopTimeout)) {
+        setWorkerError_("previous camera recording did not stop");
         return;
     }
-    if (!state_.is_opened) {
-        state_.is_error = true;
-        state_.error_message = "camera not opened";
-        CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message;
-        return;
+    bool collect_stale_stream = false;
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        collect_stale_stream = stream_thread_ &&
+            stream_worker_exited_.load(std::memory_order_acquire);
     }
-    if (state_.is_recording) {
-        state_.is_error = true;
-        state_.error_message = "already recording";
-        CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message;
+    if (collect_stale_stream &&
+        !collectStreamingWorker_(kStreamStopTimeout)) {
+        setWorkerError_("previous camera stream did not stop");
         return;
     }
 
+    std::unique_lock lock(ctrl_mtx_);
+    if (!state_.is_opened || state_.is_recording) {
+        state_.is_error = true;
+        state_.error_message = state_.is_recording
+            ? "already recording" : "camera not opened";
+        return;
+    }
+
+    bool created_stream_worker = false;
     try {
         current_video_path_ = video_path;
         std::string temp_path = current_video_path_ + ".temp"; // 临时文件
@@ -912,81 +958,102 @@ void UVCCamera::startRecording(const std::string &video_path) {
             cleanup_recording_resources_();
             return;
         }
-        //不在录像也不在流传输，但是采集线程没有退出时。
-        if (!state_.is_streaming && !state_.is_recording) {
-            if (stream_thread_) {
-                if (stream_thread_->joinable()) {
-                    stream_thread_->join();
-                    is_streaming_running = false;
-                }
-                stream_thread_.reset();
-            }
-        }
         // 开启录像
         state_.is_recording = true;
+        recording_requested_.store(true, std::memory_order_release);
         //开启流采集线程
         if (!stream_thread_) {
+            stream_worker_exited_.store(false, std::memory_order_release);
             stream_thread_ = make_shared<thread>(&UVCCamera::streaming_worker_, this);
-            //延时100ms，等待流线程获取图像
-            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            created_stream_worker = true;
         }
         // 启动录像线程
-        if (recording_thread_) {
-            if (recording_thread_->joinable()) {
-                recording_thread_->join();
-                is_recording_running = false;
-            }
-            recording_thread_.reset();
-        }
         frame_count_ = 0;
+        recording_worker_exited_.store(false, std::memory_order_release);
         recording_thread_ = make_shared<thread>(&UVCCamera::recording_worker_, this);
 
     } catch (const std::exception& e) {
+        recording_requested_.store(false, std::memory_order_release);
+        recording_worker_exited_.store(true, std::memory_order_release);
+        recording_stop_cv_.notify_all();
+        if (!stream_thread_) {
+            stream_worker_exited_.store(true, std::memory_order_release);
+            stream_stop_cv_.notify_all();
+        }
         cleanup_recording_resources_();
         state_.is_recording = false;
         state_.is_error = true;
         state_.error_message = "[UVCCamera] (startRecording): " + std::string(e.what());
         CMVR_LOG(ERROR) << state_.error_message;
+        const bool collect_created_stream = created_stream_worker &&
+            !stream_requested_.load(std::memory_order_acquire);
+        lock.unlock();
+        if (collect_created_stream &&
+            !collectStreamingWorker_(kStreamStopTimeout)) {
+            setWorkerError_(
+                "failed to collect camera stream after recording start failure");
+        }
     }
 }
 
 void UVCCamera::stopRecording() {
-    std::lock_guard lock(ctrl_mtx_);
-    clear_error_();
-    if (mode_ != VIDEO_MODE) {
-        state_.is_error = true;
-        state_.error_message = "stopRecording only supports VIDEO_MODE";
-        CMVR_LOG(ERROR) << "[UVCCamera] (stopRecording): " << state_.error_message;
-        return;
-    }
-    const bool has_recording_resources =
-        recording_thread_ || packet_ || format_context_ || stream_;
-    if (!state_.is_recording && !has_recording_resources) {
-        CMVR_LOG(WARNING) << "[UVCCamera] (stopRecording): not recording";
-        return;
+    std::lock_guard lifecycle_lock(stream_lifecycle_mtx_);
+    bool has_recording_worker = false;
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        clear_error_();
+        if (mode_ != VIDEO_MODE) {
+            state_.is_error = true;
+            state_.error_message = "stopRecording only supports VIDEO_MODE";
+            CMVR_LOG(ERROR) << "[UVCCamera] (stopRecording): " << state_.error_message;
+            return;
+        }
+        has_recording_worker = static_cast<bool>(recording_thread_);
+        const bool has_recording_resources = has_recording_worker || packet_ ||
+            format_context_ || stream_;
+        if (!state_.is_recording && !has_recording_resources) {
+            CMVR_LOG(WARNING) << "[UVCCamera] (stopRecording): not recording";
+            return;
+        }
+        recording_requested_.store(false, std::memory_order_release);
     }
 
-    // 1. 停止录像线程
-    state_.is_recording = false;
-    if (recording_thread_) {
-        if (recording_thread_->joinable()) {
-            recording_thread_->join();
-        }
-        recording_thread_.reset();
+    if (has_recording_worker &&
+        !collectRecordingWorker_(kStreamStopTimeout)) {
+        setWorkerError_("timed out waiting for camera recording to stop");
+        throw std::runtime_error(
+            "timed out waiting for camera recording to stop");
     }
 
+    std::unique_lock lock(ctrl_mtx_);
+    state_.is_recording = false;
+
     // 2. 清理FFmpeg资源
     cleanup_recording_resources_();
 
-    // 3. 重命名临时文件为目标文件
-    std::string temp_path = current_video_path_ + ".temp";
-    if (rename(temp_path.c_str(), current_video_path_.c_str()) != 0) {
+    // 3. 重命名临时文件为目标文件。即使重命名失败，也必须继续
+    // 回收仅由录像使用的采集线程。
+    const std::string completed_video_path = current_video_path_;
+    const std::string temp_path = completed_video_path + ".temp";
+    const bool rename_failed = !completed_video_path.empty() &&
+        rename(temp_path.c_str(), completed_video_path.c_str()) != 0;
+    if (rename_failed) {
         state_.is_error = true;
-        state_.error_message = "failed to rename temp file: " + temp_path + " -> " + current_video_path_;
+        state_.error_message = "failed to rename temp file: " + temp_path +
+            " -> " + completed_video_path;
         CMVR_LOG(ERROR) << "[UVCCamera] (stopRecording): " << state_.error_message;
-        return;
     }
     current_video_path_.clear();
+
+    const bool collect_stream = stream_count_ == 0 &&
+        static_cast<bool>(stream_thread_);
+    lock.unlock();
+    if (collect_stream &&
+        !collectStreamingWorker_(kStreamStopTimeout)) {
+        setWorkerError_("timed out waiting for camera stream to stop");
+        throw std::runtime_error(
+            "timed out waiting for camera stream to stop");
+    }
 }
 
 void UVCCamera::pauseRecording() {
@@ -1000,10 +1067,9 @@ void UVCCamera::resumeRecording() {
 void UVCCamera::streaming_worker_() {
     AVFrame* frame = av_frame_alloc();
     if (!frame) {
-        is_streaming_running = false;
-        state_.is_error = true;
-        state_.error_message = "failed to allocate streaming frame";
-        CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_: " << state_.error_message;
+        setWorkerError_("failed to allocate streaming frame");
+        markStreamingWorkerStopped_();
+        CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_: failed to allocate streaming frame";
         return;
     }
     try {
@@ -1011,7 +1077,7 @@ void UVCCamera::streaming_worker_() {
         const int frame_interval = 1000 / fps_;
 
         bool success = false;
-        is_streaming_running = true;
+        is_streaming_running.store(true, std::memory_order_release);
         uint64_t capture_sequence = 0;
         int64_t frame_capture_monotonic_ns = 0;
         int64_t frame_capture_utc_ns = 0;
@@ -1029,9 +1095,9 @@ void UVCCamera::streaming_worker_() {
         int64_t encode_us = 0;
         int64_t processing_us = 0;
         auto timing_window_start = std::chrono::steady_clock::now();
-
         // 处于流传输或者录像状态时就不退出线程
-        while (state_.is_streaming || state_.is_recording) {
+        while (stream_requested_.load(std::memory_order_acquire) ||
+               recording_requested_.load(std::memory_order_acquire)) {
             // 记录当前帧处理开始时间
             const auto frame_start_time = std::chrono::steady_clock::now();
             if (!wait_for_capture_frame_(frame,
@@ -1043,11 +1109,12 @@ void UVCCamera::streaming_worker_() {
                     std::lock_guard capture_lock(capture_frame_mutex_);
                     capture_error = capture_error_;
                 }
-                state_.is_error = true;
-                state_.error_message = capture_error.empty()
+                const std::string error_message = capture_error.empty()
                     ? "timed out waiting for camera frame"
                     : capture_error;
-                CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_: " << state_.error_message;
+                setWorkerError_(error_message);
+                recording_requested_.store(false, std::memory_order_release);
+                CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_: " << error_message;
                 break;
             }
             const auto capture_end_time = std::chrono::steady_clock::now();
@@ -1135,7 +1202,7 @@ void UVCCamera::streaming_worker_() {
 
         }
 
-        is_streaming_running = false;
+        markStreamingWorkerStopped_();
         // 线程结束时清空队列
         stream_frame_buffer_->clear();
         recordingIndex_ = 0;
@@ -1146,23 +1213,23 @@ void UVCCamera::streaming_worker_() {
         // 线程结束时清空队列
         stream_frame_buffer_->clear();
         // 确保线程状态正确更新
-        is_streaming_running = false;
-        state_.is_error = true;
-        state_.error_message = e.what();
-        CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_ error:" << state_.error_message;
+        recording_requested_.store(false, std::memory_order_release);
+        setWorkerError_(e.what());
+        markStreamingWorkerStopped_();
+        CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_ error:" << e.what();
     }
     av_frame_free(&frame);
 }
 
 void UVCCamera::recording_worker_() {
-    is_recording_running = true;
+    is_recording_running.store(true, std::memory_order_release);
     const int frame_interval = 1000 / fps_;
     bool is_first_key = false;
     int64_t first_capture_monotonic_ns = 0;
     int64_t last_packet_pts = AV_NOPTS_VALUE;
     try {
         //保证当前采集线程正常运行
-        while (state_.is_recording && is_streaming_running) {
+        while (recording_requested_.load(std::memory_order_acquire)) {
             // 等待缓冲区有数据
             if (stream_frame_buffer_->empty()) {
                 std::this_thread::sleep_for(std::chrono::milliseconds(frame_interval));
@@ -1235,11 +1302,12 @@ void UVCCamera::recording_worker_() {
         av_write_trailer(format_context_);
 
     } catch (const std::exception& e) {
+        setWorkerError_(e.what());
         CMVR_LOG(ERROR) << "录像线程错误: " << e.what();
     }
 
-    is_recording_running = false;
-    state_.is_recording = false;
+    recording_requested_.store(false, std::memory_order_release);
+    markRecordingWorkerStopped_();
 }
 
 void UVCCamera::getEncodedFrame(StreamFrameData& frame_data, size_t& index) {
@@ -1273,50 +1341,213 @@ bool UVCCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_
 
 bool UVCCamera::startStreaming()
 {
-    std::lock_guard lock(ctrl_mtx_);
-    //不在录像也不在流传输，但是采集线程没有退出时。
-    if (!state_.is_streaming && !state_.is_recording) {
-        if (stream_thread_) {
-            if (stream_thread_->joinable()) {
-                stream_thread_->join();
-                is_streaming_running = false;
-            }
-            stream_thread_.reset();
-        }
-    }
+    std::lock_guard lifecycle_lock(stream_lifecycle_mtx_);
 
-    // A recording session may already own the encoder worker. The streaming
-    // state still has to reflect the new client lease so stopping recording
-    // does not terminate the worker while clients are consuming frames.
-    state_.is_streaming = true;
-    //开启流采集线程
-    if (!stream_thread_) {
-        try {
-            stream_thread_ = make_shared<thread>(&UVCCamera::streaming_worker_, this);
-        } catch (const std::exception& e) {
-            state_.is_streaming = stream_count_ > 0;
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        clear_error_();
+        if (!state_.is_opened) {
             state_.is_error = true;
-            state_.error_message = "failed to start streaming worker: " + std::string(e.what());
-            CMVR_LOG(ERROR) << "[UVCCamera] (startStreaming): " << state_.error_message;
+            state_.error_message = "camera not opened";
             return false;
         }
 
-        //延时100ms，等待流线程获取图像
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        // A running worker is shared by all streaming leases and recording.
+        // Adding another lease must not wait for that worker to exit.
+        if (stream_count_ > 0) {
+            if (!stream_thread_ ||
+                stream_worker_exited_.load(std::memory_order_acquire)) {
+                state_.is_error = true;
+                state_.error_message = "camera stream worker exited";
+                return false;
+            }
+            ++stream_count_;
+            state_.is_streaming = true;
+            stream_requested_.store(true, std::memory_order_release);
+            return true;
+        }
+        if (stream_thread_ &&
+            !stream_worker_exited_.load(std::memory_order_acquire)) {
+            ++stream_count_;
+            state_.is_streaming = true;
+            stream_requested_.store(true, std::memory_order_release);
+            return true;
+        }
     }
-    stream_count_++;
+    if (!collectStreamingWorker_(kStreamStopTimeout)) {
+        setWorkerError_("previous camera stream did not stop");
+        return false;
+    }
+
+    std::lock_guard lock(ctrl_mtx_);
+    if (!state_.is_opened) {
+        state_.is_error = true;
+        state_.error_message = "camera not opened";
+        return false;
+    }
+    state_.is_streaming = true;
+    stream_requested_.store(true, std::memory_order_release);
+    stream_worker_exited_.store(false, std::memory_order_release);
+    try {
+        stream_thread_ = make_shared<thread>(&UVCCamera::streaming_worker_, this);
+    } catch (const std::exception& error) {
+        stream_requested_.store(false, std::memory_order_release);
+        stream_worker_exited_.store(true, std::memory_order_release);
+        stream_stop_cv_.notify_all();
+        state_.is_streaming = false;
+        state_.is_error = true;
+        state_.error_message =
+            std::string("failed to start camera stream worker: ") +
+            error.what();
+        return false;
+    }
+    stream_count_ = 1;
     return true;
 }
 
 void UVCCamera::stopStreaming()
 {
-    std::lock_guard lock(ctrl_mtx_);
-    if (stream_count_ > 0) {
-        --stream_count_;
-    }
-    if (stream_count_ == 0)
+    std::lock_guard lifecycle_lock(stream_lifecycle_mtx_);
     {
-        // 当前已经没有正在使用的流了，编码采集线程状态修改
+        std::lock_guard lock(ctrl_mtx_);
+        if (stream_count_ == 0) {
+            return;
+        }
+        --stream_count_;
+        if (stream_count_ != 0) {
+            return;
+        }
         state_.is_streaming = false;
+        stream_requested_.store(false, std::memory_order_release);
+        if (recording_requested_.load(std::memory_order_acquire)) {
+            return;
+        }
+    }
+    if (!collectStreamingWorker_(kStreamStopTimeout)) {
+        setWorkerError_("timed out waiting for camera stream to stop");
+        throw std::runtime_error("timed out waiting for camera stream to stop");
+    }
+}
+
+bool UVCCamera::stopOperationalActivity()
+{
+    std::lock_guard lifecycle_lock(stream_lifecycle_mtx_);
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        clear_error_();
+
+        if (stream_count_ != 0 || state_.is_streaming ||
+            state_.is_recording ||
+            recording_requested_.load(std::memory_order_acquire)) {
+            return false;
+        }
+        stream_requested_.store(false, std::memory_order_release);
+        recording_requested_.store(false, std::memory_order_release);
+    }
+
+    if (!collectRecordingWorker_(kStreamStopTimeout)) {
+        setWorkerError_("timed out waiting for camera recording to stop");
+        return false;
+    }
+    if (!collectStreamingWorker_(kStreamStopTimeout)) {
+        setWorkerError_("timed out waiting for camera stream to stop");
+        return false;
+    }
+
+    close_capture_();
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        state_.is_opened = false;
+    }
+    return !capture_running_.load(std::memory_order_acquire);
+}
+
+bool UVCCamera::collectStreamingWorker_(
+    const std::chrono::milliseconds timeout)
+{
+    std::shared_ptr<std::thread> worker;
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        worker = stream_thread_;
+    }
+    if (!worker) {
+        return true;
+    }
+
+    {
+        std::unique_lock lock(stream_stop_mtx_);
+        if (!stream_stop_cv_.wait_for(lock, timeout, [this] {
+                return stream_worker_exited_.load(std::memory_order_acquire);
+            })) {
+            return false;
+        }
+    }
+    if (worker->joinable()) {
+        worker->join();
+    }
+    std::lock_guard lock(ctrl_mtx_);
+    if (stream_thread_ == worker) {
+        stream_thread_.reset();
+    }
+    return true;
+}
+
+void UVCCamera::markStreamingWorkerStopped_() noexcept
+{
+    is_streaming_running.store(false, std::memory_order_release);
+    stream_worker_exited_.store(true, std::memory_order_release);
+    stream_stop_cv_.notify_all();
+}
+
+bool UVCCamera::collectRecordingWorker_(
+    const std::chrono::milliseconds timeout)
+{
+    std::shared_ptr<std::thread> worker;
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        worker = recording_thread_;
+    }
+    if (!worker) {
+        return true;
+    }
+
+    {
+        std::unique_lock lock(stream_stop_mtx_);
+        if (!recording_stop_cv_.wait_for(lock, timeout, [this] {
+                return recording_worker_exited_.load(
+                    std::memory_order_acquire);
+            })) {
+            return false;
+        }
+    }
+    if (worker->joinable()) {
+        worker->join();
+    }
+    std::lock_guard lock(ctrl_mtx_);
+    if (recording_thread_ == worker) {
+        recording_thread_.reset();
+    }
+    return true;
+}
+
+void UVCCamera::markRecordingWorkerStopped_() noexcept
+{
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        state_.is_recording = false;
+    }
+    is_recording_running.store(false, std::memory_order_release);
+    recording_worker_exited_.store(true, std::memory_order_release);
+    recording_stop_cv_.notify_all();
+}
+
+void UVCCamera::setWorkerError_(const std::string& message) noexcept
+{
+    try {
+        std::lock_guard lock(ctrl_mtx_);
+        state_.is_error = true;
+        state_.error_message = message;
+    } catch (...) {
+        // Error reporting from a worker must never terminate the process.
     }
 }

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/CMakeLists.txt

@@ -7,3 +7,30 @@ add_library(cmvr_es::device::rh56dftp_dexhand ALIAS rh56dftp_dexhand)
 target_link_libraries(rh56dftp_dexhand PRIVATE cmvr_es::hardware cmvr_es::proto -lmodbus)
 
 install(TARGETS rh56dftp_dexhand LIBRARY DESTINATION lib)
+
+if(BUILD_TESTING)
+    add_executable(rh56dftp_dexhand_stop_all_test
+            tests/rh56dftp_dexhand_stop_all_test.cpp
+    )
+    target_link_libraries(rh56dftp_dexhand_stop_all_test PRIVATE
+            cmvr_es::device::rh56dftp_dexhand
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME rh56dftp_dexhand_stop_all_test
+            COMMAND rh56dftp_dexhand_stop_all_test
+    )
+    set(_rh56_stop_all_test_environment
+            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
+    )
+    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+        list(APPEND _rh56_stop_all_test_environment
+                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+    endif()
+    set_tests_properties(rh56dftp_dexhand_stop_all_test PROPERTIES
+            TIMEOUT 10
+            ENVIRONMENT "${_rh56_stop_all_test_environment}"
+    )
+endif()

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,8 +411,14 @@ bool RH56DFTPDexhand::stop() {
         tactile_thread_.join();
     }
 
-    if (controller_) {
-        controller_->close();
+    {
+        // 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) {
@@ -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/dexhand/rh56dftp_dexhand/tests/rh56dftp_dexhand_stop_all_test.cpp

@@ -0,0 +1,348 @@
+#include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h"
+
+#include <array>
+#include <chrono>
+#include <condition_variable>
+#include <deque>
+#include <future>
+#include <memory>
+#include <mutex>
+#include <thread>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+namespace cmvr::device {
+namespace {
+
+using namespace std::chrono_literals;
+
+class FakeModbusController final : public ModbusController {
+public:
+    bool open(const std::string&, int) override
+    {
+        std::lock_guard lock(mutex_);
+        open_ = true;
+        return true;
+    }
+
+    void close() override
+    {
+        std::lock_guard lock(mutex_);
+        open_ = false;
+    }
+
+    bool isOpen() const override
+    {
+        std::lock_guard lock(mutex_);
+        return open_;
+    }
+
+    bool writeRegisters(
+        int,
+        const uint16_t* values,
+        const int count) override
+    {
+        std::unique_lock lock(mutex_);
+        ++write_calls_;
+        write_started_ = true;
+        condition_.notify_all();
+        condition_.wait(lock, [this] { return !block_write_; });
+        if (!write_succeeds_) {
+            return false;
+        }
+        actual_angles_.assign(values, values + count);
+        return true;
+    }
+
+    bool readRegisterBlock(
+        const int address,
+        const int count,
+        std::vector<uint16_t>& values) override
+    {
+        std::unique_lock lock(mutex_);
+        ++read_calls_;
+        read_started_ = true;
+        condition_.notify_all();
+        condition_.wait(lock, [this] { return !block_read_; });
+        const std::vector<uint16_t>* source = &actual_angles_;
+        std::vector<uint16_t> sampled_angles;
+        if (address == 1546 && !actual_angle_samples_.empty()) {
+            const auto sample = actual_angle_samples_.front();
+            actual_angle_samples_.pop_front();
+            sampled_angles.assign(sample.begin(), sample.end());
+            source = &sampled_angles;
+        }
+        values.assign(static_cast<std::size_t>(count), 0U);
+        for (std::size_t index = 0;
+             index < values.size() && index < source->size();
+             ++index) {
+            values[index] = (*source)[index];
+        }
+        return read_succeeds_;
+    }
+
+    void setActualAngles(const std::array<uint16_t, 6>& values)
+    {
+        std::lock_guard lock(mutex_);
+        actual_angles_.assign(values.begin(), values.end());
+        actual_angle_samples_.clear();
+    }
+
+    void setActualAngleSamples(
+        std::deque<std::array<uint16_t, 6>> samples)
+    {
+        std::lock_guard lock(mutex_);
+        actual_angle_samples_ = std::move(samples);
+    }
+
+    void setWriteSucceeds(const bool succeeds)
+    {
+        std::lock_guard lock(mutex_);
+        write_succeeds_ = succeeds;
+    }
+
+    void blockNextRead()
+    {
+        std::lock_guard lock(mutex_);
+        block_read_ = true;
+        read_started_ = false;
+    }
+
+    void releaseRead()
+    {
+        {
+            std::lock_guard lock(mutex_);
+            block_read_ = false;
+        }
+        condition_.notify_all();
+    }
+
+    bool waitForRead(const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(mutex_);
+        return condition_.wait_for(
+            lock, timeout, [this] { return read_started_; });
+    }
+
+    bool waitForReadCalls(
+        const int expected,
+        const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(mutex_);
+        return condition_.wait_for(
+            lock, timeout, [this, expected] { return read_calls_ >= expected; });
+    }
+
+    void blockNextWrite()
+    {
+        std::lock_guard lock(mutex_);
+        block_write_ = true;
+        write_started_ = false;
+    }
+
+    void releaseWrite()
+    {
+        {
+            std::lock_guard lock(mutex_);
+            block_write_ = false;
+        }
+        condition_.notify_all();
+    }
+
+    bool waitForWrite(const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(mutex_);
+        return condition_.wait_for(
+            lock, timeout, [this] { return write_started_; });
+    }
+
+    int readCalls() const
+    {
+        std::lock_guard lock(mutex_);
+        return read_calls_;
+    }
+
+    int writeCalls() const
+    {
+        std::lock_guard lock(mutex_);
+        return write_calls_;
+    }
+
+private:
+    mutable std::mutex mutex_;
+    std::condition_variable condition_;
+    std::vector<uint16_t> actual_angles_{6U, 0U};
+    std::deque<std::array<uint16_t, 6>> actual_angle_samples_;
+    bool open_{false};
+    bool block_read_{false};
+    bool block_write_{false};
+    bool read_started_{false};
+    bool write_started_{false};
+    bool read_succeeds_{true};
+    bool write_succeeds_{true};
+    int read_calls_{0};
+    int write_calls_{0};
+};
+
+struct TestHand {
+    TestHand()
+    {
+        config.set_id("rh56-test");
+        config.set_ip("fake-modbus");
+        auto controller = std::make_unique<FakeModbusController>();
+        fake = controller.get();
+        hand = std::make_unique<RH56DFTPDexhand>(
+            config, std::move(controller));
+        EXPECT_TRUE(hand->init());
+    }
+
+    ~TestHand()
+    {
+        if (hand) {
+            hand->stop();
+        }
+    }
+
+    config::RH56DFTPDexHandConfig config;
+    FakeModbusController* fake{nullptr};
+    std::unique_ptr<RH56DFTPDexhand> hand;
+};
+
+TEST(RH56DFTPDexhandStopAllTest, IdleTactileDeviceStopsWithoutClosingLifecycle)
+{
+    TestHand fixture;
+
+    EXPECT_TRUE(fixture.hand->stopOperationalActivity());
+    EXPECT_EQ(fixture.hand->state(), AbstractDexHand::Status::INITIALIZED);
+    EXPECT_TRUE(fixture.fake->isOpen());
+
+    const int reads_before = fixture.fake->readCalls();
+    (void)fixture.hand->getSensorData(
+        AbstractDexHand::FingerType::INDEX,
+        AbstractDexHand::TactileRegion::TIP);
+    EXPECT_EQ(fixture.fake->readCalls(), reads_before);
+}
+
+TEST(RH56DFTPDexhandStopAllTest, UnreachedAngleTargetFailsClosedThenRecovers)
+{
+    TestHand fixture;
+    const std::vector<int> target{100, 200, 300, 400, 500, 600};
+
+    fixture.hand->setAngles(target);
+    fixture.fake->setActualAngles({0, 0, 0, 0, 0, 0});
+    EXPECT_FALSE(fixture.hand->stopOperationalActivity());
+
+    fixture.fake->setActualAngles({100, 200, 300, 400, 500, 600});
+    EXPECT_TRUE(fixture.hand->stopOperationalActivity());
+    EXPECT_TRUE(fixture.fake->isOpen());
+}
+
+TEST(RH56DFTPDexhandStopAllTest, MovingSampleAtTargetDoesNotConfirmStop)
+{
+    TestHand fixture;
+    const std::vector<int> target{100, 200, 300, 400, 500, 600};
+
+    fixture.hand->setAngles(target);
+    fixture.fake->setActualAngleSamples({
+        {100, 200, 300, 400, 500, 600},
+        {103, 203, 303, 403, 503, 603},
+        {106, 206, 306, 406, 506, 606},
+    });
+    EXPECT_FALSE(fixture.hand->stopOperationalActivity());
+
+    fixture.fake->setActualAngles({100, 200, 300, 400, 500, 600});
+    EXPECT_TRUE(fixture.hand->stopOperationalActivity());
+}
+
+TEST(RH56DFTPDexhandStopAllTest, FailedAngleWriteRemainsUnconfirmed)
+{
+    TestHand fixture;
+    fixture.fake->setActualAngles({0, 0, 0, 0, 0, 0});
+    fixture.fake->setWriteSucceeds(false);
+
+    fixture.hand->setAngles({100, 200, 300, 400, 500, 600});
+
+    EXPECT_FALSE(fixture.hand->stopOperationalActivity());
+}
+
+TEST(RH56DFTPDexhandStopAllTest, StopWaitsForAdmittedAngleWrite)
+{
+    TestHand fixture;
+    fixture.fake->blockNextWrite();
+    const std::vector<int> target{100, 200, 300, 400, 500, 600};
+    auto command = std::async(std::launch::async, [&] {
+        fixture.hand->setAngles(target);
+    });
+    ASSERT_TRUE(fixture.fake->waitForWrite(500ms));
+
+    auto stop = std::async(std::launch::async, [&] {
+        return fixture.hand->stopOperationalActivity();
+    });
+    EXPECT_EQ(stop.wait_for(20ms), std::future_status::timeout);
+
+    fixture.fake->releaseWrite();
+    EXPECT_EQ(command.wait_for(500ms), std::future_status::ready);
+    command.get();
+    ASSERT_EQ(stop.wait_for(500ms), std::future_status::ready);
+    EXPECT_TRUE(stop.get());
+
+    const int writes_before = fixture.fake->writeCalls();
+    fixture.hand->setAngles(target);
+    EXPECT_EQ(fixture.fake->writeCalls(), writes_before);
+    EXPECT_TRUE(fixture.hand->resumeOperationalActivity());
+    fixture.hand->setAngles(target);
+    EXPECT_EQ(fixture.fake->writeCalls(), writes_before + 1);
+}
+
+TEST(RH56DFTPDexhandStopAllTest, StopWaitsForAdmittedTactileRead)
+{
+    TestHand fixture;
+    fixture.fake->blockNextRead();
+    auto read = std::async(std::launch::async, [&] {
+        return fixture.hand->getSensorData(
+            AbstractDexHand::FingerType::INDEX,
+            AbstractDexHand::TactileRegion::TIP);
+    });
+    ASSERT_TRUE(fixture.fake->waitForRead(500ms));
+
+    auto stop = std::async(std::launch::async, [&] {
+        return fixture.hand->stopOperationalActivity();
+    });
+    EXPECT_EQ(stop.wait_for(20ms), std::future_status::timeout);
+
+    fixture.fake->releaseRead();
+    EXPECT_EQ(read.wait_for(500ms), std::future_status::ready);
+    (void)read.get();
+    ASSERT_EQ(stop.wait_for(500ms), std::future_status::ready);
+    EXPECT_TRUE(stop.get());
+}
+
+TEST(RH56DFTPDexhandStopAllTest, StopDrainsAndPausesBackgroundTactilePolling)
+{
+    TestHand fixture;
+    ASSERT_TRUE(fixture.hand->start());
+
+    const int reads_before_block = fixture.fake->readCalls();
+    fixture.fake->blockNextRead();
+    ASSERT_TRUE(fixture.fake->waitForReadCalls(reads_before_block + 1, 500ms));
+
+    auto stop = std::async(std::launch::async, [&] {
+        return fixture.hand->stopOperationalActivity();
+    });
+    EXPECT_EQ(stop.wait_for(20ms), std::future_status::timeout);
+
+    fixture.fake->releaseRead();
+    ASSERT_EQ(stop.wait_for(500ms), std::future_status::ready);
+    EXPECT_TRUE(stop.get());
+
+    const int reads_after_stop = fixture.fake->readCalls();
+    std::this_thread::sleep_for(30ms);
+    EXPECT_EQ(fixture.fake->readCalls(), reads_after_stop);
+
+    ASSERT_TRUE(fixture.hand->resumeOperationalActivity());
+    EXPECT_TRUE(fixture.fake->waitForReadCalls(reads_after_stop + 1, 500ms));
+}
+
+} // namespace
+} // namespace cmvr::device

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/motor/manager/include/motor_manager.h

@@ -54,6 +54,15 @@ public:
         std::vector<double>& positions,
         std::vector<double>& velocities) const;
 
+    // A MotorRobotArm owns its joints for the lifetime of the arm instance.
+    // Direct per-motor control must not compete with that group controller.
+    bool claimArmJoints(const std::string& arm_id,
+                        const std::vector<std::string>& joint_names,
+                        std::uint64_t& claim_id,
+                        std::string* error = nullptr);
+    void releaseArmJoints(std::uint64_t claim_id) noexcept;
+    std::string armOwnerForJoint(const std::string& joint_name) const;
+
     static std::shared_ptr<MotorManager> managerFor(const std::string& id);
     static std::shared_ptr<simulate::MujocoWorld> mujocoWorldFor(const std::string& id);
     static void setActiveJoints(const std::string& motor_manager_id,
@@ -93,6 +102,13 @@ private:
     mutable std::mutex motors_mutex_;
     std::unordered_map<std::uint8_t, std::shared_ptr<AbstractMotor>> motors_by_id_;
     std::unordered_map<std::string, std::shared_ptr<AbstractMotor>> motors_by_joint_;
+    struct ArmJointClaim {
+        std::string arm_id;
+        std::vector<std::string> joint_names;
+    };
+    std::uint64_t next_arm_claim_id_{0};
+    std::unordered_map<std::uint64_t, ArmJointClaim> arm_claims_;
+    std::unordered_map<std::string, std::uint64_t> arm_claim_by_joint_;
     bool initialized_{false};
 
     static std::mutex registry_mutex_;

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

@@ -279,6 +279,120 @@ bool MotorManager::readFeedbacksAtomic(
     return false;
 }
 
+bool MotorManager::claimArmJoints(
+    const std::string& arm_id,
+    const std::vector<std::string>& joint_names,
+    std::uint64_t& claim_id,
+    std::string* error)
+{
+    claim_id = 0;
+    if (error) {
+        error->clear();
+    }
+    if (arm_id.empty() || joint_names.empty()) {
+        if (error) {
+            *error = "arm id and joint names are required";
+        }
+        return false;
+    }
+
+    std::unordered_set<std::string> unique_joints;
+    unique_joints.reserve(joint_names.size());
+    std::lock_guard<std::mutex> lock(motors_mutex_);
+    for (const auto& joint_name : joint_names) {
+        if (joint_name.empty() || !unique_joints.insert(joint_name).second) {
+            if (error) {
+                *error = joint_name.empty()
+                    ? "arm joint name cannot be empty"
+                    : "arm joint is listed more than once: " + joint_name;
+            }
+            return false;
+        }
+        if (motors_by_joint_.count(joint_name) == 0U) {
+            if (error) {
+                *error = "motor not found for arm joint: " + joint_name;
+            }
+            return false;
+        }
+        const auto existing = arm_claim_by_joint_.find(joint_name);
+        if (existing != arm_claim_by_joint_.end()) {
+            const auto owner = arm_claims_.find(existing->second);
+            if (error) {
+                *error = "motor joint is already controlled by RobotArm";
+                if (owner != arm_claims_.end()) {
+                    *error += " '" + owner->second.arm_id + "'";
+                }
+                *error += ": " + joint_name;
+            }
+            return false;
+        }
+    }
+
+    do {
+        ++next_arm_claim_id_;
+    } while (next_arm_claim_id_ == 0U ||
+             arm_claims_.count(next_arm_claim_id_) != 0U);
+
+    ArmJointClaim claim;
+    claim.arm_id = arm_id;
+    claim.joint_names.assign(unique_joints.begin(), unique_joints.end());
+    const auto new_claim_id = next_arm_claim_id_;
+    arm_claims_.emplace(new_claim_id, std::move(claim));
+    try {
+        for (const auto& joint_name : unique_joints) {
+            arm_claim_by_joint_.emplace(joint_name, new_claim_id);
+        }
+    } catch (...) {
+        for (auto it = arm_claim_by_joint_.begin();
+             it != arm_claim_by_joint_.end();) {
+            if (it->second == new_claim_id) {
+                it = arm_claim_by_joint_.erase(it);
+            } else {
+                ++it;
+            }
+        }
+        arm_claims_.erase(new_claim_id);
+        throw;
+    }
+    claim_id = new_claim_id;
+    return true;
+}
+
+void MotorManager::releaseArmJoints(const std::uint64_t claim_id) noexcept
+{
+    if (claim_id == 0U) {
+        return;
+    }
+    try {
+        std::lock_guard<std::mutex> lock(motors_mutex_);
+        const auto claim = arm_claims_.find(claim_id);
+        if (claim == arm_claims_.end()) {
+            return;
+        }
+        for (const auto& joint_name : claim->second.joint_names) {
+            const auto owner = arm_claim_by_joint_.find(joint_name);
+            if (owner != arm_claim_by_joint_.end() &&
+                owner->second == claim_id) {
+                arm_claim_by_joint_.erase(owner);
+            }
+        }
+        arm_claims_.erase(claim);
+    } catch (...) {
+    }
+}
+
+std::string MotorManager::armOwnerForJoint(
+    const std::string& joint_name) const
+{
+    std::lock_guard<std::mutex> lock(motors_mutex_);
+    const auto owner = arm_claim_by_joint_.find(joint_name);
+    if (owner == arm_claim_by_joint_.end()) {
+        return {};
+    }
+    const auto claim = arm_claims_.find(owner->second);
+    return claim == arm_claims_.end() ? std::string{} : claim->second.arm_id;
+}
+
 std::shared_ptr<MotorManager> MotorManager::managerFor(const std::string& id)
 {
     std::lock_guard<std::mutex> lock(registry_mutex_);

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/CMakeLists.txt

@@ -7,3 +7,32 @@ add_library(cmvr_es::device::ffmpeg_speaker ALIAS ffmpeg_speaker)
 target_link_libraries(ffmpeg_speaker PRIVATE -lpulse-simple -lpulse cmvr_es::proto)
 
 install(TARGETS ffmpeg_speaker LIBRARY DESTINATION lib)
+
+if(BUILD_TESTING)
+    add_executable(ffmpeg_speaker_lifecycle_test
+            tests/ffmpeg_speaker_lifecycle_test.cpp
+    )
+    target_link_libraries(ffmpeg_speaker_lifecycle_test
+            PRIVATE
+            cmvr_es::device::ffmpeg_speaker
+            avcodec
+            avformat
+            avutil
+            swresample
+    )
+    add_test(
+            NAME ffmpeg_speaker_lifecycle_test
+            COMMAND ffmpeg_speaker_lifecycle_test
+    )
+    set(_ffmpeg_speaker_test_environment
+            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
+    )
+    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+        list(APPEND _ffmpeg_speaker_test_environment
+                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+    endif()
+    set_tests_properties(ffmpeg_speaker_lifecycle_test PROPERTIES
+            TIMEOUT 10
+            ENVIRONMENT "${_ffmpeg_speaker_test_environment}"
+    )
+endif()

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;

cmvr-es/devices/speaker/ffmpeg_speaker/tests/ffmpeg_speaker_lifecycle_test.cpp

@@ -0,0 +1,54 @@
+#include "include/ffmpeg_speaker.h"
+
+#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()
+{
+    cmvr::config::FFMpegSpeakerConfig config;
+    config.set_id("lifecycle-test-speaker");
+    cmvr::device::ffmpegSpeaker speaker(config);
+    cmvr::device::SpeakerState state{};
+
+    CHECK_TRUE(speaker.init());
+    speaker.getState(state);
+    CHECK_TRUE(state.is_initialized);
+
+    speaker.resetPlayState();
+    speaker.getState(state);
+    CHECK_TRUE(state.is_initialized);
+
+    CHECK_TRUE(speaker.stopPlayback());
+    speaker.getState(state);
+    CHECK_TRUE(state.is_initialized);
+
+    speaker.stopStreaming();
+    speaker.getState(state);
+    CHECK_TRUE(state.is_initialized);
+
+    CHECK_TRUE(speaker.stop());
+    speaker.getState(state);
+    CHECK_TRUE(!state.is_initialized);
+
+    std::cout << "ffmpeg_speaker_lifecycle_test: PASS\n";
+    return 0;
+}

cmvr-es/manager/README.md

@@ -6,13 +6,20 @@
 
 ## 当前管理器
 
+管理模块目录统一使用 `*_manager` 后缀，主管理类使用 `*Manager` 后缀。工厂、适配器、
+账本、快照和结果结构体属于管理器内部的支撑类型，保留其职责名称，不强行改成
+`*Manager`。
+
 | 目录 | CMake target | 职责 |
 | --- | --- | --- |
+| [`control_authority_manager/`](control_authority_manager/) | `cmvr_es::control_authority_manager` | 控制权租约、代际、dispatch fence 和 quarantine |
 | [`device_manager/`](device_manager/) | `cmvr_es::device_manager` | 按配置创建、初始化、查询和批量启停设备 |
+| [`safety_manager/`](safety_manager/) | `cmvr_es::safety_manager` | Sensor/Control 安全准入、StopAll、恢复和命令账本 |
 | [`task_manager/`](task_manager/) | `cmvr_es::task_manager` | 创建任务、校验运行模式、统一启停和调度周期任务 |
-| [`media_source_hub/`](media_source_hub/) | `cmvr_es::media_source_hub`、`cmvr_es::device_media_source_adapter` | 实时媒体源注册、按需启停和多消费者分发 |
+| [`media_source_manager/`](media_source_manager/) | `cmvr_es::media_source_manager`、`cmvr_es::device_media_source_adapter` | 实时媒体源注册、按需启停和多消费者分发 |
 
-`manager/` 当前没有聚合 `CMakeLists.txt`，三个子目录由 [`../CMakeLists.txt`](../CMakeLists.txt) 分别加入。新增 manager 时必须显式更新该文件。
+`manager/` 当前没有聚合 `CMakeLists.txt`，所有模块由 [`../CMakeLists.txt`](../CMakeLists.txt)
+按依赖顺序加入。新增 manager 时必须同时更新目录、target、依赖顺序和本 README。
 
 ## 进程生命周期
 
@@ -30,7 +37,8 @@
 
 - DeviceManager 构造不会自动调用全部设备的 `start()`；
 - 当前主退出路径没有调用 `DeviceManager::stop()`；
-- `SystemService/StopAll` 会调用 DeviceManager stop；
+- `SystemService/StopAll` 只停止当前运动、控制和媒体活动，不调用
+  `DeviceManager::stop()`，成功返回后可继续接受新命令；
 - `DeviceManager::destroyInstance()` 不调用设备 stop，销毁前必须先显式停止；
 - `TaskManager::destroyInstance()` 会调用 `stopRunTask()`，但 manager 未处于 running 状态时该调用会直接返回；
 - DeviceManager 和 TaskManager 都是首次配置生效的单例，不支持热加载。
@@ -135,12 +143,12 @@
 - 有顺序依赖的工作应放入同一协调任务或显式建模；
 - task 返回后，其内部状态并发安全由具体实现负责。
 
-## MediaSourceHub
+## MediaSourceManager
 
 关键文件：
 
-- [`media_source_hub/include/media_source_hub.h`](media_source_hub/include/media_source_hub.h)
-- [`media_source_hub/src/device_media_source_adapter.cpp`](media_source_hub/src/device_media_source_adapter.cpp)
+- [`media_source_manager/include/media_source_manager.h`](media_source_manager/include/media_source_manager.h)
+- [`media_source_manager/src/device_media_source_adapter.cpp`](media_source_manager/src/device_media_source_adapter.cpp)
 - [`../common/media/media_frame.h`](../common/media/media_frame.h)
 - [`../common/base/ring_buffer.h`](../common/base/ring_buffer.h)
 
@@ -151,7 +159,8 @@
 | 摄像头彩色流 | `<device_id>/video/color` | 64 |
 | 麦克风主流 | `<device_id>/audio/main` | 256 |
 
-当前 gRPC RGB/麦克风流和 QUIC 彩色/麦克风轨道使用 Hub；gRPC Depth/RGBD 仍直接读取设备帧。
+当前 gRPC RGB/麦克风流和 QUIC 彩色/麦克风轨道使用 MediaSourceManager；gRPC Depth/RGBD
+仍直接读取设备帧。
 
 ### 注册新媒体源
 
@@ -210,7 +219,7 @@ ring generation 不等于 `TrackDescriptor::generation`，ring 的 `ReadResult.s
 
 ## 新增第四种 Manager
 
-1. 先确认能力不是 DeviceManager、TaskManager 或 MediaSourceHub 的子职责；
+1. 先确认能力不是 DeviceManager、TaskManager 或 MediaSourceManager 的子职责；
 2. 定义所有权、初始化、start/stop 和线程模型；
 3. 避免新增无必要的全局单例；
 4. 新建独立目录、头文件、实现和 CMake target；
@@ -220,13 +229,13 @@ ring generation 不等于 `TrackDescriptor::generation`，ring 的 `ReadResult.s
 
 ## 测试
 
-MediaSourceHub：
+MediaSourceManager：
 
 ```bash
-cmake --build build --target media_source_hub_test
+cmake --build build --target media_source_manager_test
 ctest \
   --test-dir build \
-  -R '^media_source_hub_test$' \
+  -R '^media_source_manager_test$' \
   --output-on-failure
 ```
 

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

@@ -1,100 +0,0 @@
-#ifndef CMVR_ES_CONTROL_AUTHORITY_MANAGER_H
-#define CMVR_ES_CONTROL_AUTHORITY_MANAGER_H
-
-#include <chrono>
-#include <cstdint>
-#include <mutex>
-#include <string>
-#include <unordered_map>
-
-namespace cmvr::control {
-
-struct ControlLeaseToken {
-    std::string resource_id;
-    std::string owner_id;
-    std::uint64_t generation{0};
-
-    bool valid() const noexcept
-    {
-        return !resource_id.empty() &&
-               !owner_id.empty() &&
-               generation != 0U;
-    }
-};
-
-struct ControlAcquireResult {
-    bool acquired{false};
-    ControlLeaseToken token;
-    std::string detail;
-};
-
-// Process-wide, transport-independent control ownership. The generation in a
-// token prevents a delayed release from an old network session from releasing
-// a newer lease on the same arm.
-class ControlAuthorityManager {
-public:
-    using Duration = std::chrono::milliseconds;
-
-    static ControlAuthorityManager& instance();
-
-    ControlAcquireResult tryAcquire(
-        const std::string& resource_id,
-        const std::string& owner_id,
-        Duration ttl);
-
-    // Atomically invalidates a normal control lease and joins a safety
-    // barrier. Each safety caller receives an independent token; normal
-    // control remains blocked until the last safety token is released.
-    ControlAcquireResult preemptAcquire(
-        const std::string& resource_id,
-        const std::string& owner_id,
-        Duration ttl);
-
-    // Converts the expected normal lease into a safety barrier only while it
-    // is still the current lease. A stale token never preempts a successor or
-    // joins an existing safety barrier.
-    ControlAcquireResult preemptAcquireIfCurrent(
-        const ControlLeaseToken& expected_token,
-        const std::string& owner_id,
-        Duration ttl);
-
-    // Permanently blocks the resource only if the expected normal lease is
-    // still current. Quarantine does not allocate and can only be removed by
-    // an explicit revoke/clear.
-    bool quarantineIfCurrent(
-        const ControlLeaseToken& expected_token) noexcept;
-    bool renew(const ControlLeaseToken& token, Duration ttl);
-    bool validate(const ControlLeaseToken& token);
-    void release(const ControlLeaseToken& token) noexcept;
-
-    // Safety/control paths which do not possess a lease use this query to
-    // reject mutating commands. Read-only state and stop/torque-off commands
-    // are intentionally allowed by their callers.
-    bool isLeased(const std::string& resource_id);
-    void revoke(const std::string& resource_id) noexcept;
-
-    // Test/process teardown hook. Runtime code should release/revoke exact
-    // resources instead of clearing unrelated ownership.
-    void clear() noexcept;
-
-private:
-    struct Entry {
-        std::string owner_id;
-        std::uint64_t generation{0};
-        std::chrono::steady_clock::time_point deadline;
-        bool preemptible{true};
-        bool quarantined{false};
-        std::unordered_map<std::uint64_t, std::string> safety_holders;
-    };
-
-    static void quarantine_(Entry& entry) noexcept;
-    bool expired_(const Entry& entry) const noexcept;
-
-    std::mutex mutex_;
-    std::unordered_map<std::string, Entry> entries_;
-    std::uint64_t next_generation_{0};
-};
-
-} // namespace cmvr::control
-
-#endif // CMVR_ES_CONTROL_AUTHORITY_MANAGER_H

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

@@ -1,323 +0,0 @@
-#include "manager/control_authority/include/control_authority_manager.h"
-
-#include <type_traits>
-#include <utility>
-
-namespace cmvr::control {
-
-ControlAuthorityManager& ControlAuthorityManager::instance()
-{
-    static ControlAuthorityManager manager;
-    return manager;
-}
-
-ControlAcquireResult ControlAuthorityManager::tryAcquire(
-    const std::string& resource_id,
-    const std::string& owner_id,
-    const Duration ttl)
-{
-    if (resource_id.empty() || owner_id.empty() ||
-        ttl <= Duration::zero()) {
-        return {false, {}, "invalid control lease request"};
-    }
-
-    std::lock_guard lock(mutex_);
-    const auto existing = entries_.find(resource_id);
-    if (existing != entries_.end()) {
-        if (!expired_(existing->second)) {
-            return {
-                false,
-                {},
-                "control resource is already leased by " +
-                    existing->second.owner_id};
-        }
-        entries_.erase(existing);
-    }
-
-    ControlLeaseToken token;
-    token.resource_id = resource_id;
-    token.owner_id = owner_id;
-    token.generation = ++next_generation_;
-    entries_.emplace(
-        resource_id,
-        Entry{
-            owner_id,
-            token.generation,
-            std::chrono::steady_clock::now() + ttl,
-            true,
-            false,
-            {}});
-    return {true, std::move(token), {}};
-}
-
-ControlAcquireResult ControlAuthorityManager::preemptAcquire(
-    const std::string& resource_id,
-    const std::string& owner_id,
-    const Duration ttl)
-{
-    if (resource_id.empty() || owner_id.empty() ||
-        ttl <= Duration::zero()) {
-        return {false, {}, "invalid control barrier request"};
-    }
-
-    std::lock_guard lock(mutex_);
-    const auto existing = entries_.find(resource_id);
-    if (existing != entries_.end()) {
-        if (!expired_(existing->second) &&
-            !existing->second.preemptible) {
-            ControlLeaseToken token;
-            token.resource_id = resource_id;
-            token.owner_id = owner_id;
-            token.generation = ++next_generation_;
-            existing->second.safety_holders.emplace(
-                token.generation, token.owner_id);
-            return {true, std::move(token), {}};
-        }
-    }
-
-    const bool has_existing = existing != entries_.end();
-    const bool replacing_normal =
-        has_existing && existing->second.preemptible;
-    try {
-        ControlLeaseToken token;
-        token.resource_id = resource_id;
-        token.owner_id = owner_id;
-        token.generation = ++next_generation_;
-        Entry replacement{
-            owner_id,
-            token.generation,
-            std::chrono::steady_clock::time_point::max(),
-            false,
-            false,
-            {{token.generation, owner_id}}};
-
-        if (has_existing) {
-            static_assert(
-                std::is_nothrow_move_assignable_v<Entry>,
-                "safety barrier replacement must not throw");
-            existing->second = std::move(replacement);
-        } else {
-            entries_.emplace(resource_id, std::move(replacement));
-        }
-        return {true, std::move(token), {}};
-    } catch (...) {
-        if (replacing_normal && existing->second.preemptible) {
-            quarantine_(existing->second);
-        }
-        throw;
-    }
-}
-
-ControlAcquireResult ControlAuthorityManager::preemptAcquireIfCurrent(
-    const ControlLeaseToken& expected_token,
-    const std::string& owner_id,
-    const Duration ttl)
-{
-    if (!expected_token.valid() || owner_id.empty() ||
-        ttl <= Duration::zero()) {
-        return {false, {}, "invalid conditional control barrier request"};
-    }
-
-    std::lock_guard lock(mutex_);
-    const auto existing = entries_.find(expected_token.resource_id);
-    if (existing == entries_.end() ||
-        expired_(existing->second) ||
-        !existing->second.preemptible ||
-        existing->second.owner_id != expected_token.owner_id ||
-        existing->second.generation != expected_token.generation) {
-        return {
-            false,
-            {},
-            "expected control lease is no longer current"};
-    }
-
-    try {
-        ControlLeaseToken token;
-        token.resource_id = expected_token.resource_id;
-        token.owner_id = owner_id;
-        token.generation = ++next_generation_;
-        Entry replacement{
-            owner_id,
-            token.generation,
-            std::chrono::steady_clock::time_point::max(),
-            false,
-            false,
-            {{token.generation, owner_id}}};
-
-        static_assert(
-            std::is_nothrow_move_assignable_v<Entry>,
-            "safety barrier replacement must not throw");
-        existing->second = std::move(replacement);
-        return {true, std::move(token), {}};
-    } catch (...) {
-        if (existing->second.preemptible) {
-            quarantine_(existing->second);
-        }
-        throw;
-    }
-}
-
-bool ControlAuthorityManager::quarantineIfCurrent(
-    const ControlLeaseToken& expected_token) noexcept
-{
-    if (!expected_token.valid()) {
-        return false;
-    }
-    try {
-        std::lock_guard lock(mutex_);
-        const auto existing =
-            entries_.find(expected_token.resource_id);
-        if (existing == entries_.end() ||
-            expired_(existing->second) ||
-            !existing->second.preemptible ||
-            existing->second.owner_id != expected_token.owner_id ||
-            existing->second.generation != expected_token.generation) {
-            return false;
-        }
-        quarantine_(existing->second);
-        return true;
-    } catch (...) {
-        return false;
-    }
-}
-
-bool ControlAuthorityManager::renew(
-    const ControlLeaseToken& token,
-    const Duration ttl)
-{
-    if (!token.valid() || ttl <= Duration::zero()) {
-        return false;
-    }
-    std::lock_guard lock(mutex_);
-    const auto found = entries_.find(token.resource_id);
-    if (found == entries_.end() || expired_(found->second)) {
-        if (found != entries_.end() && expired_(found->second)) {
-            entries_.erase(found);
-        }
-        return false;
-    }
-    if (!found->second.preemptible) {
-        const auto holder =
-            found->second.safety_holders.find(token.generation);
-        return holder != found->second.safety_holders.end() &&
-               holder->second == token.owner_id;
-    }
-    if (found->second.owner_id != token.owner_id ||
-        found->second.generation != token.generation) {
-        return false;
-    }
-    found->second.deadline =
-        std::chrono::steady_clock::now() + ttl;
-    return true;
-}
-
-bool ControlAuthorityManager::validate(
-    const ControlLeaseToken& token)
-{
-    if (!token.valid()) {
-        return false;
-    }
-    std::lock_guard lock(mutex_);
-    const auto found = entries_.find(token.resource_id);
-    if (found == entries_.end()) {
-        return false;
-    }
-    if (expired_(found->second)) {
-        entries_.erase(found);
-        return false;
-    }
-    if (!found->second.preemptible) {
-        const auto holder =
-            found->second.safety_holders.find(token.generation);
-        return holder != found->second.safety_holders.end() &&
-               holder->second == token.owner_id;
-    }
-    return found->second.owner_id == token.owner_id &&
-           found->second.generation == token.generation;
-}
-
-void ControlAuthorityManager::release(
-    const ControlLeaseToken& token) noexcept
-{
-    if (!token.valid()) {
-        return;
-    }
-    try {
-        std::lock_guard lock(mutex_);
-        const auto found = entries_.find(token.resource_id);
-        if (found == entries_.end()) {
-            return;
-        }
-        if (!found->second.preemptible) {
-            const auto holder =
-                found->second.safety_holders.find(token.generation);
-            if (holder == found->second.safety_holders.end() ||
-                holder->second != token.owner_id) {
-                return;
-            }
-            found->second.safety_holders.erase(holder);
-            if (found->second.safety_holders.empty() &&
-                !found->second.quarantined) {
-                entries_.erase(found);
-            }
-        } else if (found->second.owner_id == token.owner_id &&
-                   found->second.generation == token.generation) {
-            entries_.erase(found);
-        }
-    } catch (...) {
-    }
-}
-
-bool ControlAuthorityManager::isLeased(
-    const std::string& resource_id)
-{
-    if (resource_id.empty()) {
-        return false;
-    }
-    std::lock_guard lock(mutex_);
-    const auto found = entries_.find(resource_id);
-    if (found == entries_.end()) {
-        return false;
-    }
-    if (expired_(found->second)) {
-        entries_.erase(found);
-        return false;
-    }
-    return true;
-}
-
-void ControlAuthorityManager::revoke(
-    const std::string& resource_id) noexcept
-{
-    try {
-        std::lock_guard lock(mutex_);
-        entries_.erase(resource_id);
-    } catch (...) {
-    }
-}
-
-void ControlAuthorityManager::clear() noexcept
-{
-    try {
-        std::lock_guard lock(mutex_);
-        entries_.clear();
-    } catch (...) {
-    }
-}
-
-bool ControlAuthorityManager::expired_(
-    const Entry& entry) const noexcept
-{
-    return !entry.quarantined &&
-           std::chrono::steady_clock::now() >= entry.deadline;
-}
-
-void ControlAuthorityManager::quarantine_(Entry& entry) noexcept
-{
-    entry.deadline =
-        std::chrono::steady_clock::time_point::max();
-    entry.preemptible = false;
-    entry.quarantined = true;
-}
-
-} // namespace cmvr::control

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

@@ -1,256 +0,0 @@
-#include "manager/control_authority/include/control_authority_manager.h"
-
-#include <chrono>
-#include <thread>
-
-#include <gtest/gtest.h>
-
-namespace cmvr::control {
-namespace {
-
-using namespace std::chrono_literals;
-
-class ControlAuthorityManagerTest : public ::testing::Test {
-protected:
-    void SetUp() override
-    {
-        ControlAuthorityManager::instance().clear();
-    }
-
-    void TearDown() override
-    {
-        ControlAuthorityManager::instance().clear();
-    }
-};
-
-TEST_F(ControlAuthorityManagerTest, LeaseIsExclusiveAndExactReleaseRestoresAccess)
-{
-    auto& manager = ControlAuthorityManager::instance();
-    const auto first =
-        manager.tryAcquire("right_arm", "session-a", 100ms);
-    ASSERT_TRUE(first.acquired);
-    EXPECT_TRUE(manager.validate(first.token));
-    EXPECT_TRUE(manager.isLeased("right_arm"));
-
-    const auto conflict =
-        manager.tryAcquire("right_arm", "session-b", 100ms);
-    EXPECT_FALSE(conflict.acquired);
-
-    manager.release(first.token);
-    EXPECT_FALSE(manager.isLeased("right_arm"));
-    EXPECT_TRUE(
-        manager.tryAcquire("right_arm", "session-b", 100ms)
-            .acquired);
-}
-
-TEST_F(ControlAuthorityManagerTest, StaleGenerationCannotReleaseNewLease)
-{
-    auto& manager = ControlAuthorityManager::instance();
-    const auto old =
-        manager.tryAcquire("right_arm", "session-a", 100ms);
-    ASSERT_TRUE(old.acquired);
-    manager.release(old.token);
-    const auto current =
-        manager.tryAcquire("right_arm", "session-a", 100ms);
-    ASSERT_TRUE(current.acquired);
-    ASSERT_NE(
-        old.token.generation,
-        current.token.generation);
-
-    manager.release(old.token);
-    EXPECT_TRUE(manager.validate(current.token));
-}
-
-TEST_F(ControlAuthorityManagerTest,
-       SafetyBarrierAtomicallyPreemptsControlAndRejectsOtherOwners)
-{
-    auto& manager = ControlAuthorityManager::instance();
-    const auto control =
-        manager.tryAcquire("right_arm", "move-session", 100ms);
-    ASSERT_TRUE(control.acquired);
-
-    const auto barrier = manager.preemptAcquire(
-        "right_arm", "stop-operation", 100ms);
-    ASSERT_TRUE(barrier.acquired) << barrier.detail;
-    EXPECT_FALSE(manager.validate(control.token));
-    EXPECT_TRUE(manager.validate(barrier.token));
-
-    const auto move_during_stop =
-        manager.tryAcquire("right_arm", "new-move", 100ms);
-    EXPECT_FALSE(move_during_stop.acquired);
-    const auto second_stop = manager.preemptAcquire(
-        "right_arm", "second-stop", 100ms);
-    ASSERT_TRUE(second_stop.acquired) << second_stop.detail;
-
-    manager.release(control.token);
-    EXPECT_TRUE(manager.validate(barrier.token));
-    manager.release(barrier.token);
-    EXPECT_TRUE(manager.validate(second_stop.token));
-    EXPECT_FALSE(
-        manager.tryAcquire("right_arm", "new-move", 100ms)
-            .acquired);
-    manager.release(second_stop.token);
-    EXPECT_FALSE(manager.isLeased("right_arm"));
-}
-
-TEST_F(ControlAuthorityManagerTest,
-       ConditionalSafetyBarrierPreemptsMatchingCurrentLease)
-{
-    auto& manager = ControlAuthorityManager::instance();
-    const auto control =
-        manager.tryAcquire("right_arm", "move-session", 100ms);
-    ASSERT_TRUE(control.acquired);
-
-    const auto barrier = manager.preemptAcquireIfCurrent(
-        control.token, "timed-out-action", 100ms);
-    ASSERT_TRUE(barrier.acquired) << barrier.detail;
-    EXPECT_FALSE(manager.validate(control.token));
-    EXPECT_TRUE(manager.validate(barrier.token));
-    manager.release(control.token);
-    EXPECT_TRUE(manager.validate(barrier.token));
-    EXPECT_FALSE(
-        manager.tryAcquire("right_arm", "new-move", 100ms)
-            .acquired);
-
-    manager.release(barrier.token);
-    EXPECT_FALSE(manager.isLeased("right_arm"));
-}
-
-TEST_F(ControlAuthorityManagerTest,
-       ConditionalSafetyBarrierDoesNotPreemptSuccessorForStaleToken)
-{
-    auto& manager = ControlAuthorityManager::instance();
-    const auto old =
-        manager.tryAcquire("right_arm", "move-session", 100ms);
-    ASSERT_TRUE(old.acquired);
-    const auto direct_stop = manager.preemptAcquire(
-        "right_arm", "direct-stop", 100ms);
-    ASSERT_TRUE(direct_stop.acquired) << direct_stop.detail;
-    manager.release(direct_stop.token);
-    const auto successor =
-        manager.tryAcquire("right_arm", "move-session", 100ms);
-    ASSERT_TRUE(successor.acquired);
-    ASSERT_NE(old.token.generation, successor.token.generation);
-
-    const auto barrier = manager.preemptAcquireIfCurrent(
-        old.token, "delayed-stop", 100ms);
-    EXPECT_FALSE(barrier.acquired);
-    EXPECT_FALSE(barrier.token.valid());
-    EXPECT_TRUE(manager.validate(successor.token));
-    EXPECT_FALSE(
-        manager.tryAcquire("right_arm", "competing-move", 100ms)
-            .acquired);
-
-    manager.release(successor.token);
-    EXPECT_FALSE(manager.isLeased("right_arm"));
-}
-
-TEST_F(ControlAuthorityManagerTest,
-       ConditionalSafetyBarrierDoesNotJoinExistingSafetyBarrier)
-{
-    auto& manager = ControlAuthorityManager::instance();
-    const auto control =
-        manager.tryAcquire("right_arm", "move-session", 100ms);
-    ASSERT_TRUE(control.acquired);
-    const auto existing_barrier = manager.preemptAcquire(
-        "right_arm", "direct-stop", 100ms);
-    ASSERT_TRUE(existing_barrier.acquired) << existing_barrier.detail;
-
-    const auto delayed_barrier = manager.preemptAcquireIfCurrent(
-        control.token, "delayed-action-stop", 100ms);
-    EXPECT_FALSE(delayed_barrier.acquired);
-    EXPECT_FALSE(delayed_barrier.token.valid());
-    EXPECT_TRUE(manager.validate(existing_barrier.token));
-
-    manager.release(existing_barrier.token);
-    EXPECT_FALSE(manager.isLeased("right_arm"));
-    EXPECT_TRUE(
-        manager.tryAcquire("right_arm", "new-move", 100ms)
-            .acquired);
-}
-
-TEST_F(ControlAuthorityManagerTest,
-       QuarantineSurvivesNormalAndTemporarySafetyTokenRelease)
-{
-    auto& manager = ControlAuthorityManager::instance();
-    const auto control =
-        manager.tryAcquire("right_arm", "move-session", 20ms);
-    ASSERT_TRUE(control.acquired);
-
-    ASSERT_TRUE(manager.quarantineIfCurrent(control.token));
-    EXPECT_FALSE(manager.validate(control.token));
-    EXPECT_TRUE(manager.isLeased("right_arm"));
-
-    manager.release(control.token);
-    std::this_thread::sleep_for(30ms);
-    EXPECT_TRUE(manager.isLeased("right_arm"));
-    EXPECT_FALSE(
-        manager.tryAcquire("right_arm", "new-move", 100ms)
-            .acquired);
-
-    const auto temporary_stop = manager.preemptAcquire(
-        "right_arm", "temporary-stop", 100ms);
-    ASSERT_TRUE(temporary_stop.acquired) << temporary_stop.detail;
-    EXPECT_TRUE(manager.validate(temporary_stop.token));
-    manager.release(temporary_stop.token);
-
-    EXPECT_TRUE(manager.isLeased("right_arm"));
-    EXPECT_FALSE(
-        manager.tryAcquire("right_arm", "new-move", 100ms)
-            .acquired);
-
-    manager.revoke("right_arm");
-    EXPECT_FALSE(manager.isLeased("right_arm"));
-}
-
-TEST_F(ControlAuthorityManagerTest,
-       QuarantineWithStaleTokenDoesNotAffectSuccessor)
-{
-    auto& manager = ControlAuthorityManager::instance();
-    const auto old =
-        manager.tryAcquire("right_arm", "move-session", 100ms);
-    ASSERT_TRUE(old.acquired);
-    manager.release(old.token);
-    const auto successor =
-        manager.tryAcquire("right_arm", "move-session", 100ms);
-    ASSERT_TRUE(successor.acquired);
-    ASSERT_NE(old.token.generation, successor.token.generation);
-
-    EXPECT_FALSE(manager.quarantineIfCurrent(old.token));
-    EXPECT_TRUE(manager.validate(successor.token));
-    EXPECT_FALSE(
-        manager.tryAcquire("right_arm", "competing-move", 100ms)
-            .acquired);
-
-    manager.release(successor.token);
-    EXPECT_FALSE(manager.isLeased("right_arm"));
-}
-
-TEST_F(ControlAuthorityManagerTest, ExpiryAndRenewUseMonotonicLocalTime)
-{
-    auto& manager = ControlAuthorityManager::instance();
-    const auto lease =
-        manager.tryAcquire("right_arm", "session-a", 20ms);
-    ASSERT_TRUE(lease.acquired);
-    std::this_thread::sleep_for(10ms);
-    ASSERT_TRUE(manager.renew(lease.token, 30ms));
-    std::this_thread::sleep_for(20ms);
-    EXPECT_TRUE(manager.validate(lease.token));
-    std::this_thread::sleep_for(20ms);
-    EXPECT_FALSE(manager.validate(lease.token));
-    EXPECT_FALSE(manager.isLeased("right_arm"));
-}
-
-TEST_F(ControlAuthorityManagerTest, DifferentArmsCanBeLeasedIndependently)
-{
-    auto& manager = ControlAuthorityManager::instance();
-    EXPECT_TRUE(
-        manager.tryAcquire("right_arm", "session-a", 100ms)
-            .acquired);
-    EXPECT_TRUE(
-        manager.tryAcquire("left_arm", "session-b", 100ms)
-            .acquired);
-}
-
-} // namespace
-} // namespace cmvr::control

cmvr-es/manager/control_authority_manager/CMakeLists.txt

@@ -1,17 +1,17 @@
-add_library(control_authority STATIC
+add_library(control_authority_manager STATIC
         src/control_authority_manager.cpp
 )
-target_compile_features(control_authority PUBLIC cxx_std_17)
-target_include_directories(control_authority
+target_compile_features(control_authority_manager PUBLIC cxx_std_17)
+target_include_directories(control_authority_manager
         PUBLIC
         ${PROJECT_SOURCE_DIR}/cmvr-es
 )
 
 add_library(
-        cmvr_es::control_authority
-        ALIAS control_authority
+        cmvr_es::control_authority_manager
+        ALIAS control_authority_manager
 )
-install(TARGETS control_authority ARCHIVE DESTINATION lib)
+install(TARGETS control_authority_manager ARCHIVE DESTINATION lib)
 
 if(BUILD_TESTING)
     add_executable(control_authority_manager_test
@@ -19,7 +19,7 @@ if(BUILD_TESTING)
     )
     target_link_libraries(control_authority_manager_test
             PRIVATE
-            cmvr_es::control_authority
+            cmvr_es::control_authority_manager
             gtest
             gtest_main
             pthread

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

@@ -0,0 +1,170 @@
+#ifndef CMVR_ES_CONTROL_AUTHORITY_MANAGER_H
+#define CMVR_ES_CONTROL_AUTHORITY_MANAGER_H
+
+#include <chrono>
+#include <condition_variable>
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <unordered_map>
+
+namespace cmvr::control {
+
+struct ControlLeaseToken {
+    std::string resource_id;
+    std::string owner_id;
+    std::uint64_t generation{0};
+
+    bool valid() const noexcept
+    {
+        return !resource_id.empty() &&
+               !owner_id.empty() &&
+               generation != 0U;
+    }
+};
+
+struct ControlAcquireResult {
+    bool acquired{false};
+    ControlLeaseToken token;
+    std::string detail;
+};
+
+class ControlDispatchGuard;
+
+// Process-wide, transport-independent control ownership. The generation in a
+// token prevents a delayed release from an old network session from releasing
+// a newer lease on the same arm.
+class ControlAuthorityManager {
+public:
+    using Duration = std::chrono::milliseconds;
+
+    static ControlAuthorityManager& instance();
+
+    ControlAcquireResult tryAcquire(
+        const std::string& resource_id,
+        const std::string& owner_id,
+        Duration ttl);
+
+    // Atomically invalidates a normal control lease and joins a safety
+    // barrier. Each safety caller receives an independent token; normal
+    // control remains blocked until the last safety token is released.
+    ControlAcquireResult preemptAcquire(
+        const std::string& resource_id,
+        const std::string& owner_id,
+        Duration ttl);
+
+    // Converts the expected normal lease into a safety barrier only while it
+    // is still the current lease. A stale token never preempts a successor or
+    // joins an existing safety barrier.
+    ControlAcquireResult preemptAcquireIfCurrent(
+        const ControlLeaseToken& expected_token,
+        const std::string& owner_id,
+        Duration ttl);
+
+    // Waits for normal lease handlers displaced by the current safety barrier
+    // to release their tokens and for their in-flight dispatches to finish.
+    // Returns false on timeout or when safety_token is no longer a holder of
+    // the current entry.
+    bool waitForPreemptedRelease(
+        const ControlLeaseToken& safety_token,
+        Duration timeout);
+
+    // Permanently blocks the resource only if the expected normal lease is
+    // still current. A later safety holder may clear this fail-closed state
+    // only after it has independently confirmed the preempted handler exited.
+    bool quarantineIfCurrent(
+        const ControlLeaseToken& expected_token) noexcept;
+
+    // Abandons a safety token while retaining its barrier. A retired token can
+    // no longer be validated, released, or used as a recovery authority. This
+    // lets a failed stop path discard local token ownership without silently
+    // reopening the resource.
+    bool retireSafetyHolder(
+        const ControlLeaseToken& safety_token) noexcept;
+
+    // Clears retired safety holders and a tokenless quarantine after a newer,
+    // active safety holder has confirmed every preempted normal handler has
+    // exited. Other active safety holders are deliberately preserved.
+    bool recoverRetiredSafetyHolders(
+        const ControlLeaseToken& recovery_token) noexcept;
+
+    bool renew(const ControlLeaseToken& token, Duration ttl);
+    bool validate(const ControlLeaseToken& token);
+
+    // Use only around a bounded device-command submission. Never retain this
+    // guard while waiting for physical motion or another long-running task.
+    ControlDispatchGuard tryBeginDispatch(
+        const ControlLeaseToken& token);
+    void release(const ControlLeaseToken& token) noexcept;
+
+    // Safety/control paths which do not possess a lease use this query to
+    // reject mutating commands. Read-only state and stop/torque-off commands
+    // are intentionally allowed by their callers.
+    bool isLeased(const std::string& resource_id);
+    void revoke(const std::string& resource_id) noexcept;
+
+    // Test/process teardown hook. Runtime code should release/revoke exact
+    // resources instead of clearing unrelated ownership.
+    void clear() noexcept;
+
+private:
+    friend class ControlDispatchGuard;
+
+    struct SafetyHolder {
+        std::string owner_id;
+        bool retired{false};
+    };
+
+    struct Entry;
+
+    static void quarantine_(Entry& entry) noexcept;
+    bool expired_(const Entry& entry) const noexcept;
+    static bool isSafetyHolder_(
+        const Entry& entry,
+        const ControlLeaseToken& token) noexcept;
+    static bool isActiveSafetyHolder_(
+        const Entry& entry,
+        const ControlLeaseToken& token) noexcept;
+    static bool canErase_(const Entry& entry) noexcept;
+    static void invalidateToDispatchFence_(Entry& entry) noexcept;
+    void endDispatch_(const std::shared_ptr<Entry>& entry) noexcept;
+
+    std::mutex mutex_;
+    std::condition_variable release_cv_;
+    std::unordered_map<std::string, std::shared_ptr<Entry>> entries_;
+    std::uint64_t next_generation_{0};
+};
+
+// Tracks one bounded backend dispatch without retaining the process-wide
+// authority lock. Safety preemption invalidates the lease immediately, while
+// waitForPreemptedRelease() joins both the displaced handler and its in-flight
+// dispatches before the safety operation reaches the device.
+class ControlDispatchGuard final {
+public:
+    ControlDispatchGuard() noexcept = default;
+    ~ControlDispatchGuard() noexcept;
+
+    ControlDispatchGuard(ControlDispatchGuard&& other) noexcept;
+    ControlDispatchGuard& operator=(ControlDispatchGuard&& other) noexcept;
+
+    ControlDispatchGuard(const ControlDispatchGuard&) = delete;
+    ControlDispatchGuard& operator=(const ControlDispatchGuard&) = delete;
+
+    bool acquired() const noexcept { return entry_ != nullptr; }
+
+private:
+    friend class ControlAuthorityManager;
+
+    ControlDispatchGuard(
+        ControlAuthorityManager* manager,
+        std::shared_ptr<ControlAuthorityManager::Entry> entry) noexcept;
+    void reset_() noexcept;
+
+    ControlAuthorityManager* manager_{nullptr};
+    std::shared_ptr<ControlAuthorityManager::Entry> entry_;
+};
+
+} // namespace cmvr::control
+
+#endif // CMVR_ES_CONTROL_AUTHORITY_MANAGER_H

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

@@ -0,0 +1,682 @@
+#include "manager/control_authority_manager/include/control_authority_manager.h"
+
+#include <utility>
+
+namespace cmvr::control {
+
+struct ControlAuthorityManager::Entry {
+    std::string resource_id;
+    std::string owner_id;
+    std::uint64_t generation{0};
+    std::chrono::steady_clock::time_point deadline;
+    bool preemptible{true};
+    bool quarantined{false};
+    bool quarantined_normal_pending{false};
+    bool dispatch_fence_only{false};
+    std::uint64_t in_flight_dispatches{0};
+    std::unordered_map<std::uint64_t, SafetyHolder> safety_holders;
+    std::unordered_map<std::uint64_t, std::string>
+        preempted_normal_holders;
+};
+
+ControlDispatchGuard::ControlDispatchGuard(
+    ControlAuthorityManager* const manager,
+    std::shared_ptr<ControlAuthorityManager::Entry> entry) noexcept
+    : manager_(manager),
+      entry_(std::move(entry))
+{
+}
+
+ControlDispatchGuard::~ControlDispatchGuard() noexcept
+{
+    reset_();
+}
+
+ControlDispatchGuard::ControlDispatchGuard(
+    ControlDispatchGuard&& other) noexcept
+    : manager_(std::exchange(other.manager_, nullptr)),
+      entry_(std::move(other.entry_))
+{
+}
+
+ControlDispatchGuard& ControlDispatchGuard::operator=(
+    ControlDispatchGuard&& other) noexcept
+{
+    if (this != &other) {
+        reset_();
+        manager_ = std::exchange(other.manager_, nullptr);
+        entry_ = std::move(other.entry_);
+    }
+    return *this;
+}
+
+void ControlDispatchGuard::reset_() noexcept
+{
+    if (entry_ == nullptr) {
+        return;
+    }
+    auto entry = std::move(entry_);
+    auto* const manager = std::exchange(manager_, nullptr);
+    manager->endDispatch_(entry);
+}
+
+ControlAuthorityManager& ControlAuthorityManager::instance()
+{
+    static ControlAuthorityManager manager;
+    return manager;
+}
+
+ControlAcquireResult ControlAuthorityManager::tryAcquire(
+    const std::string& resource_id,
+    const std::string& owner_id,
+    const Duration ttl)
+{
+    if (resource_id.empty() || owner_id.empty() ||
+        ttl <= Duration::zero()) {
+        return {false, {}, "invalid control lease request"};
+    }
+
+    std::lock_guard lock(mutex_);
+    const auto existing = entries_.find(resource_id);
+    if (existing != entries_.end()) {
+        auto& entry = *existing->second;
+        if (!expired_(entry)) {
+            if (entry.dispatch_fence_only) {
+                return {
+                    false,
+                    {},
+                    "control resource still has an in-flight dispatch"};
+            }
+            return {
+                false,
+                {},
+                "control resource is already leased by " +
+                    entry.owner_id};
+        }
+        if (entry.in_flight_dispatches != 0U) {
+            invalidateToDispatchFence_(entry);
+            return {
+                false,
+                {},
+                "control resource still has an in-flight dispatch"};
+        }
+        entries_.erase(existing);
+    }
+
+    ControlLeaseToken token;
+    token.resource_id = resource_id;
+    token.owner_id = owner_id;
+    token.generation = ++next_generation_;
+
+    auto entry = std::make_shared<Entry>();
+    entry->resource_id = resource_id;
+    entry->owner_id = owner_id;
+    entry->generation = token.generation;
+    entry->deadline = std::chrono::steady_clock::now() + ttl;
+    entries_.emplace(resource_id, std::move(entry));
+    return {true, std::move(token), {}};
+}
+
+ControlAcquireResult ControlAuthorityManager::preemptAcquire(
+    const std::string& resource_id,
+    const std::string& owner_id,
+    const Duration ttl)
+{
+    if (resource_id.empty() || owner_id.empty() ||
+        ttl <= Duration::zero()) {
+        return {false, {}, "invalid control barrier request"};
+    }
+
+    std::lock_guard lock(mutex_);
+    const auto existing = entries_.find(resource_id);
+    if (existing != entries_.end() &&
+        !existing->second->preemptible &&
+        !existing->second->dispatch_fence_only) {
+        ControlLeaseToken token;
+        token.resource_id = resource_id;
+        token.owner_id = owner_id;
+        token.generation = ++next_generation_;
+        existing->second->safety_holders.emplace(
+            token.generation,
+            SafetyHolder{token.owner_id, false});
+        return {true, std::move(token), {}};
+    }
+
+    const bool replacing_normal =
+        existing != entries_.end() && existing->second->preemptible;
+    try {
+        ControlLeaseToken token;
+        token.resource_id = resource_id;
+        token.owner_id = owner_id;
+        token.generation = ++next_generation_;
+
+        std::unordered_map<std::uint64_t, SafetyHolder> safety_holders;
+        safety_holders.emplace(
+            token.generation,
+            SafetyHolder{owner_id, false});
+        std::string safety_owner = owner_id;
+        std::unordered_map<std::uint64_t, std::string>
+            preempted_normal_holders;
+        if (replacing_normal) {
+            preempted_normal_holders.emplace(
+                existing->second->generation,
+                existing->second->owner_id);
+        }
+
+        std::shared_ptr<Entry> entry;
+        if (existing == entries_.end()) {
+            entry = std::make_shared<Entry>();
+            entry->resource_id = resource_id;
+            entry->owner_id.swap(safety_owner);
+            entry->generation = token.generation;
+            entry->deadline =
+                std::chrono::steady_clock::time_point::max();
+            entry->preemptible = false;
+            entry->safety_holders.swap(safety_holders);
+            entry->preempted_normal_holders.swap(
+                preempted_normal_holders);
+            entries_.emplace(resource_id, entry);
+        } else {
+            entry = existing->second;
+            entry->owner_id.swap(safety_owner);
+            entry->generation = token.generation;
+            entry->deadline =
+                std::chrono::steady_clock::time_point::max();
+            entry->preemptible = false;
+            entry->quarantined = false;
+            entry->quarantined_normal_pending = false;
+            entry->dispatch_fence_only = false;
+            entry->safety_holders.swap(safety_holders);
+            entry->preempted_normal_holders.swap(
+                preempted_normal_holders);
+        }
+        return {true, std::move(token), {}};
+    } catch (...) {
+        if (replacing_normal && existing->second->preemptible) {
+            quarantine_(*existing->second);
+        }
+        throw;
+    }
+}
+
+ControlAcquireResult ControlAuthorityManager::preemptAcquireIfCurrent(
+    const ControlLeaseToken& expected_token,
+    const std::string& owner_id,
+    const Duration ttl)
+{
+    if (!expected_token.valid() || owner_id.empty() ||
+        ttl <= Duration::zero()) {
+        return {false, {}, "invalid conditional control barrier request"};
+    }
+
+    std::lock_guard lock(mutex_);
+    const auto existing = entries_.find(expected_token.resource_id);
+    if (existing == entries_.end() ||
+        expired_(*existing->second) ||
+        !existing->second->preemptible ||
+        existing->second->owner_id != expected_token.owner_id ||
+        existing->second->generation != expected_token.generation) {
+        return {
+            false,
+            {},
+            "expected control lease is no longer current"};
+    }
+
+    try {
+        ControlLeaseToken token;
+        token.resource_id = expected_token.resource_id;
+        token.owner_id = owner_id;
+        token.generation = ++next_generation_;
+
+        std::unordered_map<std::uint64_t, SafetyHolder> safety_holders;
+        safety_holders.emplace(
+            token.generation,
+            SafetyHolder{owner_id, false});
+        std::string safety_owner = owner_id;
+        std::unordered_map<std::uint64_t, std::string>
+            preempted_normal_holders;
+        preempted_normal_holders.emplace(
+            existing->second->generation,
+            existing->second->owner_id);
+
+        auto& entry = *existing->second;
+        entry.owner_id.swap(safety_owner);
+        entry.generation = token.generation;
+        entry.deadline = std::chrono::steady_clock::time_point::max();
+        entry.preemptible = false;
+        entry.quarantined = false;
+        entry.quarantined_normal_pending = false;
+        entry.dispatch_fence_only = false;
+        entry.safety_holders.swap(safety_holders);
+        entry.preempted_normal_holders.swap(
+            preempted_normal_holders);
+        return {true, std::move(token), {}};
+    } catch (...) {
+        if (existing->second->preemptible) {
+            quarantine_(*existing->second);
+        }
+        throw;
+    }
+}
+
+bool ControlAuthorityManager::waitForPreemptedRelease(
+    const ControlLeaseToken& safety_token,
+    const Duration timeout)
+{
+    if (!safety_token.valid() || timeout < Duration::zero()) {
+        return false;
+    }
+
+    std::unique_lock lock(mutex_);
+    const auto currentState = [this, &safety_token]() {
+        const auto found = entries_.find(safety_token.resource_id);
+        if (found == entries_.end() ||
+            !isActiveSafetyHolder_(*found->second, safety_token)) {
+            return -1;
+        }
+        return !found->second->quarantined_normal_pending &&
+                       found->second->preempted_normal_holders.empty() &&
+                       found->second->in_flight_dispatches == 0U
+                   ? 1
+                   : 0;
+    };
+
+    if (currentState() < 0) {
+        return false;
+    }
+    release_cv_.wait_for(
+        lock,
+        timeout,
+        [&currentState]() { return currentState() != 0; });
+    return currentState() == 1;
+}
+
+bool ControlAuthorityManager::quarantineIfCurrent(
+    const ControlLeaseToken& expected_token) noexcept
+{
+    if (!expected_token.valid()) {
+        return false;
+    }
+    try {
+        std::lock_guard lock(mutex_);
+        const auto existing = entries_.find(expected_token.resource_id);
+        if (existing == entries_.end() ||
+            expired_(*existing->second) ||
+            !existing->second->preemptible ||
+            existing->second->owner_id != expected_token.owner_id ||
+            existing->second->generation != expected_token.generation) {
+            return false;
+        }
+        quarantine_(*existing->second);
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+bool ControlAuthorityManager::retireSafetyHolder(
+    const ControlLeaseToken& safety_token) noexcept
+{
+    if (!safety_token.valid()) {
+        return false;
+    }
+    try {
+        std::lock_guard lock(mutex_);
+        const auto existing = entries_.find(safety_token.resource_id);
+        if (existing == entries_.end() ||
+            existing->second->preemptible ||
+            existing->second->dispatch_fence_only) {
+            return false;
+        }
+        const auto holder = existing->second->safety_holders.find(
+            safety_token.generation);
+        if (holder == existing->second->safety_holders.end() ||
+            holder->second.owner_id != safety_token.owner_id) {
+            return false;
+        }
+        holder->second.retired = true;
+        release_cv_.notify_all();
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+bool ControlAuthorityManager::recoverRetiredSafetyHolders(
+    const ControlLeaseToken& recovery_token) noexcept
+{
+    if (!recovery_token.valid()) {
+        return false;
+    }
+    try {
+        std::lock_guard lock(mutex_);
+        const auto existing = entries_.find(recovery_token.resource_id);
+        if (existing == entries_.end() ||
+            !isActiveSafetyHolder_(*existing->second, recovery_token) ||
+            existing->second->quarantined_normal_pending ||
+            !existing->second->preempted_normal_holders.empty() ||
+            existing->second->in_flight_dispatches != 0U) {
+            return false;
+        }
+
+        for (auto holder = existing->second->safety_holders.begin();
+             holder != existing->second->safety_holders.end();) {
+            if (holder->second.retired) {
+                holder = existing->second->safety_holders.erase(holder);
+            } else {
+                ++holder;
+            }
+        }
+        existing->second->quarantined = false;
+        release_cv_.notify_all();
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+bool ControlAuthorityManager::renew(
+    const ControlLeaseToken& token,
+    const Duration ttl)
+{
+    if (!token.valid() || ttl <= Duration::zero()) {
+        return false;
+    }
+    std::lock_guard lock(mutex_);
+    const auto found = entries_.find(token.resource_id);
+    if (found == entries_.end()) {
+        return false;
+    }
+    if (expired_(*found->second)) {
+        if (found->second->in_flight_dispatches != 0U) {
+            invalidateToDispatchFence_(*found->second);
+        } else {
+            entries_.erase(found);
+        }
+        return false;
+    }
+    if (!found->second->preemptible) {
+        const auto holder = found->second->safety_holders.find(
+            token.generation);
+        return holder != found->second->safety_holders.end() &&
+               holder->second.owner_id == token.owner_id &&
+               !holder->second.retired;
+    }
+    if (found->second->owner_id != token.owner_id ||
+        found->second->generation != token.generation) {
+        return false;
+    }
+    found->second->deadline = std::chrono::steady_clock::now() + ttl;
+    return true;
+}
+
+bool ControlAuthorityManager::validate(
+    const ControlLeaseToken& token)
+{
+    if (!token.valid()) {
+        return false;
+    }
+    std::lock_guard lock(mutex_);
+    const auto found = entries_.find(token.resource_id);
+    if (found == entries_.end()) {
+        return false;
+    }
+    if (expired_(*found->second)) {
+        if (found->second->in_flight_dispatches != 0U) {
+            invalidateToDispatchFence_(*found->second);
+        } else {
+            entries_.erase(found);
+        }
+        return false;
+    }
+    if (!found->second->preemptible) {
+        const auto holder = found->second->safety_holders.find(
+            token.generation);
+        return holder != found->second->safety_holders.end() &&
+               holder->second.owner_id == token.owner_id &&
+               !holder->second.retired;
+    }
+    return found->second->owner_id == token.owner_id &&
+           found->second->generation == token.generation;
+}
+
+ControlDispatchGuard ControlAuthorityManager::tryBeginDispatch(
+    const ControlLeaseToken& token)
+{
+    if (!token.valid()) {
+        return {};
+    }
+    std::lock_guard lock(mutex_);
+    const auto found = entries_.find(token.resource_id);
+    if (found == entries_.end()) {
+        return {};
+    }
+    if (expired_(*found->second)) {
+        if (found->second->in_flight_dispatches != 0U) {
+            invalidateToDispatchFence_(*found->second);
+        } else {
+            entries_.erase(found);
+        }
+        return {};
+    }
+    if (!found->second->preemptible ||
+        found->second->owner_id != token.owner_id ||
+        found->second->generation != token.generation) {
+        return {};
+    }
+    ++found->second->in_flight_dispatches;
+    return ControlDispatchGuard(this, found->second);
+}
+
+void ControlAuthorityManager::release(
+    const ControlLeaseToken& token) noexcept
+{
+    if (!token.valid()) {
+        return;
+    }
+    try {
+        std::lock_guard lock(mutex_);
+        const auto found = entries_.find(token.resource_id);
+        if (found == entries_.end()) {
+            return;
+        }
+        auto& entry = *found->second;
+        if (!entry.preemptible) {
+            if (entry.dispatch_fence_only) {
+                return;
+            }
+            const auto holder = entry.safety_holders.find(token.generation);
+            if (holder != entry.safety_holders.end() &&
+                holder->second.owner_id == token.owner_id) {
+                if (holder->second.retired) {
+                    return;
+                }
+                entry.safety_holders.erase(holder);
+                if (canErase_(entry)) {
+                    entries_.erase(found);
+                }
+                release_cv_.notify_all();
+                return;
+            }
+
+            const auto preempted = entry.preempted_normal_holders.find(
+                token.generation);
+            if (preempted != entry.preempted_normal_holders.end() &&
+                preempted->second == token.owner_id) {
+                entry.preempted_normal_holders.erase(preempted);
+                if (canErase_(entry)) {
+                    entries_.erase(found);
+                }
+                release_cv_.notify_all();
+                return;
+            }
+
+            if (entry.quarantined_normal_pending &&
+                entry.owner_id == token.owner_id &&
+                entry.generation == token.generation) {
+                entry.quarantined_normal_pending = false;
+                if (canErase_(entry)) {
+                    entries_.erase(found);
+                }
+                release_cv_.notify_all();
+            }
+        } else if (entry.owner_id == token.owner_id &&
+                   entry.generation == token.generation) {
+            if (entry.in_flight_dispatches != 0U) {
+                invalidateToDispatchFence_(entry);
+            } else {
+                entries_.erase(found);
+            }
+            release_cv_.notify_all();
+        }
+    } catch (...) {
+    }
+}
+
+bool ControlAuthorityManager::isLeased(
+    const std::string& resource_id)
+{
+    if (resource_id.empty()) {
+        return false;
+    }
+    std::lock_guard lock(mutex_);
+    const auto found = entries_.find(resource_id);
+    if (found == entries_.end()) {
+        return false;
+    }
+    if (expired_(*found->second)) {
+        if (found->second->in_flight_dispatches != 0U) {
+            invalidateToDispatchFence_(*found->second);
+            return true;
+        }
+        entries_.erase(found);
+        return false;
+    }
+    return true;
+}
+
+void ControlAuthorityManager::revoke(
+    const std::string& resource_id) noexcept
+{
+    try {
+        std::lock_guard lock(mutex_);
+        const auto found = entries_.find(resource_id);
+        if (found == entries_.end()) {
+            return;
+        }
+        if (found->second->in_flight_dispatches != 0U) {
+            invalidateToDispatchFence_(*found->second);
+        } else {
+            entries_.erase(found);
+        }
+        release_cv_.notify_all();
+    } catch (...) {
+    }
+}
+
+void ControlAuthorityManager::clear() noexcept
+{
+    try {
+        std::lock_guard lock(mutex_);
+        for (auto entry = entries_.begin(); entry != entries_.end();) {
+            if (entry->second->in_flight_dispatches != 0U) {
+                invalidateToDispatchFence_(*entry->second);
+                ++entry;
+            } else {
+                entry = entries_.erase(entry);
+            }
+        }
+        release_cv_.notify_all();
+    } catch (...) {
+    }
+}
+
+bool ControlAuthorityManager::expired_(const Entry& entry) const noexcept
+{
+    return !entry.quarantined &&
+           std::chrono::steady_clock::now() >= entry.deadline;
+}
+
+bool ControlAuthorityManager::isSafetyHolder_(
+    const Entry& entry,
+    const ControlLeaseToken& token) noexcept
+{
+    if (entry.preemptible || entry.dispatch_fence_only) {
+        return false;
+    }
+    const auto holder = entry.safety_holders.find(token.generation);
+    return holder != entry.safety_holders.end() &&
+           holder->second.owner_id == token.owner_id;
+}
+
+bool ControlAuthorityManager::isActiveSafetyHolder_(
+    const Entry& entry,
+    const ControlLeaseToken& token) noexcept
+{
+    if (entry.preemptible || entry.dispatch_fence_only) {
+        return false;
+    }
+    const auto holder = entry.safety_holders.find(token.generation);
+    return holder != entry.safety_holders.end() &&
+           holder->second.owner_id == token.owner_id &&
+           !holder->second.retired;
+}
+
+bool ControlAuthorityManager::canErase_(const Entry& entry) noexcept
+{
+    if (entry.in_flight_dispatches != 0U) {
+        return false;
+    }
+    if (entry.dispatch_fence_only) {
+        return true;
+    }
+    return !entry.preemptible &&
+           entry.safety_holders.empty() &&
+           entry.preempted_normal_holders.empty() &&
+           !entry.quarantined_normal_pending &&
+           !entry.quarantined;
+}
+
+void ControlAuthorityManager::invalidateToDispatchFence_(
+    Entry& entry) noexcept
+{
+    entry.owner_id.clear();
+    entry.generation = 0U;
+    entry.deadline = std::chrono::steady_clock::time_point::max();
+    entry.preemptible = false;
+    entry.quarantined = false;
+    entry.quarantined_normal_pending = false;
+    entry.dispatch_fence_only = true;
+    entry.safety_holders.clear();
+    entry.preempted_normal_holders.clear();
+}
+
+void ControlAuthorityManager::endDispatch_(
+    const std::shared_ptr<Entry>& entry) noexcept
+{
+    try {
+        std::lock_guard lock(mutex_);
+        if (entry->in_flight_dispatches == 0U) {
+            return;
+        }
+        --entry->in_flight_dispatches;
+        const auto found = entries_.find(entry->resource_id);
+        if (found != entries_.end() && found->second == entry &&
+            canErase_(*entry)) {
+            entries_.erase(found);
+        }
+        release_cv_.notify_all();
+    } catch (...) {
+    }
+}
+
+void ControlAuthorityManager::quarantine_(Entry& entry) noexcept
+{
+    entry.quarantined_normal_pending = entry.preemptible;
+    entry.deadline = std::chrono::steady_clock::time_point::max();
+    entry.preemptible = false;
+    entry.quarantined = true;
+    entry.dispatch_fence_only = false;
+}
+
+} // namespace cmvr::control

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

@@ -0,0 +1,923 @@
+#include "manager/control_authority_manager/include/control_authority_manager.h"
+
+#include <chrono>
+#include <future>
+#include <thread>
+
+#include <gtest/gtest.h>
+
+namespace cmvr::control {
+namespace {
+
+using namespace std::chrono_literals;
+
+class ControlAuthorityManagerTest : public ::testing::Test {
+protected:
+    void SetUp() override
+    {
+        ControlAuthorityManager::instance().clear();
+    }
+
+    void TearDown() override
+    {
+        ControlAuthorityManager::instance().clear();
+    }
+};
+
+TEST_F(ControlAuthorityManagerTest, LeaseIsExclusiveAndExactReleaseRestoresAccess)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto first =
+        manager.tryAcquire("right_arm", "session-a", 100ms);
+    ASSERT_TRUE(first.acquired);
+    EXPECT_TRUE(manager.validate(first.token));
+    EXPECT_TRUE(manager.isLeased("right_arm"));
+
+    const auto conflict =
+        manager.tryAcquire("right_arm", "session-b", 100ms);
+    EXPECT_FALSE(conflict.acquired);
+
+    manager.release(first.token);
+    EXPECT_FALSE(manager.isLeased("right_arm"));
+    EXPECT_TRUE(
+        manager.tryAcquire("right_arm", "session-b", 100ms)
+            .acquired);
+}
+
+TEST_F(ControlAuthorityManagerTest, StaleGenerationCannotReleaseNewLease)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto old =
+        manager.tryAcquire("right_arm", "session-a", 100ms);
+    ASSERT_TRUE(old.acquired);
+    manager.release(old.token);
+    const auto current =
+        manager.tryAcquire("right_arm", "session-a", 100ms);
+    ASSERT_TRUE(current.acquired);
+    ASSERT_NE(
+        old.token.generation,
+        current.token.generation);
+
+    manager.release(old.token);
+    EXPECT_TRUE(manager.validate(current.token));
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       SafetyBarrierAtomicallyPreemptsControlAndRejectsOtherOwners)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 100ms);
+    ASSERT_TRUE(control.acquired);
+
+    const auto barrier = manager.preemptAcquire(
+        "right_arm", "stop-operation", 100ms);
+    ASSERT_TRUE(barrier.acquired) << barrier.detail;
+    EXPECT_FALSE(manager.validate(control.token));
+    EXPECT_TRUE(manager.validate(barrier.token));
+
+    const auto move_during_stop =
+        manager.tryAcquire("right_arm", "new-move", 100ms);
+    EXPECT_FALSE(move_during_stop.acquired);
+    const auto second_stop = manager.preemptAcquire(
+        "right_arm", "second-stop", 100ms);
+    ASSERT_TRUE(second_stop.acquired) << second_stop.detail;
+
+    manager.release(control.token);
+    EXPECT_TRUE(manager.validate(barrier.token));
+    manager.release(barrier.token);
+    EXPECT_TRUE(manager.validate(second_stop.token));
+    EXPECT_FALSE(
+        manager.tryAcquire("right_arm", "new-move", 100ms)
+            .acquired);
+    manager.release(second_stop.token);
+    EXPECT_FALSE(manager.isLeased("right_arm"));
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       SafetyPreemptionReturnsWhileDispatchIsInFlightAndWaitsForBoth)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 1s);
+    ASSERT_TRUE(control.acquired);
+
+    auto dispatch = manager.tryBeginDispatch(control.token);
+    ASSERT_TRUE(dispatch.acquired());
+    auto pending_barrier = std::async(
+        std::launch::async,
+        [&manager]() {
+            return manager.preemptAcquire(
+                "right_arm", "stop-operation", 1s);
+        });
+
+    const auto preempt_status = pending_barrier.wait_for(100ms);
+    if (preempt_status != std::future_status::ready) {
+        dispatch = {};
+        const auto cleanup_barrier = pending_barrier.get();
+        if (cleanup_barrier.acquired) {
+            manager.release(cleanup_barrier.token);
+        }
+        FAIL() << "safety preemption waited for an in-flight dispatch";
+        return;
+    }
+
+    const auto acquired_barrier = pending_barrier.get();
+    ASSERT_TRUE(acquired_barrier.acquired)
+        << acquired_barrier.detail;
+    EXPECT_FALSE(manager.validate(control.token));
+    EXPECT_FALSE(manager.tryBeginDispatch(control.token).acquired());
+
+    EXPECT_FALSE(manager.waitForPreemptedRelease(
+        acquired_barrier.token, 10ms));
+    manager.release(control.token);
+    EXPECT_FALSE(manager.waitForPreemptedRelease(
+        acquired_barrier.token, 10ms));
+
+    dispatch = {};
+    EXPECT_TRUE(manager.waitForPreemptedRelease(
+        acquired_barrier.token, 10ms));
+    manager.release(acquired_barrier.token);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       DispatchOnOneResourceDoesNotDelaySafetyPreemptionOnAnother)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto right_control =
+        manager.tryAcquire("right_arm", "right-move", 1s);
+    const auto left_control =
+        manager.tryAcquire("left_arm", "left-move", 1s);
+    ASSERT_TRUE(right_control.acquired);
+    ASSERT_TRUE(left_control.acquired);
+
+    auto right_dispatch = manager.tryBeginDispatch(right_control.token);
+    ASSERT_TRUE(right_dispatch.acquired());
+    auto pending_left_barrier = std::async(
+        std::launch::async,
+        [&manager]() {
+            return manager.preemptAcquire(
+                "left_arm", "left-stop", 1s);
+        });
+
+    const auto preempt_status = pending_left_barrier.wait_for(100ms);
+    if (preempt_status != std::future_status::ready) {
+        right_dispatch = {};
+        const auto cleanup_barrier = pending_left_barrier.get();
+        if (cleanup_barrier.acquired) {
+            manager.release(cleanup_barrier.token);
+        }
+        FAIL() << "one resource's dispatch blocked another resource's stop";
+        return;
+    }
+
+    const auto left_barrier = pending_left_barrier.get();
+    ASSERT_TRUE(left_barrier.acquired) << left_barrier.detail;
+    manager.release(left_control.token);
+    EXPECT_TRUE(manager.waitForPreemptedRelease(
+        left_barrier.token, 0ms));
+    manager.release(left_barrier.token);
+
+    EXPECT_TRUE(right_dispatch.acquired());
+    right_dispatch = {};
+    manager.release(right_control.token);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       DispatchGuardDestructorReleasesTheInFlightFence)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 1s);
+    ASSERT_TRUE(control.acquired);
+
+    ControlAcquireResult barrier;
+    {
+        auto dispatch = manager.tryBeginDispatch(control.token);
+        ASSERT_TRUE(dispatch.acquired());
+        barrier = manager.preemptAcquire(
+            "right_arm", "stop-operation", 1s);
+        ASSERT_TRUE(barrier.acquired) << barrier.detail;
+        manager.release(control.token);
+        EXPECT_FALSE(manager.waitForPreemptedRelease(
+            barrier.token, 0ms));
+    }
+
+    EXPECT_TRUE(manager.waitForPreemptedRelease(barrier.token, 0ms));
+    manager.release(barrier.token);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       DispatchGuardMoveAssignmentReleasesOnlyItsPreviousFence)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto right_control =
+        manager.tryAcquire("right_arm", "right-move", 1s);
+    const auto left_control =
+        manager.tryAcquire("left_arm", "left-move", 1s);
+    ASSERT_TRUE(right_control.acquired);
+    ASSERT_TRUE(left_control.acquired);
+
+    auto right_dispatch = manager.tryBeginDispatch(right_control.token);
+    auto left_dispatch = manager.tryBeginDispatch(left_control.token);
+    ASSERT_TRUE(right_dispatch.acquired());
+    ASSERT_TRUE(left_dispatch.acquired());
+    const auto right_barrier = manager.preemptAcquire(
+        "right_arm", "right-stop", 1s);
+    const auto left_barrier = manager.preemptAcquire(
+        "left_arm", "left-stop", 1s);
+    ASSERT_TRUE(right_barrier.acquired) << right_barrier.detail;
+    ASSERT_TRUE(left_barrier.acquired) << left_barrier.detail;
+    manager.release(right_control.token);
+    manager.release(left_control.token);
+
+    right_dispatch = std::move(left_dispatch);
+    EXPECT_TRUE(right_dispatch.acquired());
+    EXPECT_FALSE(left_dispatch.acquired());
+    EXPECT_TRUE(manager.waitForPreemptedRelease(
+        right_barrier.token, 0ms));
+    EXPECT_FALSE(manager.waitForPreemptedRelease(
+        left_barrier.token, 0ms));
+
+    right_dispatch = {};
+    EXPECT_TRUE(manager.waitForPreemptedRelease(
+        left_barrier.token, 0ms));
+    manager.release(right_barrier.token);
+    manager.release(left_barrier.token);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       RevokeKeepsAnInFlightDispatchFencedFromNormalSuccessors)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 1s);
+    ASSERT_TRUE(control.acquired);
+    auto dispatch = manager.tryBeginDispatch(control.token);
+    ASSERT_TRUE(dispatch.acquired());
+
+    manager.revoke("right_arm");
+    EXPECT_FALSE(manager.validate(control.token));
+    EXPECT_TRUE(manager.isLeased("right_arm"));
+    EXPECT_FALSE(
+        manager.tryAcquire("right_arm", "successor", 1s).acquired);
+    manager.release(control.token);
+    EXPECT_FALSE(
+        manager.tryAcquire("right_arm", "successor", 1s).acquired);
+
+    dispatch = {};
+    const auto successor =
+        manager.tryAcquire("right_arm", "successor", 1s);
+    ASSERT_TRUE(successor.acquired) << successor.detail;
+    manager.release(successor.token);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       ClearKeepsInFlightDispatchesWhileErasingIdleEntries)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto active =
+        manager.tryAcquire("right_arm", "move-session", 1s);
+    ASSERT_TRUE(active.acquired);
+    ASSERT_TRUE(manager.tryAcquire("idle_arm", "idle-session", 1s).acquired);
+    auto dispatch = manager.tryBeginDispatch(active.token);
+    ASSERT_TRUE(dispatch.acquired());
+
+    manager.clear();
+    EXPECT_FALSE(
+        manager.tryAcquire("right_arm", "successor", 1s).acquired);
+    const auto idle_successor =
+        manager.tryAcquire("idle_arm", "idle-successor", 1s);
+    ASSERT_TRUE(idle_successor.acquired) << idle_successor.detail;
+    manager.release(idle_successor.token);
+
+    dispatch = {};
+    const auto active_successor =
+        manager.tryAcquire("right_arm", "successor", 1s);
+    ASSERT_TRUE(active_successor.acquired) << active_successor.detail;
+    manager.release(active_successor.token);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       ExpiredLeaseKeepsInFlightDispatchFencedFromNormalSuccessors)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 10ms);
+    ASSERT_TRUE(control.acquired);
+    auto dispatch = manager.tryBeginDispatch(control.token);
+    ASSERT_TRUE(dispatch.acquired());
+    std::this_thread::sleep_for(20ms);
+
+    EXPECT_FALSE(manager.validate(control.token));
+    EXPECT_FALSE(
+        manager.tryAcquire("right_arm", "successor", 1s).acquired);
+    manager.release(control.token);
+    EXPECT_FALSE(
+        manager.tryAcquire("right_arm", "successor", 1s).acquired);
+
+    dispatch = {};
+    const auto successor =
+        manager.tryAcquire("right_arm", "successor", 1s);
+    ASSERT_TRUE(successor.acquired) << successor.detail;
+    manager.release(successor.token);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       DispatchGuardSurvivesAuthorityMapRehash)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 1s);
+    ASSERT_TRUE(control.acquired);
+    auto dispatch = manager.tryBeginDispatch(control.token);
+    ASSERT_TRUE(dispatch.acquired());
+
+    for (int index = 0; index < 512; ++index) {
+        ASSERT_TRUE(manager.tryAcquire(
+            "rehash-resource-" + std::to_string(index),
+            "rehash-owner",
+            1s).acquired);
+    }
+
+    const auto barrier = manager.preemptAcquire(
+        "right_arm", "stop-operation", 1s);
+    ASSERT_TRUE(barrier.acquired) << barrier.detail;
+    manager.release(control.token);
+    EXPECT_FALSE(manager.waitForPreemptedRelease(barrier.token, 0ms));
+    dispatch = {};
+    EXPECT_TRUE(manager.waitForPreemptedRelease(barrier.token, 0ms));
+    manager.release(barrier.token);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       StaleLeaseCannotBeginDispatchAfterSafetyPreemption)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 1s);
+    ASSERT_TRUE(control.acquired);
+    const auto barrier = manager.preemptAcquire(
+        "right_arm", "stop-operation", 1s);
+    ASSERT_TRUE(barrier.acquired) << barrier.detail;
+
+    EXPECT_FALSE(manager.tryBeginDispatch(control.token).acquired());
+    manager.release(barrier.token);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       ReleasedLastSafetyBarrierKeepsPreemptedHandlerFenced)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 1s);
+    ASSERT_TRUE(control.acquired);
+    const auto barrier = manager.preemptAcquire(
+        "right_arm", "stop-operation", 1s);
+    ASSERT_TRUE(barrier.acquired) << barrier.detail;
+
+    manager.release(barrier.token);
+    EXPECT_TRUE(manager.isLeased("right_arm"));
+    EXPECT_FALSE(
+        manager.tryAcquire("right_arm", "new-move", 1s).acquired);
+
+    manager.release(control.token);
+    EXPECT_FALSE(manager.isLeased("right_arm"));
+    EXPECT_TRUE(
+        manager.tryAcquire("right_arm", "new-move", 1s).acquired);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       SafetyBarrierWaitsForPreemptedNormalLeaseRelease)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 1s);
+    ASSERT_TRUE(control.acquired);
+    const auto barrier = manager.preemptAcquire(
+        "right_arm", "stop-operation", 1s);
+    ASSERT_TRUE(barrier.acquired) << barrier.detail;
+
+    auto wait_result = std::async(
+        std::launch::async,
+        [&manager, token = barrier.token]() {
+            return manager.waitForPreemptedRelease(token, 1s);
+        });
+    EXPECT_EQ(
+        wait_result.wait_for(30ms),
+        std::future_status::timeout);
+
+    manager.release(control.token);
+    EXPECT_TRUE(wait_result.get());
+    EXPECT_TRUE(manager.validate(barrier.token));
+    manager.release(barrier.token);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       ConditionalSafetyBarrierTracksPreemptedNormalLeaseRelease)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 1s);
+    ASSERT_TRUE(control.acquired);
+    auto dispatch = manager.tryBeginDispatch(control.token);
+    ASSERT_TRUE(dispatch.acquired());
+    const auto barrier = manager.preemptAcquireIfCurrent(
+        control.token, "timed-out-action", 1s);
+    ASSERT_TRUE(barrier.acquired) << barrier.detail;
+
+    EXPECT_FALSE(
+        manager.waitForPreemptedRelease(barrier.token, 10ms));
+    dispatch = {};
+    EXPECT_FALSE(
+        manager.waitForPreemptedRelease(barrier.token, 10ms));
+    manager.release(control.token);
+    EXPECT_TRUE(
+        manager.waitForPreemptedRelease(barrier.token, 10ms));
+    manager.release(barrier.token);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       ReleasedSafetyHolderStopsWaitingWithoutAffectingOtherHolder)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 1s);
+    ASSERT_TRUE(control.acquired);
+    const auto first_barrier = manager.preemptAcquire(
+        "right_arm", "first-stop", 1s);
+    ASSERT_TRUE(first_barrier.acquired) << first_barrier.detail;
+    const auto second_barrier = manager.preemptAcquire(
+        "right_arm", "second-stop", 1s);
+    ASSERT_TRUE(second_barrier.acquired) << second_barrier.detail;
+
+    auto first_wait = std::async(
+        std::launch::async,
+        [&manager, token = first_barrier.token]() {
+            return manager.waitForPreemptedRelease(token, 1s);
+        });
+    auto second_wait = std::async(
+        std::launch::async,
+        [&manager, token = second_barrier.token]() {
+            return manager.waitForPreemptedRelease(token, 1s);
+        });
+    EXPECT_EQ(first_wait.wait_for(30ms), std::future_status::timeout);
+    EXPECT_EQ(second_wait.wait_for(30ms), std::future_status::timeout);
+
+    manager.release(first_barrier.token);
+    EXPECT_FALSE(first_wait.get());
+    EXPECT_EQ(second_wait.wait_for(30ms), std::future_status::timeout);
+    manager.release(control.token);
+    EXPECT_TRUE(second_wait.get());
+    manager.release(second_barrier.token);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       PreemptedReleaseRequiresExactNormalLeaseToken)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 1s);
+    ASSERT_TRUE(control.acquired);
+    const auto barrier = manager.preemptAcquire(
+        "right_arm", "stop-operation", 1s);
+    ASSERT_TRUE(barrier.acquired) << barrier.detail;
+
+    auto forged = control.token;
+    forged.owner_id = "different-owner";
+    manager.release(forged);
+    EXPECT_FALSE(
+        manager.waitForPreemptedRelease(barrier.token, 10ms));
+
+    manager.release(control.token);
+    EXPECT_TRUE(
+        manager.waitForPreemptedRelease(barrier.token, 0ms));
+    manager.release(barrier.token);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       ClearWakesPreemptedReleaseWaiterAndInvalidatesSafetyHolder)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 1s);
+    ASSERT_TRUE(control.acquired);
+    const auto barrier = manager.preemptAcquire(
+        "right_arm", "stop-operation", 1s);
+    ASSERT_TRUE(barrier.acquired) << barrier.detail;
+
+    auto wait_result = std::async(
+        std::launch::async,
+        [&manager, token = barrier.token]() {
+            return manager.waitForPreemptedRelease(token, 1s);
+        });
+    EXPECT_EQ(
+        wait_result.wait_for(30ms),
+        std::future_status::timeout);
+
+    manager.clear();
+    EXPECT_FALSE(wait_result.get());
+    manager.release(control.token);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       RevokeWakesPreemptedReleaseWaiterAndInvalidatesSafetyHolder)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 1s);
+    ASSERT_TRUE(control.acquired);
+    const auto barrier = manager.preemptAcquire(
+        "right_arm", "stop-operation", 1s);
+    ASSERT_TRUE(barrier.acquired) << barrier.detail;
+
+    auto wait_result = std::async(
+        std::launch::async,
+        [&manager, token = barrier.token]() {
+            return manager.waitForPreemptedRelease(token, 1s);
+        });
+    EXPECT_EQ(
+        wait_result.wait_for(30ms),
+        std::future_status::timeout);
+
+    manager.revoke("right_arm");
+    EXPECT_FALSE(wait_result.get());
+    manager.release(control.token);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       ExpiredNormalLeaseStillRequiresHandlerReleaseAfterPreemption)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 10ms);
+    ASSERT_TRUE(control.acquired);
+    std::this_thread::sleep_for(20ms);
+
+    const auto barrier = manager.preemptAcquire(
+        "right_arm", "stop-operation", 1s);
+    ASSERT_TRUE(barrier.acquired) << barrier.detail;
+    EXPECT_FALSE(
+        manager.waitForPreemptedRelease(barrier.token, 10ms));
+
+    manager.release(control.token);
+    EXPECT_TRUE(
+        manager.waitForPreemptedRelease(barrier.token, 0ms));
+    manager.release(barrier.token);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       QuarantinedFallbackTracksOriginalNormalHandlerRelease)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 1s);
+    ASSERT_TRUE(control.acquired);
+    ASSERT_TRUE(manager.quarantineIfCurrent(control.token));
+
+    const auto barrier = manager.preemptAcquire(
+        "right_arm", "stop-operation", 1s);
+    ASSERT_TRUE(barrier.acquired) << barrier.detail;
+    EXPECT_FALSE(
+        manager.waitForPreemptedRelease(barrier.token, 10ms));
+
+    manager.release(control.token);
+    EXPECT_TRUE(
+        manager.waitForPreemptedRelease(barrier.token, 0ms));
+    manager.release(barrier.token);
+    EXPECT_TRUE(manager.isLeased("right_arm"));
+
+    manager.revoke("right_arm");
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       WaitRejectsStaleSafetyTokenForSuccessorEntry)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto old_control =
+        manager.tryAcquire("right_arm", "old-move", 1s);
+    ASSERT_TRUE(old_control.acquired);
+    const auto old_barrier = manager.preemptAcquire(
+        "right_arm", "old-stop", 1s);
+    ASSERT_TRUE(old_barrier.acquired) << old_barrier.detail;
+    manager.release(old_barrier.token);
+
+    EXPECT_FALSE(
+        manager.tryAcquire("right_arm", "new-move", 1s).acquired);
+    manager.release(old_control.token);
+
+    const auto successor =
+        manager.tryAcquire("right_arm", "new-move", 1s);
+    ASSERT_TRUE(successor.acquired);
+    const auto current_barrier = manager.preemptAcquire(
+        "right_arm", "new-stop", 1s);
+    ASSERT_TRUE(current_barrier.acquired) << current_barrier.detail;
+
+    EXPECT_FALSE(manager.waitForPreemptedRelease(
+        old_barrier.token, 0ms));
+    EXPECT_FALSE(manager.waitForPreemptedRelease(
+        current_barrier.token, 10ms));
+
+    manager.release(successor.token);
+    EXPECT_TRUE(manager.waitForPreemptedRelease(
+        current_barrier.token, 0ms));
+    manager.release(current_barrier.token);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       ConditionalSafetyBarrierPreemptsMatchingCurrentLease)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 100ms);
+    ASSERT_TRUE(control.acquired);
+
+    const auto barrier = manager.preemptAcquireIfCurrent(
+        control.token, "timed-out-action", 100ms);
+    ASSERT_TRUE(barrier.acquired) << barrier.detail;
+    EXPECT_FALSE(manager.validate(control.token));
+    EXPECT_TRUE(manager.validate(barrier.token));
+    manager.release(control.token);
+    EXPECT_TRUE(manager.validate(barrier.token));
+    EXPECT_FALSE(
+        manager.tryAcquire("right_arm", "new-move", 100ms)
+            .acquired);
+
+    manager.release(barrier.token);
+    EXPECT_FALSE(manager.isLeased("right_arm"));
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       ConditionalSafetyBarrierDoesNotPreemptSuccessorForStaleToken)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto old =
+        manager.tryAcquire("right_arm", "move-session", 100ms);
+    ASSERT_TRUE(old.acquired);
+    const auto direct_stop = manager.preemptAcquire(
+        "right_arm", "direct-stop", 100ms);
+    ASSERT_TRUE(direct_stop.acquired) << direct_stop.detail;
+    manager.release(direct_stop.token);
+    EXPECT_FALSE(
+        manager.tryAcquire("right_arm", "move-session", 100ms).acquired);
+    manager.release(old.token);
+    const auto successor =
+        manager.tryAcquire("right_arm", "move-session", 100ms);
+    ASSERT_TRUE(successor.acquired);
+    ASSERT_NE(old.token.generation, successor.token.generation);
+
+    const auto barrier = manager.preemptAcquireIfCurrent(
+        old.token, "delayed-stop", 100ms);
+    EXPECT_FALSE(barrier.acquired);
+    EXPECT_FALSE(barrier.token.valid());
+    EXPECT_TRUE(manager.validate(successor.token));
+    EXPECT_FALSE(
+        manager.tryAcquire("right_arm", "competing-move", 100ms)
+            .acquired);
+
+    manager.release(successor.token);
+    EXPECT_FALSE(manager.isLeased("right_arm"));
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       ConditionalSafetyBarrierDoesNotJoinExistingSafetyBarrier)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 100ms);
+    ASSERT_TRUE(control.acquired);
+    const auto existing_barrier = manager.preemptAcquire(
+        "right_arm", "direct-stop", 100ms);
+    ASSERT_TRUE(existing_barrier.acquired) << existing_barrier.detail;
+
+    const auto delayed_barrier = manager.preemptAcquireIfCurrent(
+        control.token, "delayed-action-stop", 100ms);
+    EXPECT_FALSE(delayed_barrier.acquired);
+    EXPECT_FALSE(delayed_barrier.token.valid());
+    EXPECT_TRUE(manager.validate(existing_barrier.token));
+
+    manager.release(existing_barrier.token);
+    EXPECT_TRUE(manager.isLeased("right_arm"));
+    manager.release(control.token);
+    EXPECT_FALSE(manager.isLeased("right_arm"));
+    EXPECT_TRUE(
+        manager.tryAcquire("right_arm", "new-move", 100ms)
+            .acquired);
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       QuarantineSurvivesNormalAndTemporarySafetyTokenRelease)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 20ms);
+    ASSERT_TRUE(control.acquired);
+
+    ASSERT_TRUE(manager.quarantineIfCurrent(control.token));
+    EXPECT_FALSE(manager.validate(control.token));
+    EXPECT_TRUE(manager.isLeased("right_arm"));
+
+    manager.release(control.token);
+    std::this_thread::sleep_for(30ms);
+    EXPECT_TRUE(manager.isLeased("right_arm"));
+    EXPECT_FALSE(
+        manager.tryAcquire("right_arm", "new-move", 100ms)
+            .acquired);
+
+    const auto temporary_stop = manager.preemptAcquire(
+        "right_arm", "temporary-stop", 100ms);
+    ASSERT_TRUE(temporary_stop.acquired) << temporary_stop.detail;
+    EXPECT_TRUE(manager.validate(temporary_stop.token));
+    manager.release(temporary_stop.token);
+
+    EXPECT_TRUE(manager.isLeased("right_arm"));
+    EXPECT_FALSE(
+        manager.tryAcquire("right_arm", "new-move", 100ms)
+            .acquired);
+
+    manager.revoke("right_arm");
+    EXPECT_FALSE(manager.isLeased("right_arm"));
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       QuarantineWithStaleTokenDoesNotAffectSuccessor)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto old =
+        manager.tryAcquire("right_arm", "move-session", 100ms);
+    ASSERT_TRUE(old.acquired);
+    manager.release(old.token);
+    const auto successor =
+        manager.tryAcquire("right_arm", "move-session", 100ms);
+    ASSERT_TRUE(successor.acquired);
+    ASSERT_NE(old.token.generation, successor.token.generation);
+
+    EXPECT_FALSE(manager.quarantineIfCurrent(old.token));
+    EXPECT_TRUE(manager.validate(successor.token));
+    EXPECT_FALSE(
+        manager.tryAcquire("right_arm", "competing-move", 100ms)
+            .acquired);
+
+    manager.release(successor.token);
+    EXPECT_FALSE(manager.isLeased("right_arm"));
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       RetiredSafetyHolderStaysFailClosedUntilConfirmedRecovery)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 1s);
+    ASSERT_TRUE(control.acquired);
+    const auto failed_stop = manager.preemptAcquire(
+        "right_arm", "failed-stop", 1s);
+    ASSERT_TRUE(failed_stop.acquired) << failed_stop.detail;
+
+    manager.release(control.token);
+    ASSERT_TRUE(manager.waitForPreemptedRelease(
+        failed_stop.token, 0ms));
+    ASSERT_TRUE(manager.retireSafetyHolder(failed_stop.token));
+    EXPECT_FALSE(manager.validate(failed_stop.token));
+
+    // A delayed destructor release cannot undo the retained fail-closed state.
+    manager.release(failed_stop.token);
+    EXPECT_TRUE(manager.isLeased("right_arm"));
+    EXPECT_FALSE(
+        manager.tryAcquire("right_arm", "new-move", 1s).acquired);
+
+    const auto recovery = manager.preemptAcquire(
+        "right_arm", "confirmed-recovery", 1s);
+    ASSERT_TRUE(recovery.acquired) << recovery.detail;
+    ASSERT_TRUE(manager.waitForPreemptedRelease(recovery.token, 0ms));
+    ASSERT_TRUE(manager.recoverRetiredSafetyHolders(recovery.token));
+    EXPECT_TRUE(manager.validate(recovery.token));
+
+    manager.release(recovery.token);
+    EXPECT_FALSE(manager.isLeased("right_arm"));
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       RecoveryWaitsForPreemptedNormalHandlerToRelease)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 1s);
+    ASSERT_TRUE(control.acquired);
+    const auto failed_stop = manager.preemptAcquire(
+        "right_arm", "failed-stop", 1s);
+    ASSERT_TRUE(failed_stop.acquired) << failed_stop.detail;
+    ASSERT_TRUE(manager.retireSafetyHolder(failed_stop.token));
+
+    const auto recovery = manager.preemptAcquire(
+        "right_arm", "confirmed-recovery", 1s);
+    ASSERT_TRUE(recovery.acquired) << recovery.detail;
+    EXPECT_FALSE(manager.recoverRetiredSafetyHolders(recovery.token));
+    EXPECT_FALSE(manager.waitForPreemptedRelease(recovery.token, 10ms));
+
+    manager.release(control.token);
+    ASSERT_TRUE(manager.waitForPreemptedRelease(recovery.token, 0ms));
+    ASSERT_TRUE(manager.recoverRetiredSafetyHolders(recovery.token));
+    manager.release(recovery.token);
+    EXPECT_FALSE(manager.isLeased("right_arm"));
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       RecoveryPreservesEveryOtherActiveSafetyHolder)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto retired = manager.preemptAcquire(
+        "right_arm", "failed-stop", 1s);
+    ASSERT_TRUE(retired.acquired) << retired.detail;
+    ASSERT_TRUE(manager.retireSafetyHolder(retired.token));
+
+    const auto independent_stop = manager.preemptAcquire(
+        "right_arm", "independent-stop", 1s);
+    const auto recovery = manager.preemptAcquire(
+        "right_arm", "confirmed-recovery", 1s);
+    ASSERT_TRUE(independent_stop.acquired) << independent_stop.detail;
+    ASSERT_TRUE(recovery.acquired) << recovery.detail;
+
+    ASSERT_TRUE(manager.recoverRetiredSafetyHolders(recovery.token));
+    EXPECT_TRUE(manager.validate(independent_stop.token));
+    EXPECT_TRUE(manager.validate(recovery.token));
+    EXPECT_FALSE(manager.validate(retired.token));
+
+    manager.release(recovery.token);
+    EXPECT_TRUE(manager.isLeased("right_arm"));
+    EXPECT_TRUE(manager.validate(independent_stop.token));
+    manager.release(independent_stop.token);
+    EXPECT_FALSE(manager.isLeased("right_arm"));
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       RetiredOrForgedSafetyTokenCannotAuthorizeRecovery)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto retired = manager.preemptAcquire(
+        "right_arm", "failed-stop", 1s);
+    ASSERT_TRUE(retired.acquired) << retired.detail;
+    ASSERT_TRUE(manager.retireSafetyHolder(retired.token));
+    EXPECT_FALSE(manager.recoverRetiredSafetyHolders(retired.token));
+
+    const auto recovery = manager.preemptAcquire(
+        "right_arm", "confirmed-recovery", 1s);
+    ASSERT_TRUE(recovery.acquired) << recovery.detail;
+    auto forged = recovery.token;
+    forged.owner_id = "different-owner";
+    EXPECT_FALSE(manager.recoverRetiredSafetyHolders(forged));
+    EXPECT_TRUE(manager.isLeased("right_arm"));
+
+    ASSERT_TRUE(manager.recoverRetiredSafetyHolders(recovery.token));
+    manager.release(recovery.token);
+    EXPECT_FALSE(manager.isLeased("right_arm"));
+}
+
+TEST_F(ControlAuthorityManagerTest,
+       ConfirmedRecoveryCanClearTokenlessQuarantine)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 1s);
+    ASSERT_TRUE(control.acquired);
+    ASSERT_TRUE(manager.quarantineIfCurrent(control.token));
+
+    const auto recovery = manager.preemptAcquire(
+        "right_arm", "confirmed-recovery", 1s);
+    ASSERT_TRUE(recovery.acquired) << recovery.detail;
+    EXPECT_FALSE(manager.recoverRetiredSafetyHolders(recovery.token));
+
+    manager.release(control.token);
+    ASSERT_TRUE(manager.waitForPreemptedRelease(recovery.token, 0ms));
+    ASSERT_TRUE(manager.recoverRetiredSafetyHolders(recovery.token));
+    manager.release(recovery.token);
+    EXPECT_FALSE(manager.isLeased("right_arm"));
+}
+
+TEST_F(ControlAuthorityManagerTest, ExpiryAndRenewUseMonotonicLocalTime)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto lease =
+        manager.tryAcquire("right_arm", "session-a", 20ms);
+    ASSERT_TRUE(lease.acquired);
+    std::this_thread::sleep_for(10ms);
+    ASSERT_TRUE(manager.renew(lease.token, 30ms));
+    std::this_thread::sleep_for(20ms);
+    EXPECT_TRUE(manager.validate(lease.token));
+    std::this_thread::sleep_for(20ms);
+    EXPECT_FALSE(manager.validate(lease.token));
+    EXPECT_FALSE(manager.isLeased("right_arm"));
+}
+
+TEST_F(ControlAuthorityManagerTest, DifferentArmsCanBeLeasedIndependently)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    EXPECT_TRUE(
+        manager.tryAcquire("right_arm", "session-a", 100ms)
+            .acquired);
+    EXPECT_TRUE(
+        manager.tryAcquire("left_arm", "session-b", 100ms)
+            .acquired);
+}
+
+} // namespace
+} // namespace cmvr::control

cmvr-es/manager/device_manager/CMakeLists.txt

@@ -1,5 +1,6 @@
 add_library(device_manager STATIC
         src/device_factory.cpp
+        src/device_safety_adapters.cpp
         src/device_manager.cpp
 )
 
@@ -7,6 +8,7 @@ target_include_directories(device_manager PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 target_link_libraries(device_manager PRIVATE
         cmvr_es::proto
+        cmvr_es::safety_manager
         cmvr_es::device::camera
         cmvr_es::device::agv
         cmvr_es::device::speaker

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;
@@ -36,8 +43,20 @@ namespace cmvr::device {
         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;
@@ -55,6 +74,7 @@ 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);
@@ -67,6 +87,13 @@ namespace cmvr::device {
         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,6 +4,7 @@
 //
 
 #include "../include/device_manager.h"
+#include "../include/device_safety_adapters.h"
 
 #include <algorithm>
 #include <chrono>
@@ -35,6 +36,68 @@ 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
 {
@@ -170,10 +233,12 @@ 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_();
@@ -248,6 +313,7 @@ bool DeviceManager::start(){
         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;
@@ -264,6 +330,8 @@ bool DeviceManager::start(){
                             << " 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 {
@@ -273,13 +341,29 @@ bool DeviceManager::start(){
             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]: At least one enabled device failed "
-                           "to start; stopping all devices";
+        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;
 }
@@ -338,12 +422,18 @@ void DeviceManager::stop_devices_(const bool 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});
         }
     }
 }
@@ -377,6 +467,24 @@ 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_);
@@ -427,6 +535,16 @@ void DeviceManager::registerDevice(const std::string& device_id,
         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()
@@ -491,48 +609,43 @@ void DeviceManager::update_device_status_(
     const ManagedDeviceState state,
     const std::string& error_message)
 {
-    std::unique_lock lock(devices_mutex_);
-    auto& status = device_statuses_[device_id];
-    if (status.id.empty()) {
-        status.id = device_id;
+    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;
     }
-    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();
+    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(device_statuses_.size());
         for (const auto& [id, stored_status] : device_statuses_) {
-            SnapshotSource source;
-            source.status = stored_status;
-            const auto device_it = devices_.find(id);
-            if (device_it != devices_.end()) {
-                source.device = device_it->second.device;
-            }
-            sources.push_back(std::move(source));
+            (void)id;
+            sources.push_back(stored_status);
         }
     }
 
@@ -543,36 +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.status.health.error_message =
-            truncateDeviceError(source.status.health.error_message);
+        source.health.error_message =
+            truncateDeviceError(source.health.error_message);
         const bool lifecycle_error =
-            source.status.state == ManagedDeviceState::Error;
+            source.state == ManagedDeviceState::Error;
         const bool health_error =
-            source.status.health.state == DeviceHealthState::Degraded ||
-            source.status.health.state == DeviceHealthState::Fault;
-        source.status.abnormal = lifecycle_error || health_error;
-        if (source.status.error_message.empty()) {
-            source.status.error_message =
-                source.status.health.error_message;
+            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.status.error_message =
-            truncateDeviceError(source.status.error_message);
-        if (source.status.status_updated_at_unix_ms == 0) {
-            source.status.status_updated_at_unix_ms = unixTimeMs();
+        source.error_message = truncateDeviceError(source.error_message);
+        if (source.status_updated_at_unix_ms == 0) {
+            source.status_updated_at_unix_ms = unixTimeMs();
         }
-        result.devices.push_back(std::move(source.status));
+        result.devices.push_back(std::move(source));
     }
 
     std::sort(result.devices.begin(), result.devices.end(),
@@ -583,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();
 }
@@ -896,6 +1067,8 @@ bool DeviceManager::init_devices_() {
                        << ", type=" << record.type_name
                        << ", kind=" << toString(record.kind)
                        << ", config_file=" << entry.config_file();
+        const auto registered_device = record.device;
+        std::string registered_id;
         {
             std::unique_lock lock(devices_mutex_);
             const auto id = record.id;
@@ -923,6 +1096,23 @@ bool DeviceManager::init_devices_() {
             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;

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

@@ -121,4 +121,33 @@ TEST_F(DeviceManagerLifecycleTest,
     EXPECT_EQ(device->stop_calls, 1);
 }
 
+TEST_F(DeviceManagerLifecycleTest,
+       EnforceSelectedCannotStartWithMissingConfiguredTarget)
+{
+    cmvr::config::DeviceManagerConfig config;
+    auto* safety = config.mutable_safety();
+    safety->set_mode(
+        cmvr::config::SafetyManagerConfig::ENFORCE_SELECTED);
+    safety->add_enforced_device_ids("missing-arm");
+
+    auto& manager = cmvr::device::DeviceManager::getInstance(config);
+    ASSERT_TRUE(manager.initialized());
+    EXPECT_FALSE(manager.start());
+    EXPECT_EQ(
+        manager.safetyManager().snapshot().system_state,
+        cmvr::safety::SystemAdmissionState::Starting);
+}
+
+TEST_F(DeviceManagerLifecycleTest,
+       EnforceSelectedCannotSilentlyCoverNoDevices)
+{
+    cmvr::config::DeviceManagerConfig config;
+    config.mutable_safety()->set_mode(
+        cmvr::config::SafetyManagerConfig::ENFORCE_SELECTED);
+
+    auto& manager = cmvr::device::DeviceManager::getInstance(config);
+    ASSERT_TRUE(manager.initialized());
+    EXPECT_FALSE(manager.start());
+}
+
 } // namespace

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;
@@ -374,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()
@@ -382,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(device_media_source_adapter STATIC
-            src/device_media_source_adapter.cpp
-    )
-    target_compile_features(device_media_source_adapter PUBLIC cxx_std_17)
-    target_include_directories(device_media_source_adapter
-            PUBLIC
-            ${CMAKE_CURRENT_SOURCE_DIR}/../..
-    )
-    target_link_libraries(device_media_source_adapter
-            PUBLIC
-            cmvr_es::media_source_hub
-            cmvr_es::common
-            cmvr_es::proto
-            cmvr_es::logging
-    )
-    add_library(cmvr_es::device_media_source_adapter ALIAS device_media_source_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/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/media_source_manager/CMakeLists.txt

@@ -0,0 +1,97 @@
+if(CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR)
+    cmake_minimum_required(VERSION 3.22)
+    project(cmvr_media_source_manager LANGUAGES CXX)
+    enable_testing()
+    add_subdirectory(
+            ${CMAKE_CURRENT_SOURCE_DIR}/../../service/grpc/stop_all
+            ${CMAKE_CURRENT_BINARY_DIR}/stop_all
+    )
+endif()
+
+add_library(media_source_manager STATIC
+        src/media_source_manager.cpp
+)
+
+target_compile_features(media_source_manager PUBLIC cxx_std_17)
+target_include_directories(media_source_manager
+        PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}/../..
+)
+target_link_libraries(media_source_manager
+        PUBLIC
+        cmvr_es::stop_all_admission_gate
+)
+
+add_library(cmvr_es::media_source_manager ALIAS media_source_manager)
+
+if(NOT CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR)
+    add_library(device_media_source_adapter STATIC
+            src/device_media_source_adapter.cpp
+    )
+    target_compile_features(device_media_source_adapter PUBLIC cxx_std_17)
+    target_include_directories(device_media_source_adapter
+            PUBLIC
+            ${CMAKE_CURRENT_SOURCE_DIR}/../..
+    )
+    target_link_libraries(device_media_source_adapter
+            PUBLIC
+            cmvr_es::media_source_manager
+            cmvr_es::common
+            cmvr_es::proto
+            cmvr_es::logging
+            cmvr_es::safety_manager
+    )
+    add_library(cmvr_es::device_media_source_adapter ALIAS device_media_source_adapter)
+
+    if(BUILD_TESTING)
+        add_executable(device_media_source_adapter_test
+                tests/device_media_source_adapter_test.cpp
+        )
+        target_compile_features(device_media_source_adapter_test PRIVATE cxx_std_17)
+        target_link_libraries(device_media_source_adapter_test
+                PRIVATE
+                cmvr_es::device_media_source_adapter
+                gtest
+                gtest_main
+        )
+        add_test(
+                NAME device_media_source_adapter_test
+                COMMAND device_media_source_adapter_test
+        )
+        set(_device_media_source_adapter_test_environment
+                "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}")
+        if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+            list(APPEND _device_media_source_adapter_test_environment
+                    "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+        endif()
+        set_tests_properties(device_media_source_adapter_test PROPERTIES
+                ENVIRONMENT
+                "${_device_media_source_adapter_test_environment}"
+        )
+    endif()
+endif()
+
+option(CMVR_MEDIA_SOURCE_MANAGER_BUILD_TESTS
+       "Build the standalone MediaSourceManager self-test"
+       ${PROJECT_IS_TOP_LEVEL})
+
+if(CMVR_MEDIA_SOURCE_MANAGER_BUILD_TESTS)
+    find_package(Threads REQUIRED)
+    add_executable(media_source_manager_test
+            tests/media_source_manager_test.cpp
+    )
+    target_compile_features(media_source_manager_test PRIVATE cxx_std_17)
+    target_link_libraries(media_source_manager_test
+            PRIVATE
+            cmvr_es::media_source_manager
+            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_manager_test PROPERTIES
+            SKIP_BUILD_RPATH TRUE
+    )
+    add_test(NAME media_source_manager_test COMMAND media_source_manager_test)
+endif()

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

@@ -9,27 +9,35 @@
 
 #include "devices/camera/abstract_camera.h"
 #include "devices/microphone/abstract_microphone.h"
-#include "manager/media_source_hub/include/media_source_hub.h"
+#include "manager/media_source_manager/include/media_source_manager.h"
+#include "manager/safety_manager/include/safety_manager.h"
 
 namespace cmvr::media {
 
 // Process-wide protocol-neutral media hub shared by gRPC and QUIC services.
-MediaSourceHub& globalMediaSourceHub();
+MediaSourceManager& globalMediaSourceManager();
 
 std::string cameraColorTrackId(const std::string& device_id);
 std::string microphoneTrackId(const std::string& device_id);
 
+// Acquires the Coordinator's Sensor/StartActivity lane and performs the
+// device endpoint's final hardware check. Keep the returned guard alive until
+// the operation which can start the physical media producer has returned.
+safety::DispatchGuard beginMediaSourceStartDispatch(
+    safety::SafetyManager& coordinator,
+    const std::string& device_id);
+
 // 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,
+    MediaSourceManager& hub,
     const std::shared_ptr<device::AbstractCamera>& camera,
     size_t ring_capacity = 64);
 
 bool ensureMicrophoneMediaSource(
-    MediaSourceHub& hub,
+    MediaSourceManager& hub,
     const std::shared_ptr<device::AbstractMicrophone>& microphone,
     size_t ring_capacity = 256);
 

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

@@ -1,5 +1,5 @@
-#ifndef CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H
-#define CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H
+#ifndef CMVR_ES_MANAGER_MEDIA_SOURCE_MANAGER_H
+#define CMVR_ES_MANAGER_MEDIA_SOURCE_MANAGER_H
 
 #pragma once
 
@@ -15,17 +15,21 @@
 #include "common/base/ring_buffer.h"
 #include "common/media/media_frame.h"
 
+namespace cmvr::service {
+class StopAllAdmissionGate;
+}
+
 namespace cmvr::media {
 
-// MediaSourceHub owns no protocol-specific state. A device or capture adapter registers
+// MediaSourceManager 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 {
+class MediaSourceManager 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
+    // Cancellation checks run while MediaSourceManager 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()>;
@@ -33,7 +37,7 @@ public:
     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
+        // MediaSourceManager retains the callback state until a non-cooperative start
         // eventually returns, so late completion cannot access destroyed state.
         std::function<bool(
             const FrameSink& sink,
@@ -41,6 +45,10 @@ public:
         // stop() is the synchronous publication barrier for the last lease and
         // must unblock and join the source producer before returning.
         std::function<void()> stop;
+        // Optional confirmed variant used by operational StopAll. Returning
+        // false keeps the source quarantined so a later StopAll can retry it.
+        // When omitted, a non-throwing stop() call is treated as confirmation.
+        std::function<bool()> stop_confirmed;
         std::function<bool()> request_key_frame;
     };
 
@@ -76,7 +84,7 @@ public:
         void reset();
 
     private:
-        friend class MediaSourceHub;
+        friend class MediaSourceManager;
         Subscription(std::shared_ptr<SourceState> source, FrameRing::Cursor cursor);
 
         std::shared_ptr<SourceState> source_;
@@ -84,11 +92,14 @@ public:
         bool active_{false};
     };
 
-    MediaSourceHub();
-    ~MediaSourceHub();
+    // Pass the process-wide StopAll gate for a hub whose sources are part of
+    // whole-machine operational stopping. Test/private hubs may remain local.
+    explicit MediaSourceManager(
+        service::StopAllAdmissionGate* admission_gate = nullptr);
+    ~MediaSourceManager();
 
-    MediaSourceHub(const MediaSourceHub&) = delete;
-    MediaSourceHub& operator=(const MediaSourceHub&) = delete;
+    MediaSourceManager(const MediaSourceManager&) = delete;
+    MediaSourceManager& operator=(const MediaSourceManager&) = delete;
 
     bool registerSource(
         TrackDescriptorPtr initial_descriptor,
@@ -100,6 +111,11 @@ public:
 
     bool hasSource(const std::string& track_id) const;
     std::vector<TrackDescriptorPtr> listTracks() const;
+    // Returns a stable, sorted snapshot of physical source IDs. The snapshot
+    // includes sources temporarily removed from the public track map while a
+    // stop callback is in progress, so StopAll can discover orphaned activity
+    // without consulting DeviceManager.
+    std::vector<std::string> trackedSourceIds() const;
     size_t subscriberCount(const std::string& track_id) const;
 
     // Protocol adapters can request an IDR after a discontinuity without knowing the
@@ -111,18 +127,36 @@ public:
         StartPosition start_position = StartPosition::NEXT_PUBLISHED,
         CancelPredicate cancelled = {});
 
+    // Stops and unregisters every source whose registered descriptor belongs
+    // to source_id. Sources for other physical devices remain registered and
+    // keep running. A failed source is restored for a later retry.
+    bool stopSourcesForDevice(
+        const std::string& source_id,
+        std::vector<std::string>* failures = nullptr);
+
+    // Stops and unregisters every source that was registered before this call's
+    // stop phase began. Outstanding subscriptions are invalidated and blocked
+    // waitRead calls are awakened. Registrations concurrent with the stop wait
+    // for that phase to finish and are retained, so sources can be ensured and
+    // subscribed again after this method returns.
+    bool stopAllSources(std::vector<std::string>* failures = nullptr);
+
     // 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.
+    // shutdown or risking a use-after-free. Equivalent to stopAllSources().
     void shutdown();
 
 private:
+    bool stopSources(
+        const std::optional<std::string>& source_id,
+        std::vector<std::string>* failures);
+
     struct Impl;
     std::shared_ptr<Impl> impl_;
 };
 
 } // namespace cmvr::media
 
-#endif // CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H
+#endif // CMVR_ES_MANAGER_MEDIA_SOURCE_MANAGER_H

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

@@ -1,4 +1,6 @@
-#include "manager/media_source_hub/include/device_media_source_adapter.h"
+#include "manager/media_source_manager/include/device_media_source_adapter.h"
+
+#include "service/grpc/stop_all/include/stop_all_admission_gate.h"
 
 #include <algorithm>
 #include <atomic>
@@ -18,6 +20,8 @@
 namespace cmvr::media {
 namespace {
 
+std::atomic<std::uint64_t> media_start_sequence{0};
+
 std::string normalizedCodec(std::string codec) {
     codec.erase(
         std::remove_if(codec.begin(), codec.end(), [](const unsigned char c) {
@@ -125,12 +129,12 @@ struct PumpState : public std::enable_shared_from_this<PumpState<DeviceT>> {
         : device(std::move(device_ptr)) {}
 
     virtual ~PumpState() {
-        stop();
+        (void)stop();
     }
 
     bool begin(
-        const MediaSourceHub::FrameSink& frame_sink,
-        const MediaSourceHub::CancelPredicate& cancelled) {
+        const MediaSourceManager::FrameSink& frame_sink,
+        const MediaSourceManager::CancelPredicate& cancelled) {
         if (!frame_sink || !device) {
             return false;
         }
@@ -154,7 +158,9 @@ struct PumpState : public std::enable_shared_from_this<PumpState<DeviceT>> {
             // 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));
+            if (!collectThread(std::move(stale_worker))) {
+                return false;
+            }
             lock.lock();
         }
 
@@ -215,7 +221,7 @@ struct PumpState : public std::enable_shared_from_this<PumpState<DeviceT>> {
         return true;
     }
 
-    void stop() noexcept {
+    bool stop() noexcept {
         std::thread thread;
         bool stop_streaming = false;
         {
@@ -226,24 +232,28 @@ struct PumpState : public std::enable_shared_from_this<PumpState<DeviceT>> {
             sink = {};
             thread = std::move(worker);
         }
+        bool stopped = true;
         if (stop_streaming) {
-            stopDeviceStreaming();
+            stopped = stopDeviceStreaming();
         }
         if (thread.joinable()) {
-            collectThread(std::move(thread));
+            stopped = collectThread(std::move(thread)) && stopped;
         }
+        return stopped;
     }
 
-    static void collectThread(std::thread thread) noexcept {
+    static bool collectThread(std::thread thread) noexcept {
         if (!thread.joinable()) {
-            return;
+            return true;
         }
         try {
             if (thread.get_id() == std::this_thread::get_id()) {
                 thread.detach();
+                return false;
             } else {
                 thread.join();
             }
+            return true;
         } catch (const std::exception& error) {
             CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to collect media pump: "
                             << error.what();
@@ -255,36 +265,39 @@ struct PumpState : public std::enable_shared_from_this<PumpState<DeviceT>> {
                     // platform error occurred; there is no recoverable ownership path.
                 }
             }
+            return false;
         }
     }
 
     virtual void run() = 0;
 
-    void stopDeviceStreaming() noexcept {
+    bool stopDeviceStreaming() noexcept {
         try {
             if (device) {
                 device->stopStreaming();
             }
+            return true;
         } 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";
         }
+        return false;
     }
 
     std::shared_ptr<DeviceT> device;
     std::atomic<bool> running{false};
     std::mutex mutex;
     std::thread worker;
-    MediaSourceHub::FrameSink sink;
+    MediaSourceManager::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(); }
+    ~CameraPump() override { (void)stop(); }
 
     void run() override {
         size_t cursor = 0;
@@ -437,7 +450,7 @@ struct CameraPump final : PumpState<device::AbstractCamera> {
             frame.key_frame = source.bKey;
             frame.discontinuity = pending_discontinuity;
 
-            MediaSourceHub::FrameSink current_sink;
+            MediaSourceManager::FrameSink current_sink;
             {
                 std::lock_guard<std::mutex> lock(mutex);
                 current_sink = sink;
@@ -461,7 +474,7 @@ struct CameraPump final : PumpState<device::AbstractCamera> {
 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(); }
+    ~MicrophonePump() override { (void)stop(); }
 
     void run() override {
         size_t cursor = 0;
@@ -570,7 +583,7 @@ struct MicrophonePump final : PumpState<device::AbstractMicrophone> {
             frame.key_frame = true;
             frame.discontinuity = pending_discontinuity;
 
-            MediaSourceHub::FrameSink current_sink;
+            MediaSourceManager::FrameSink current_sink;
             {
                 std::lock_guard<std::mutex> lock(mutex);
                 current_sink = sink;
@@ -608,8 +621,8 @@ TrackDescriptorPtr initialTrack(
 
 } // namespace
 
-MediaSourceHub& globalMediaSourceHub() {
-    static MediaSourceHub hub;
+MediaSourceManager& globalMediaSourceManager() {
+    static MediaSourceManager hub(&service::globalStopAllAdmissionGate());
     return hub;
 }
 
@@ -621,8 +634,45 @@ std::string microphoneTrackId(const std::string& device_id) {
     return device_id + "/audio/main";
 }
 
+safety::DispatchGuard beginMediaSourceStartDispatch(
+    safety::SafetyManager& coordinator,
+    const std::string& device_id)
+{
+    safety::AdmissionRequest request;
+    request.command = {
+        "cmvr.internal.MediaSourceManager/StartSource",
+        safety::CommandIntent::StartActivity,
+        safety::SafetyPolicyFamily::Sensor,
+        true,
+        false};
+    request.actor.principal_id = "internal:media-source-hub";
+    request.actor.authenticated = true;
+    request.command_id = "media-source-start:" + device_id + ':' +
+        std::to_string(
+            media_start_sequence.fetch_add(1, std::memory_order_relaxed) + 1U);
+    request.device_id = device_id;
+
+    auto admission = coordinator.admit(request);
+    if (!admission.permit.has_value()) {
+        CMVR_LOG(WARNING)
+            << "[DeviceMediaSourceAdapter] Media source admission rejected for "
+            << device_id << ": " << safety::toString(admission.decision.reason)
+            << " (" << admission.decision.detail << ')';
+        return {};
+    }
+    auto dispatch = coordinator.beginDispatch(*admission.permit);
+    if (!dispatch.acquired()) {
+        CMVR_LOG(WARNING)
+            << "[DeviceMediaSourceAdapter] Media source final check rejected for "
+            << device_id << ": "
+            << safety::toString(dispatch.hardwareCheck().reason) << " ("
+            << dispatch.hardwareCheck().detail << ')';
+    }
+    return dispatch;
+}
+
 bool ensureCameraMediaSource(
-    MediaSourceHub& hub,
+    MediaSourceManager& hub,
     const std::shared_ptr<device::AbstractCamera>& camera,
     const size_t ring_capacity) {
     if (!camera || camera->id().empty()) {
@@ -634,13 +684,13 @@ bool ensureCameraMediaSource(
     }
 
     const auto pump = std::make_shared<CameraPump>(camera, track_id);
-    MediaSourceHub::SourceCallbacks callbacks;
+    MediaSourceManager::SourceCallbacks callbacks;
     callbacks.start = [pump](
-                          const MediaSourceHub::FrameSink& sink,
-                          const MediaSourceHub::CancelPredicate& cancelled) {
+                          const MediaSourceManager::FrameSink& sink,
+                          const MediaSourceManager::CancelPredicate& cancelled) {
         return pump->begin(sink, cancelled);
     };
-    callbacks.stop = [pump] { pump->stop(); };
+    callbacks.stop_confirmed = [pump] { return pump->stop(); };
     callbacks.request_key_frame = [camera] { return camera->requestKeyFrame(); };
     const bool registered = hub.registerSource(
         initialTrack(track_id, camera->id(), MediaKind::VIDEO),
@@ -654,7 +704,7 @@ bool ensureCameraMediaSource(
 }
 
 bool ensureMicrophoneMediaSource(
-    MediaSourceHub& hub,
+    MediaSourceManager& hub,
     const std::shared_ptr<device::AbstractMicrophone>& microphone,
     const size_t ring_capacity) {
     if (!microphone || microphone->id().empty()) {
@@ -666,13 +716,13 @@ bool ensureMicrophoneMediaSource(
     }
 
     const auto pump = std::make_shared<MicrophonePump>(microphone, track_id);
-    MediaSourceHub::SourceCallbacks callbacks;
+    MediaSourceManager::SourceCallbacks callbacks;
     callbacks.start = [pump](
-                          const MediaSourceHub::FrameSink& sink,
-                          const MediaSourceHub::CancelPredicate& cancelled) {
+                          const MediaSourceManager::FrameSink& sink,
+                          const MediaSourceManager::CancelPredicate& cancelled) {
         return pump->begin(sink, cancelled);
     };
-    callbacks.stop = [pump] { pump->stop(); };
+    callbacks.stop_confirmed = [pump] { return pump->stop(); };
     const bool registered = hub.registerSource(
         initialTrack(track_id, microphone->id(), MediaKind::AUDIO),
         std::move(callbacks),

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

@@ -1,4 +1,4 @@
-#include "manager/media_source_hub/include/media_source_hub.h"
+#include "manager/media_source_manager/include/media_source_manager.h"
 
 #include <algorithm>
 #include <atomic>
@@ -6,11 +6,14 @@
 #include <mutex>
 #include <thread>
 #include <unordered_map>
+#include <unordered_set>
 #include <utility>
 
+#include "service/grpc/stop_all/include/stop_all_admission_gate.h"
+
 namespace cmvr::media {
 
-struct MediaSourceHub::SourceState final : public std::enable_shared_from_this<SourceState> {
+struct MediaSourceManager::SourceState final : public std::enable_shared_from_this<SourceState> {
     enum class Lifecycle {
         STOPPED,
         STARTING,
@@ -28,11 +31,14 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_this<S
     SourceState(
         TrackDescriptorPtr initial_descriptor,
         SourceCallbacks source_callbacks,
-        const size_t ring_capacity)
+        const size_t ring_capacity,
+        service::StopAllAdmissionGate* source_admission_gate)
         : track_id(initial_descriptor->id),
+          source_id(initial_descriptor->source_id),
           descriptor(std::move(initial_descriptor)),
           callbacks(std::move(source_callbacks)),
-          ring(ring_capacity) {}
+          ring(ring_capacity),
+          admission_gate(source_admission_gate) {}
 
     FrameSink makeSink() {
         const std::weak_ptr<SourceState> weak_source = shared_from_this();
@@ -85,11 +91,18 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_this<S
         }
     }
 
-    void invokeStop() noexcept {
+    bool invokeStop() noexcept {
         std::lock_guard<std::mutex> callback_lock(callback_mutex);
         try {
-            if (callbacks.stop) callbacks.stop();
+            if (callbacks.stop_confirmed) {
+                return callbacks.stop_confirmed();
+            }
+            if (callbacks.stop) {
+                callbacks.stop();
+            }
+            return true;
         } catch (...) {
+            return false;
         }
     }
 
@@ -117,9 +130,10 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_this<S
         }
 
         if (stop_abandoned_start) {
-            invokeStop();
+            const bool stopped = invokeStop();
             std::lock_guard<std::mutex> lock(lifecycle_mutex);
-            if (lifecycle == Lifecycle::STOPPING) {
+            stop_unconfirmed = !stopped;
+            if (stopped && lifecycle == Lifecycle::STOPPING) {
                 lifecycle = Lifecycle::STOPPED;
             }
             lifecycle_condition.notify_all();
@@ -130,12 +144,29 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_this<S
         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));
+        std::optional<service::StopAllAdmissionGate::AdmissionGuard>
+            admission;
+        std::unique_lock<std::mutex> lock(lifecycle_mutex, std::defer_lock);
+        for (;;) {
+            if (admission_gate) {
+                admission.emplace(admission_gate->lockAdmission());
+            }
+            lock.lock();
+            if (!registered || isCancelled(cancelled) ||
+                (admission && !admission->accepting())) {
+                return false;
+            }
+            if (lifecycle != Lifecycle::STOPPING) {
+                break;
+            }
+
+            // A device stop may block, so never wait for it while retaining
+            // the process-wide admission lock.
+            admission.reset();
+            lifecycle_condition.wait_for(
+                lock, std::chrono::milliseconds(10));
+            lock.unlock();
         }
-        if (!registered || isCancelled(cancelled)) return false;
 
         if (lifecycle == Lifecycle::RUNNING) {
             ++subscriber_count;
@@ -178,6 +209,10 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_this<S
             return false;
         }
 
+        // Startup is now ordered before beginStopAll(). Do not retain the
+        // process-wide gate while waiting for the device callback to return.
+        admission.reset();
+
         while (registered && !attempt->completed) {
             if (isCancelled(cancelled)) {
                 if (attempt->waiters != 0U) --attempt->waiters;
@@ -207,9 +242,10 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_this<S
                 lifecycle = Lifecycle::STOPPING;
                 ring.close();
                 lock.unlock();
-                invokeStop();
+                const bool stopped = invokeStop();
                 lock.lock();
-                if (lifecycle == Lifecycle::STOPPING) {
+                stop_unconfirmed = !stopped;
+                if (stopped && lifecycle == Lifecycle::STOPPING) {
                     lifecycle = Lifecycle::STOPPED;
                 }
                 lifecycle_condition.notify_all();
@@ -235,9 +271,12 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_this<S
         lifecycle = Lifecycle::STOPPING;
         ring.close();
         lock.unlock();
-        invokeStop();
+        const bool stopped = invokeStop();
         lock.lock();
-        lifecycle = Lifecycle::STOPPED;
+        stop_unconfirmed = !stopped;
+        if (stopped) {
+            lifecycle = Lifecycle::STOPPED;
+        }
         lifecycle_condition.notify_all();
     }
 
@@ -262,16 +301,17 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_this<S
 
         lifecycle = Lifecycle::STOPPING;
         lock.unlock();
-        invokeStop();
+        const bool stopped = invokeStop();
         lock.lock();
-        if (lifecycle == Lifecycle::STOPPING) {
+        stop_unconfirmed = !stopped;
+        if (stopped && lifecycle == Lifecycle::STOPPING) {
             lifecycle = Lifecycle::STOPPED;
         }
         lifecycle_condition.notify_all();
-        return true;
+        return stopped;
     }
 
-    void shutdown() {
+    bool shutdown() {
         std::unique_lock<std::mutex> lock(lifecycle_mutex);
         registered = false;
         ring.close();
@@ -280,25 +320,41 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_this<S
                 start_attempt->cancel_requested.store(true, std::memory_order_release);
             }
             lifecycle_condition.notify_all();
-            return;
+            return false;
         }
         if (lifecycle == Lifecycle::STOPPING) {
+            if (stop_unconfirmed) {
+                lock.unlock();
+                const bool stopped = invokeStop();
+                lock.lock();
+                stop_unconfirmed = !stopped;
+                if (stopped) {
+                    lifecycle = Lifecycle::STOPPED;
+                }
+                lifecycle_condition.notify_all();
+                return stopped;
+            }
+            lifecycle_condition.wait(lock, [this] {
+                return lifecycle != Lifecycle::STOPPING;
+            });
             lifecycle_condition.notify_all();
-            return;
+            return lifecycle == Lifecycle::STOPPED && !stop_unconfirmed;
         }
         if (lifecycle == Lifecycle::STOPPED) {
             lifecycle_condition.notify_all();
-            return;
+            return !stop_unconfirmed;
         }
 
         lifecycle = Lifecycle::STOPPING;
         lock.unlock();
-        invokeStop();
+        const bool stopped = invokeStop();
         lock.lock();
-        if (lifecycle == Lifecycle::STOPPING) {
+        stop_unconfirmed = !stopped;
+        if (stopped && lifecycle == Lifecycle::STOPPING) {
             lifecycle = Lifecycle::STOPPED;
         }
         lifecycle_condition.notify_all();
+        return stopped;
     }
 
     bool validForSubscription() const {
@@ -334,9 +390,11 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_this<S
     }
 
     const std::string track_id;
+    const std::string source_id;
     mutable TrackDescriptorPtr descriptor;
     const SourceCallbacks callbacks;
     FrameRing ring;
+    service::StopAllAdmissionGate* const admission_gate;
 
     mutable std::mutex callback_mutex;
     mutable std::mutex lifecycle_mutex;
@@ -344,33 +402,43 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_this<S
     Lifecycle lifecycle{Lifecycle::STOPPED};
     size_t subscriber_count{0};
     bool registered{true};
+    bool stop_unconfirmed{false};
     std::shared_ptr<StartAttempt> start_attempt;
 };
 
-struct MediaSourceHub::Impl final {
+struct MediaSourceManager::Impl final {
+    explicit Impl(service::StopAllAdmissionGate* source_admission_gate)
+        : admission_gate(source_admission_gate) {}
+
     mutable std::mutex mutex;
+    std::condition_variable stop_condition;
+    bool stop_all_in_progress{false};
+    std::unordered_set<std::string> device_stops_in_progress;
+    std::unordered_set<std::string> track_stops_in_progress;
+    std::unordered_map<std::string, size_t> tracked_source_counts;
     std::unordered_map<std::string, std::shared_ptr<SourceState>> sources;
+    service::StopAllAdmissionGate* const admission_gate;
 };
 
-MediaSourceHub::Subscription::Subscription(
+MediaSourceManager::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() {
+MediaSourceManager::Subscription::~Subscription() {
     reset();
 }
 
-MediaSourceHub::Subscription::Subscription(Subscription&& other) noexcept
+MediaSourceManager::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 {
+MediaSourceManager::Subscription& MediaSourceManager::Subscription::operator=(Subscription&& other) noexcept {
     if (this == &other) {
         return *this;
     }
@@ -382,22 +450,22 @@ MediaSourceHub::Subscription& MediaSourceHub::Subscription::operator=(Subscripti
     return *this;
 }
 
-bool MediaSourceHub::Subscription::valid() const {
+bool MediaSourceManager::Subscription::valid() const {
     return active_ && source_ && source_->validForSubscription();
 }
 
-TrackDescriptorPtr MediaSourceHub::Subscription::descriptor() const {
+TrackDescriptorPtr MediaSourceManager::Subscription::descriptor() const {
     return source_ ? source_->currentDescriptor() : nullptr;
 }
 
-std::optional<MediaSourceHub::FrameReadResult> MediaSourceHub::Subscription::tryRead() {
+std::optional<MediaSourceManager::FrameReadResult> MediaSourceManager::Subscription::tryRead() {
     if (!active_ || !source_) {
         return std::nullopt;
     }
     return source_->ring.tryRead(cursor_);
 }
 
-std::optional<MediaSourceHub::FrameReadResult> MediaSourceHub::Subscription::waitRead(
+std::optional<MediaSourceManager::FrameReadResult> MediaSourceManager::Subscription::waitRead(
     const std::chrono::milliseconds timeout) {
     if (!active_ || !source_) {
         return std::nullopt;
@@ -405,7 +473,7 @@ std::optional<MediaSourceHub::FrameReadResult> MediaSourceHub::Subscription::wai
     return source_->ring.waitRead(cursor_, timeout);
 }
 
-uint64_t MediaSourceHub::Subscription::discardPendingIfExceeds(
+uint64_t MediaSourceManager::Subscription::discardPendingIfExceeds(
     const size_t maximum_pending_frames) {
     if (!active_ || !source_) {
         return 0;
@@ -413,11 +481,11 @@ uint64_t MediaSourceHub::Subscription::discardPendingIfExceeds(
     return source_->ring.discardPendingIfExceeds(cursor_, maximum_pending_frames);
 }
 
-uint64_t MediaSourceHub::Subscription::droppedCount() const noexcept {
+uint64_t MediaSourceManager::Subscription::droppedCount() const noexcept {
     return cursor_.dropped_count;
 }
 
-void MediaSourceHub::Subscription::reset() {
+void MediaSourceManager::Subscription::reset() {
     if (active_ && source_) {
         source_->release();
     }
@@ -425,14 +493,15 @@ void MediaSourceHub::Subscription::reset() {
     source_.reset();
 }
 
-MediaSourceHub::MediaSourceHub()
-    : impl_(std::make_shared<Impl>()) {}
+MediaSourceManager::MediaSourceManager(
+    service::StopAllAdmissionGate* admission_gate)
+    : impl_(std::make_shared<Impl>(admission_gate)) {}
 
-MediaSourceHub::~MediaSourceHub() {
+MediaSourceManager::~MediaSourceManager() {
     shutdown();
 }
 
-bool MediaSourceHub::registerSource(
+bool MediaSourceManager::registerSource(
     TrackDescriptorPtr initial_descriptor,
     SourceCallbacks callbacks,
     const size_t ring_capacity) {
@@ -444,16 +513,50 @@ bool MediaSourceHub::registerSource(
     std::shared_ptr<SourceState> source;
     try {
         source = std::make_shared<SourceState>(
-            std::move(initial_descriptor), std::move(callbacks), ring_capacity);
+            std::move(initial_descriptor), std::move(callbacks), ring_capacity,
+            impl_->admission_gate);
     } catch (...) {
         return false;
     }
 
-    std::lock_guard<std::mutex> lock(impl_->mutex);
-    return impl_->sources.emplace(source->track_id, std::move(source)).second;
+    std::optional<service::StopAllAdmissionGate::AdmissionGuard> admission;
+    std::unique_lock<std::mutex> lock(impl_->mutex, std::defer_lock);
+    for (;;) {
+        if (impl_->admission_gate) {
+            admission.emplace(impl_->admission_gate->lockAdmission());
+        }
+        lock.lock();
+        if (admission && !admission->accepting()) {
+            return false;
+        }
+        const bool can_register =
+            !impl_->stop_all_in_progress &&
+            impl_->device_stops_in_progress.count(source->source_id) == 0U &&
+            impl_->track_stops_in_progress.count(source->track_id) == 0U;
+        if (can_register) {
+            break;
+        }
+
+        // Hub-local stops may invoke arbitrary device callbacks. Wait for
+        // them without delaying process-wide StopAll admission.
+        admission.reset();
+        impl_->stop_condition.wait(lock, [this, &source] {
+            return !impl_->stop_all_in_progress &&
+                   impl_->device_stops_in_progress.count(source->source_id) == 0U &&
+                   impl_->track_stops_in_progress.count(source->track_id) == 0U;
+        });
+        lock.unlock();
+    }
+    const std::string source_id = source->source_id;
+    const bool inserted =
+        impl_->sources.emplace(source->track_id, std::move(source)).second;
+    if (inserted) {
+        ++impl_->tracked_source_counts[source_id];
+    }
+    return inserted;
 }
 
-bool MediaSourceHub::unregisterSource(const std::string& track_id) {
+bool MediaSourceManager::unregisterSource(const std::string& track_id) {
     if (!impl_ || track_id.empty()) {
         return false;
     }
@@ -475,13 +578,19 @@ bool MediaSourceHub::unregisterSource(const std::string& track_id) {
     std::lock_guard<std::mutex> lock(impl_->mutex);
     const auto it = impl_->sources.find(track_id);
     if (it != impl_->sources.end() && it->second == source) {
+        const std::string source_id = source->source_id;
         impl_->sources.erase(it);
+        const auto count_it = impl_->tracked_source_counts.find(source_id);
+        if (count_it != impl_->tracked_source_counts.end() &&
+            --count_it->second == 0U) {
+            impl_->tracked_source_counts.erase(count_it);
+        }
         return true;
     }
     return false;
 }
 
-bool MediaSourceHub::hasSource(const std::string& track_id) const {
+bool MediaSourceManager::hasSource(const std::string& track_id) const {
     if (!impl_) {
         return false;
     }
@@ -489,7 +598,7 @@ bool MediaSourceHub::hasSource(const std::string& track_id) const {
     return impl_->sources.find(track_id) != impl_->sources.end();
 }
 
-std::vector<TrackDescriptorPtr> MediaSourceHub::listTracks() const {
+std::vector<TrackDescriptorPtr> MediaSourceManager::listTracks() const {
     std::vector<std::shared_ptr<SourceState>> sources;
     if (!impl_) {
         return {};
@@ -516,7 +625,26 @@ std::vector<TrackDescriptorPtr> MediaSourceHub::listTracks() const {
     return descriptors;
 }
 
-size_t MediaSourceHub::subscriberCount(const std::string& track_id) const {
+std::vector<std::string> MediaSourceManager::trackedSourceIds() const {
+    if (!impl_) {
+        return {};
+    }
+
+    std::vector<std::string> source_ids;
+    {
+        std::lock_guard<std::mutex> lock(impl_->mutex);
+        source_ids.reserve(impl_->tracked_source_counts.size());
+        for (const auto& [source_id, count] : impl_->tracked_source_counts) {
+            if (count != 0U) {
+                source_ids.push_back(source_id);
+            }
+        }
+    }
+    std::sort(source_ids.begin(), source_ids.end());
+    return source_ids;
+}
+
+size_t MediaSourceManager::subscriberCount(const std::string& track_id) const {
     if (!impl_) {
         return 0;
     }
@@ -532,7 +660,7 @@ size_t MediaSourceHub::subscriberCount(const std::string& track_id) const {
     return source->subscriberCount();
 }
 
-bool MediaSourceHub::requestKeyFrame(const std::string& track_id) const {
+bool MediaSourceManager::requestKeyFrame(const std::string& track_id) const {
     if (!impl_) {
         return false;
     }
@@ -548,7 +676,7 @@ bool MediaSourceHub::requestKeyFrame(const std::string& track_id) const {
     return source->requestKeyFrame();
 }
 
-MediaSourceHub::Subscription MediaSourceHub::subscribe(
+MediaSourceManager::Subscription MediaSourceManager::subscribe(
     const std::string& track_id,
     const StartPosition start_position,
     CancelPredicate cancelled) {
@@ -573,24 +701,103 @@ MediaSourceHub::Subscription MediaSourceHub::subscribe(
     return Subscription(std::move(source), std::move(cursor));
 }
 
-void MediaSourceHub::shutdown() {
+bool MediaSourceManager::stopSourcesForDevice(
+    const std::string& source_id,
+    std::vector<std::string>* failures) {
+    if (source_id.empty()) {
+        if (failures) {
+            failures->clear();
+        }
+        return true;
+    }
+    return stopSources(source_id, failures);
+}
+
+bool MediaSourceManager::stopAllSources(std::vector<std::string>* failures) {
+    return stopSources(std::nullopt, failures);
+}
+
+bool MediaSourceManager::stopSources(
+    const std::optional<std::string>& source_id,
+    std::vector<std::string>* failures) {
+    if (failures) {
+        failures->clear();
+    }
     if (!impl_) {
-        return;
+        return true;
+    }
+
+    std::unordered_map<std::string, std::shared_ptr<SourceState>> sources;
+    {
+        std::unique_lock<std::mutex> lock(impl_->mutex);
+        if (source_id) {
+            impl_->stop_condition.wait(lock, [this, &source_id] {
+                return !impl_->stop_all_in_progress &&
+                       impl_->device_stops_in_progress.count(*source_id) == 0U;
+            });
+            impl_->device_stops_in_progress.insert(*source_id);
+            for (auto source_it = impl_->sources.begin();
+                 source_it != impl_->sources.end();) {
+                if (source_it->second->source_id != *source_id) {
+                    ++source_it;
+                    continue;
+                }
+                impl_->track_stops_in_progress.insert(source_it->first);
+                sources.emplace(source_it->first, std::move(source_it->second));
+                source_it = impl_->sources.erase(source_it);
+            }
+        } else {
+            impl_->stop_condition.wait(lock, [this] {
+                return !impl_->stop_all_in_progress &&
+                       impl_->device_stops_in_progress.empty();
+            });
+            impl_->stop_all_in_progress = true;
+            sources.swap(impl_->sources);
+        }
+    }
+
+    std::unordered_map<std::string, std::shared_ptr<SourceState>> quarantined;
+    for (const auto& [track_id, source] : sources) {
+        if (!source->shutdown()) {
+            quarantined.emplace(track_id, source);
+            if (failures) {
+                failures->push_back(track_id);
+            }
+        }
     }
 
-    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));
+        for (auto& [track_id, source] : quarantined) {
+            impl_->sources.emplace(track_id, std::move(source));
+        }
+        for (const auto& [track_id, source] : sources) {
+            if (quarantined.count(track_id) != 0U) {
+                continue;
+            }
+            const auto count_it =
+                impl_->tracked_source_counts.find(source->source_id);
+            if (count_it != impl_->tracked_source_counts.end() &&
+                --count_it->second == 0U) {
+                impl_->tracked_source_counts.erase(count_it);
+            }
+        }
+        if (source_id) {
+            for (const auto& [track_id, source] : sources) {
+                (void)source;
+                impl_->track_stops_in_progress.erase(track_id);
+            }
+            impl_->device_stops_in_progress.erase(*source_id);
+        } else {
+            impl_->stop_all_in_progress = false;
         }
-        impl_->sources.clear();
-    }
-    for (const auto& source : sources) {
-        source->shutdown();
     }
+    impl_->stop_condition.notify_all();
+    return quarantined.empty();
+}
+
+void MediaSourceManager::shutdown() {
+    (void)stopAllSources();
 }
 
 } // namespace cmvr::media

cmvr-es/manager/media_source_manager/tests/device_media_source_adapter_test.cpp

@@ -0,0 +1,113 @@
+#include "manager/media_source_manager/include/device_media_source_adapter.h"
+
+#include <atomic>
+#include <chrono>
+#include <memory>
+#include <string>
+
+#include <gtest/gtest.h>
+
+#include "manager/safety_manager/include/device_safety_endpoint.h"
+
+namespace cmvr::media {
+namespace {
+
+class FakeSensorEndpoint final : public safety::DeviceSafetyEndpoint {
+public:
+    explicit FakeSensorEndpoint(std::string device_id)
+    {
+        descriptor_.device_id = std::move(device_id);
+        descriptor_.kind = device::DeviceKind::Camera;
+        descriptor_.default_policy = safety::SafetyPolicyFamily::Sensor;
+        descriptor_.maximum_snapshot_age = std::chrono::seconds(1);
+        descriptor_.supports_active_refresh = true;
+    }
+
+    safety::DeviceSafetyDescriptor descriptor() const override
+    {
+        return descriptor_;
+    }
+
+    void bindPublisher(safety::SafetySnapshotPublisher publisher) override
+    {
+        publisher_ = std::move(publisher);
+    }
+
+    void requestSafetyRefresh() noexcept override
+    {
+        if (!publisher_) {
+            return;
+        }
+        safety::DeviceSafetySnapshot snapshot;
+        snapshot.device_id = descriptor_.device_id;
+        snapshot.condition = safety::SafetyCondition::Nominal;
+        snapshot.device_generation = 1;
+        snapshot.sample_sequence = ++sequence_;
+        snapshot.observed_at = safety::SafetyClock::now();
+        snapshot.connected = safety::TriState::True;
+        snapshot.operational_ready = safety::TriState::True;
+        snapshot.quiescent = safety::TriState::True;
+        snapshot.motion_active = safety::TriState::False;
+        snapshot.actuator_enabled = safety::TriState::False;
+        snapshot.emergency_stop_active = safety::TriState::False;
+        snapshot.protective_stop_active = safety::TriState::False;
+        snapshot.fault_active = safety::TriState::False;
+        (void)publisher_(std::move(snapshot));
+    }
+
+    safety::HardwareCheckResult validateBeforeDispatch(
+        const safety::AdmissionPermit& permit) override
+    {
+        ++hardware_checks;
+        last_intent = permit.intent;
+        return {true, safety::SafetyReason::None, {}};
+    }
+
+    safety::RecoveryCheckResult reconcileAdmissionState(
+        const safety::RecoveryContext&) override
+    {
+        return {true, safety::SafetyReason::None, {}};
+    }
+
+    std::atomic<int> hardware_checks{0};
+    safety::CommandIntent last_intent{safety::CommandIntent::Observe};
+
+private:
+    safety::DeviceSafetyDescriptor descriptor_;
+    safety::SafetySnapshotPublisher publisher_;
+    std::atomic<std::uint64_t> sequence_{0};
+};
+
+TEST(DeviceMediaSourceAdapterTest,
+     SensorStartUsesFinalCheckAndQuarantineRejectsRestart)
+{
+    safety::SafetyManagerConfig config;
+    config.enforcement_mode = safety::EnforcementMode::EnforceAll;
+    safety::SafetyManager coordinator(config);
+    auto endpoint = std::make_shared<FakeSensorEndpoint>("camera");
+    ASSERT_TRUE(coordinator.registerDevice(
+        {endpoint->descriptor(), endpoint, {}}));
+    endpoint->requestSafetyRefresh();
+    coordinator.updateDeviceRuntimeState(
+        "camera",
+        device::ManagedDeviceState::Running,
+        {device::DeviceHealthState::Healthy, {}});
+    coordinator.markStartupComplete();
+
+    {
+        auto dispatch = beginMediaSourceStartDispatch(coordinator, "camera");
+        ASSERT_TRUE(dispatch.acquired());
+        EXPECT_EQ(endpoint->hardware_checks.load(), 1);
+        EXPECT_EQ(endpoint->last_intent, safety::CommandIntent::StartActivity);
+    }
+
+    coordinator.quarantineDevice(
+        "camera", safety::SafetyReason::OutcomeUnknown,
+        "uncertain-media-start");
+    auto rejected = beginMediaSourceStartDispatch(coordinator, "camera");
+    EXPECT_FALSE(rejected.acquired());
+    EXPECT_EQ(endpoint->hardware_checks.load(), 1);
+}
+
+} // namespace
+} // namespace cmvr::media

cmvr-es/manager/safety_manager/CMakeLists.txt

@@ -0,0 +1,65 @@
+add_library(safety_manager STATIC
+        src/command_ledger.cpp
+        src/safety_manager.cpp
+        src/safety_reason.cpp
+        src/safety_snapshot_store.cpp
+)
+
+target_include_directories(safety_manager PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}
+        ${CMAKE_SOURCE_DIR}/cmvr-es
+)
+
+target_link_libraries(safety_manager PUBLIC
+        cmvr_es::control_authority_manager
+)
+
+add_library(cmvr_es::safety_manager ALIAS safety_manager)
+install(TARGETS safety_manager LIBRARY DESTINATION lib)
+
+if(BUILD_TESTING)
+    add_executable(safety_snapshot_store_test
+            tests/safety_snapshot_store_test.cpp
+    )
+    target_link_libraries(safety_snapshot_store_test PRIVATE
+            cmvr_es::safety_manager
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME safety_snapshot_store_test
+            COMMAND safety_snapshot_store_test
+    )
+    set_tests_properties(safety_snapshot_store_test PROPERTIES TIMEOUT 10)
+
+    add_executable(command_ledger_test
+            tests/command_ledger_test.cpp
+    )
+    target_link_libraries(command_ledger_test PRIVATE
+            cmvr_es::safety_manager
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME command_ledger_test
+            COMMAND command_ledger_test
+    )
+    set_tests_properties(command_ledger_test PROPERTIES TIMEOUT 10)
+
+    add_executable(safety_manager_test
+            tests/safety_manager_test.cpp
+    )
+    target_link_libraries(safety_manager_test PRIVATE
+            cmvr_es::safety_manager
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME safety_manager_test
+            COMMAND safety_manager_test
+    )
+    set_tests_properties(safety_manager_test PROPERTIES TIMEOUT 15)
+endif()

cmvr-es/manager/safety_manager/include/command_ledger.h

@@ -0,0 +1,130 @@
+#pragma once
+
+#include <chrono>
+#include <condition_variable>
+#include <cstddef>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <string>
+#include <unordered_map>
+
+#include "manager/safety_manager/include/safety_types.h"
+
+namespace cmvr::safety {
+
+struct CommandKey {
+    std::string effective_principal_id;
+    std::string command_id;
+
+    bool operator==(const CommandKey& other) const noexcept
+    {
+        return effective_principal_id == other.effective_principal_id &&
+               command_id == other.command_id;
+    }
+};
+
+struct CommandOutcome {
+    CommandLifecycle lifecycle{CommandLifecycle::Failed};
+    SafetyReason reason{SafetyReason::InternalError};
+    std::string detail;
+    std::string serialized_response;
+    std::uint64_t safety_epoch{0};
+    std::uint64_t device_generation{0};
+    bool hardware_submission_possible{false};
+};
+
+enum class CommandReservationStatus {
+    AcceptedNew,
+    JoinedInFlight,
+    CachedResult,
+    CommandIdConflict,
+    ResultEvicted,
+    LedgerExhausted,
+    Invalid,
+};
+
+class CommandLedger final {
+private:
+    struct State;
+
+public:
+    struct Config {
+        std::size_t result_capacity{4096};
+        std::size_t total_id_capacity{256U * 1024U};
+    };
+
+    class Ticket final {
+    public:
+        Ticket() = default;
+        bool valid() const noexcept { return state_ != nullptr; }
+
+    private:
+        friend class CommandLedger;
+        explicit Ticket(std::shared_ptr<State> state)
+            : state_(std::move(state))
+        {
+        }
+
+        std::shared_ptr<State> state_;
+    };
+
+    struct Reservation {
+        CommandReservationStatus status{CommandReservationStatus::Invalid};
+        Ticket ticket;
+        std::optional<CommandOutcome> cached_outcome;
+    };
+
+    CommandLedger();
+    explicit CommandLedger(Config config);
+
+    Reservation reserve(CommandKey key, std::string payload_hash);
+    bool setLifecycle(const Ticket& ticket,
+                      CommandLifecycle lifecycle,
+                      std::uint64_t safety_epoch = 0,
+                      std::uint64_t device_generation = 0,
+                      bool hardware_submission_possible = false);
+    bool complete(const Ticket& ticket, CommandOutcome outcome);
+    std::optional<CommandOutcome> wait(
+        const Ticket& ticket,
+        SafetyClock::time_point deadline = SafetyClock::time_point::max()) const;
+    std::optional<CommandOutcome> lookup(
+        const CommandKey& key,
+        const std::string& payload_hash) const;
+
+    std::size_t acceptedIdCount() const;
+    std::size_t liveRecordCount() const;
+    std::size_t retiredIdCount() const;
+
+private:
+    struct KeyHash {
+        std::size_t operator()(const CommandKey& key) const noexcept;
+    };
+
+    struct State {
+        CommandKey key;
+        std::string payload_hash;
+        mutable std::mutex mutex;
+        mutable std::condition_variable condition;
+        CommandLifecycle lifecycle{CommandLifecycle::Reserved};
+        std::optional<CommandOutcome> outcome;
+        std::uint64_t safety_epoch{0};
+        std::uint64_t device_generation{0};
+        bool hardware_submission_possible{false};
+        bool terminal{false};
+    };
+
+    static bool validKey_(const CommandKey& key) noexcept;
+    void trimTerminalResultsLocked_();
+
+    const Config config_;
+    mutable std::mutex mutex_;
+    std::unordered_map<CommandKey, std::shared_ptr<State>, KeyHash> records_;
+    std::unordered_map<CommandKey, std::string, KeyHash> retired_ids_;
+    std::vector<CommandKey> terminal_order_;
+    std::size_t terminal_result_count_{0};
+};
+
+const char* toString(CommandReservationStatus status) noexcept;
+
+} // namespace cmvr::safety

cmvr-es/manager/safety_manager/include/device_safety_endpoint.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#include <functional>
+#include <memory>
+
+#include "manager/safety_manager/include/safety_types.h"
+
+namespace cmvr::safety {
+
+using SafetySnapshotPublisher =
+    std::function<bool(DeviceSafetySnapshot)>;
+
+class DeviceSafetyEndpoint {
+public:
+    virtual ~DeviceSafetyEndpoint() = default;
+
+    virtual DeviceSafetyDescriptor descriptor() const = 0;
+    virtual void bindPublisher(SafetySnapshotPublisher publisher) = 0;
+    virtual void requestSafetyRefresh() noexcept = 0;
+    // Called after a backend/session restart. Implementations must publish
+    // subsequent samples with this generation or remain fail-closed.
+    virtual void onDeviceGenerationChanged(
+        std::uint64_t generation) noexcept
+    {
+        (void)generation;
+    }
+    virtual HardwareCheckResult validateBeforeDispatch(
+        const AdmissionPermit& permit) = 0;
+    virtual RecoveryCheckResult reconcileAdmissionState(
+        const RecoveryContext& context) = 0;
+};
+
+class DeviceSafetyEndpointProvider {
+public:
+    virtual ~DeviceSafetyEndpointProvider() = default;
+    virtual std::shared_ptr<DeviceSafetyEndpoint> safetyEndpoint() = 0;
+};
+
+} // namespace cmvr::safety

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

@@ -0,0 +1,225 @@
+#pragma once
+
+#include <chrono>
+#include <cstddef>
+#include <cstdint>
+#include <functional>
+#include <memory>
+#include <optional>
+#include <string>
+#include <unordered_set>
+#include <vector>
+
+#include "manager/safety_manager/include/command_ledger.h"
+#include "manager/safety_manager/include/device_safety_endpoint.h"
+#include "manager/safety_manager/include/safety_participant.h"
+#include "manager/safety_manager/include/safety_snapshot_store.h"
+
+namespace cmvr::safety {
+
+struct SafetyManagerConfig {
+    EnforcementMode enforcement_mode{EnforcementMode::Shadow};
+    std::unordered_set<std::string> enforced_device_ids;
+    std::chrono::milliseconds stop_all_timeout{15000};
+    std::chrono::milliseconds recovery_timeout{10000};
+    CommandLedger::Config command_ledger;
+    std::size_t event_history_capacity{2048};
+    bool fail_startup_on_missing_control_capability{false};
+};
+
+struct AdmissionResult {
+    AdmissionDecision decision;
+    std::optional<AdmissionPermit> permit;
+};
+
+struct StartupCoverageIssue {
+    std::string target_id;
+    SafetyReason reason{SafetyReason::None};
+    std::string detail;
+};
+
+struct StartupCoverageResult {
+    bool ready{false};
+    std::vector<StartupCoverageIssue> issues;
+};
+
+struct DeviceSafetyStateView {
+    DeviceSafetyDescriptor descriptor;
+    SafetySnapshotView safety;
+    device::ManagedDeviceState lifecycle{
+        device::ManagedDeviceState::Unknown};
+    device::DeviceHealthSnapshot health;
+    DeviceAdmissionState admission_state{DeviceAdmissionState::Observing};
+    std::vector<SafetyBlocker> blockers;
+};
+
+struct ParticipantResultView {
+    bool recorded{false};
+    bool success{false};
+    SafetyReason reason{SafetyReason::None};
+    std::string detail;
+};
+
+struct ParticipantSafetyStateView {
+    ParticipantDescriptor descriptor;
+    bool registered{false};
+    bool barrier_active{false};
+    bool barrier_retained{false};
+    std::string operation_id;
+    std::uint64_t safety_epoch{0};
+    ParticipantResultView last_request;
+    ParticipantResultView last_verify;
+    ParticipantResultView last_release;
+};
+
+struct SafetyManagerSnapshot {
+    SystemAdmissionState system_state{SystemAdmissionState::Starting};
+    std::uint64_t safety_epoch{0};
+    std::string service_instance_id;
+    EnforcementMode enforcement_mode{EnforcementMode::Shadow};
+    std::string active_operation_id;
+    std::string active_operation_phase;
+    std::vector<DeviceSafetyStateView> devices;
+    std::vector<ParticipantSafetyStateView> participants;
+    std::vector<SafetyEvent> recent_events;
+};
+
+struct SafetyTargetResult {
+    std::string target_id;
+    bool success{false};
+    SafetyReason reason{SafetyReason::None};
+    std::string detail;
+    DeviceAdmissionState before_state{DeviceAdmissionState::Observing};
+    DeviceAdmissionState after_state{DeviceAdmissionState::Observing};
+};
+
+struct StopAllResult {
+    bool success{false};
+    std::string operation_id;
+    std::uint64_t previous_safety_epoch{0};
+    std::uint64_t current_safety_epoch{0};
+    SystemAdmissionState system_state{SystemAdmissionState::Starting};
+    std::vector<SafetyTargetResult> targets;
+};
+
+enum class RecoveryResultCode {
+    Recovered,
+    VerifiedButStillBlocked,
+    BlockerRemains,
+    EpochMismatch,
+    NothingToRecover,
+    TimedOut,
+    Failed,
+};
+
+struct RecoveryRequest {
+    std::string recovery_id;
+    std::vector<std::string> device_ids;
+    bool all_devices{false};
+    std::uint64_t expected_safety_epoch{0};
+    bool verify_only{true};
+    std::string reason;
+    SafetyClock::time_point deadline{SafetyClock::time_point::max()};
+    // Called only for a latch-clearing transaction, after hardware facts have
+    // been verified and before any software barrier is reconciled or released.
+    // A false result leaves admission latched.
+    std::function<bool()> authorize_clear;
+};
+
+struct RecoveryResult {
+    RecoveryResultCode result{RecoveryResultCode::Failed};
+    std::string recovery_id;
+    std::uint64_t previous_safety_epoch{0};
+    std::uint64_t current_safety_epoch{0};
+    SystemAdmissionState system_state{SystemAdmissionState::Starting};
+    std::vector<SafetyTargetResult> targets;
+};
+
+class SafetyManager;
+
+class DispatchGuard final {
+public:
+    DispatchGuard() noexcept = default;
+    ~DispatchGuard() noexcept;
+    DispatchGuard(DispatchGuard&& other) noexcept;
+    DispatchGuard& operator=(DispatchGuard&& other) noexcept;
+    DispatchGuard(const DispatchGuard&) = delete;
+    DispatchGuard& operator=(const DispatchGuard&) = delete;
+
+    bool acquired() const noexcept { return coordinator_ != nullptr; }
+    const HardwareCheckResult& hardwareCheck() const noexcept
+    {
+        return hardware_check_;
+    }
+
+private:
+    friend class SafetyManager;
+    DispatchGuard(SafetyManager* coordinator,
+                  std::string device_id,
+                  HardwareCheckResult hardware_check) noexcept;
+    void reset_() noexcept;
+
+    SafetyManager* coordinator_{nullptr};
+    std::string device_id_;
+    HardwareCheckResult hardware_check_;
+};
+
+class SafetyManager final {
+public:
+    explicit SafetyManager(SafetyManagerConfig config = {});
+    ~SafetyManager();
+    SafetyManager(const SafetyManager&) = delete;
+    SafetyManager& operator=(const SafetyManager&) = delete;
+
+    bool registerDevice(DeviceSafetyRegistration registration);
+    bool unregisterDevice(const std::string& device_id);
+    bool registerParticipant(std::shared_ptr<SafetyParticipant> participant);
+    bool unregisterParticipant(const std::string& participant_id);
+
+    void updateDeviceRuntimeState(
+        const std::string& device_id,
+        device::ManagedDeviceState lifecycle,
+        device::DeviceHealthSnapshot health = {});
+    bool publishSafetySnapshot(DeviceSafetySnapshot snapshot);
+    std::optional<std::uint64_t> advanceDeviceGeneration(
+        const std::string& device_id);
+    StartupCoverageResult validateStartupCoverage(
+        SafetyClock::time_point deadline);
+    void markStartupComplete();
+    void beginShutdown() noexcept;
+
+    AdmissionDecision evaluate(const AdmissionRequest& request) const;
+    AdmissionResult admit(const AdmissionRequest& request);
+    // Lightweight session check. This validates the coordinator-owned epoch,
+    // generation, freshness, and admission state without calling the device
+    // endpoint or entering the hardware dispatch set.
+    HardwareCheckResult revalidatePermit(
+        const AdmissionPermit& permit) const;
+    DispatchGuard beginDispatch(const AdmissionPermit& permit);
+    void quarantineDevice(const std::string& device_id,
+                          SafetyReason reason,
+                          std::string operation_id = {});
+
+    StopAllResult stopAll(
+        std::string operation_id,
+        SafetyClock::time_point deadline = SafetyClock::time_point::max());
+    RecoveryResult recover(const RecoveryRequest& request);
+
+    SafetyManagerSnapshot snapshot() const;
+    SafetySnapshotStore& snapshotStore() noexcept;
+    const SafetySnapshotStore& snapshotStore() const noexcept;
+    CommandLedger& commandLedger() noexcept;
+    const CommandLedger& commandLedger() const noexcept;
+    const std::string& serviceInstanceId() const noexcept;
+    const SafetyManagerConfig& config() const noexcept;
+
+private:
+    friend class DispatchGuard;
+    struct Impl;
+    void endDispatch_(const std::string& device_id) noexcept;
+    std::unique_ptr<Impl> impl_;
+};
+
+const char* toString(RecoveryResultCode value) noexcept;
+
+} // namespace cmvr::safety

cmvr-es/manager/safety_manager/include/safety_participant.h

@@ -0,0 +1,88 @@
+#pragma once
+
+#include <chrono>
+#include <cstdint>
+#include <memory>
+#include <string>
+
+#include "manager/safety_manager/include/safety_types.h"
+
+namespace cmvr::safety {
+
+class DeviceSafetyEndpoint;
+
+enum class ParticipantPhase {
+    Ingress,
+    Scheduler,
+    ControlSession,
+    Actuator,
+    PeripheralActivity,
+    Verification,
+};
+
+struct ParticipantDescriptor {
+    std::string participant_id;
+    ParticipantPhase phase{ParticipantPhase::Actuator};
+    bool required{true};
+    std::chrono::milliseconds timeout{5000};
+};
+
+struct SafetyOperationContext {
+    std::string operation_id;
+    std::uint64_t safety_epoch{0};
+    SafetyClock::time_point deadline{SafetyClock::time_point::max()};
+};
+
+struct BarrierToken {
+    std::string participant_id;
+    std::string operation_id;
+    std::uint64_t safety_epoch{0};
+    std::uint64_t generation{0};
+
+    bool valid() const noexcept
+    {
+        return !participant_id.empty() && !operation_id.empty() &&
+               safety_epoch != 0 && generation != 0;
+    }
+};
+
+struct ParticipantResult {
+    bool success{false};
+    SafetyReason reason{SafetyReason::StopUnconfirmed};
+    std::string detail;
+};
+
+class SafetyParticipant {
+public:
+    virtual ~SafetyParticipant() = default;
+    virtual ParticipantDescriptor descriptor() const = 0;
+    virtual BarrierToken beginBarrier(
+        const SafetyOperationContext& context) = 0;
+    virtual ParticipantResult requestQuiesce(
+        const BarrierToken& token,
+        const SafetyOperationContext& context) = 0;
+    virtual ParticipantResult verifyQuiescent(
+        const BarrierToken& token,
+        const SafetyOperationContext& context) = 0;
+    virtual RecoveryCheckResult recoverAdmission(
+        const BarrierToken& token,
+        const RecoveryContext& context) = 0;
+    // Commits the participant's admission reopening. A failed commit must
+    // leave that participant fail-closed and be retryable through recovery.
+    virtual ParticipantResult releaseBarrier(
+        const BarrierToken& token) noexcept = 0;
+};
+
+class SafetyParticipantProvider {
+public:
+    virtual ~SafetyParticipantProvider() = default;
+    virtual std::shared_ptr<SafetyParticipant> safetyParticipant() = 0;
+};
+
+struct DeviceSafetyRegistration {
+    DeviceSafetyDescriptor descriptor;
+    std::shared_ptr<DeviceSafetyEndpoint> endpoint;
+    std::shared_ptr<SafetyParticipant> participant;
+};
+
+} // namespace cmvr::safety

cmvr-es/manager/safety_manager/include/safety_reason.h

@@ -0,0 +1,48 @@
+#pragma once
+
+#include <string>
+
+namespace cmvr::safety {
+
+enum class SafetyReason {
+    None,
+    InvalidArgument,
+    Unauthenticated,
+    PermissionDenied,
+    RecoveryRpcDisabled,
+    DeviceNotFound,
+    DeviceUnavailable,
+    UnsupportedCommand,
+    SystemStarting,
+    SystemStopping,
+    SafetyLatched,
+    SafetyStateMissing,
+    SafetyStateStale,
+    HardwareUnsafe,
+    EmergencyStopActive,
+    ProtectiveStopActive,
+    DeviceDisconnected,
+    DeviceFault,
+    DeviceNotReady,
+    DeviceStillMoving,
+    ControlBusy,
+    GenerationMismatch,
+    CommandIdRequired,
+    CommandIdConflict,
+    ResultEvicted,
+    LedgerExhausted,
+    Backpressure,
+    DeadlineExceededBeforeDispatch,
+    OutcomeUnknown,
+    ParticipantTimeout,
+    StopUnconfirmed,
+    RecoveryEpochMismatch,
+    RecoveryReasonRequired,
+    RecoveryAuditFailed,
+    InternalError,
+};
+
+const char* toString(SafetyReason reason) noexcept;
+bool retryWithSameCommandId(SafetyReason reason) noexcept;
+
+} // namespace cmvr::safety

cmvr-es/manager/safety_manager/include/safety_snapshot_store.h

@@ -0,0 +1,54 @@
+#pragma once
+
+#include <condition_variable>
+#include <optional>
+#include <shared_mutex>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "manager/safety_manager/include/safety_types.h"
+
+namespace cmvr::safety {
+
+class SafetySnapshotStore final {
+public:
+    bool registerDevice(const DeviceSafetyDescriptor& descriptor,
+                        std::uint64_t initial_generation = 1);
+    bool unregisterDevice(const std::string& device_id);
+    bool publish(DeviceSafetySnapshot snapshot);
+    bool markUnknown(const std::string& device_id,
+                     SafetyReason reason,
+                     std::string source_id = {});
+    std::optional<std::uint64_t> bumpGeneration(
+        const std::string& device_id);
+
+    SafetySnapshotView get(
+        const std::string& device_id,
+        SafetyClock::time_point now = SafetyClock::now()) const;
+    std::vector<SafetySnapshotView> snapshot(
+        SafetyClock::time_point now = SafetyClock::now()) const;
+
+    bool waitForNewerSample(
+        const std::string& device_id,
+        std::uint64_t previous_sequence,
+        SafetyClock::time_point deadline,
+        SafetySnapshotView& result) const;
+
+private:
+    struct Slot {
+        DeviceSafetyDescriptor descriptor;
+        DeviceSafetySnapshot snapshot;
+        bool has_sample{false};
+    };
+
+    static SafetySnapshotView viewOf_(
+        const Slot& slot,
+        SafetyClock::time_point now);
+
+    mutable std::shared_mutex mutex_;
+    mutable std::condition_variable_any changed_;
+    std::unordered_map<std::string, Slot> slots_;
+};
+
+} // namespace cmvr::safety

cmvr-es/manager/safety_manager/include/safety_types.h

@@ -0,0 +1,235 @@
+#pragma once
+
+#include <chrono>
+#include <cstdint>
+#include <optional>
+#include <string>
+#include <vector>
+
+#include "devices/device_types.h"
+#include "manager/safety_manager/include/safety_reason.h"
+
+namespace cmvr::safety {
+
+using SafetyClock = std::chrono::steady_clock;
+
+enum class TriState {
+    Unknown,
+    False,
+    True,
+};
+
+enum class SafetyCondition {
+    Nominal,
+    Restricted,
+    Unsafe,
+    Unknown,
+};
+
+enum class CommandIntent {
+    Observe,
+    StartActivity,
+    Configure,
+    Actuate,
+    Stop,
+    ResetFault,
+    RecoverAdmission,
+};
+
+enum class SafetyPolicyFamily {
+    Sensor,
+    Control,
+};
+
+enum class BlockerScope {
+    Device,
+    System,
+};
+
+enum class RecoveryRequirement {
+    RefreshOnly,
+    ClearSoftwareLatch,
+    HardwareReleaseRequired,
+    ManualInspectionRequired,
+};
+
+enum class SystemAdmissionState {
+    Starting,
+    Open,
+    Stopping,
+    Latched,
+    Recovering,
+    ShuttingDown,
+};
+
+enum class DeviceAdmissionState {
+    Observing,
+    Open,
+    Blocked,
+    Quarantined,
+    Recovering,
+    Removed,
+};
+
+enum class EnforcementMode {
+    Legacy,
+    Shadow,
+    EnforceSelected,
+    EnforceAll,
+};
+
+enum class CommandLifecycle {
+    Received,
+    Reserved,
+    RejectedBeforeDispatch,
+    Admitted,
+    Dispatching,
+    AcceptedByHardware,
+    Completed,
+    Failed,
+    CanceledBeforeDispatch,
+    OutcomeUnknown,
+};
+
+struct SafetyBlocker {
+    SafetyReason reason{SafetyReason::None};
+    BlockerScope scope{BlockerScope::Device};
+    RecoveryRequirement recovery_requirement{
+        RecoveryRequirement::RefreshOnly};
+    std::string source_id;
+    std::string operation_id;
+    std::uint64_t first_observed_at_unix_ms{0};
+    std::uint64_t last_observed_at_unix_ms{0};
+};
+
+struct DeviceSafetySnapshot {
+    std::string device_id;
+    SafetyCondition condition{SafetyCondition::Unknown};
+    std::uint64_t device_generation{0};
+    std::uint64_t sample_sequence{0};
+    SafetyClock::time_point observed_at{};
+    std::uint64_t observed_at_unix_ms{0};
+
+    TriState connected{TriState::Unknown};
+    TriState operational_ready{TriState::Unknown};
+    TriState quiescent{TriState::Unknown};
+    TriState motion_active{TriState::Unknown};
+    TriState actuator_enabled{TriState::Unknown};
+    TriState emergency_stop_active{TriState::Unknown};
+    TriState protective_stop_active{TriState::Unknown};
+    TriState fault_active{TriState::Unknown};
+
+    std::vector<SafetyBlocker> blockers;
+};
+
+struct DeviceSafetyDescriptor {
+    std::string device_id;
+    device::DeviceKind kind{device::DeviceKind::Unknown};
+    SafetyPolicyFamily default_policy{SafetyPolicyFamily::Sensor};
+    std::chrono::milliseconds maximum_snapshot_age{1000};
+    bool requires_safe_stop{false};
+    bool supports_active_refresh{false};
+    bool supports_non_enabling_fault_reset{false};
+};
+
+struct SafetySnapshotView {
+    DeviceSafetyDescriptor descriptor;
+    DeviceSafetySnapshot snapshot;
+    bool registered{false};
+    bool has_sample{false};
+    bool fresh{false};
+    std::chrono::milliseconds sample_age{
+        std::chrono::milliseconds::max()};
+};
+
+struct CommandActor {
+    std::string principal_id{"anonymous"};
+    bool authenticated{false};
+    std::vector<std::string> roles;
+};
+
+struct CommandDescriptor {
+    std::string full_method_name;
+    CommandIntent intent{CommandIntent::Observe};
+    SafetyPolicyFamily policy_family{SafetyPolicyFamily::Sensor};
+    bool mutating{false};
+    bool safety_lane{false};
+};
+
+struct AdmissionRequest {
+    CommandDescriptor command;
+    CommandActor actor;
+    std::string command_id;
+    std::string device_id;
+    std::optional<std::uint64_t> expected_device_generation;
+    std::uint64_t authority_generation{0};
+    SafetyClock::time_point deadline{SafetyClock::time_point::max()};
+};
+
+struct AdmissionDecision {
+    bool allowed{false};
+    bool policy_allowed{false};
+    bool enforced{false};
+    SafetyReason reason{SafetyReason::None};
+    std::string detail;
+    std::uint64_t safety_epoch{0};
+    std::uint64_t device_generation{0};
+};
+
+struct AdmissionPermit {
+    AdmissionPermit() = default;
+    AdmissionPermit(AdmissionPermit&&) noexcept = default;
+    AdmissionPermit& operator=(AdmissionPermit&&) noexcept = default;
+    AdmissionPermit(const AdmissionPermit&) = delete;
+    AdmissionPermit& operator=(const AdmissionPermit&) = delete;
+
+    std::string command_id;
+    std::string device_id;
+    CommandIntent intent{CommandIntent::Observe};
+    std::uint64_t safety_epoch{0};
+    std::uint64_t device_generation{0};
+    std::uint64_t authority_generation{0};
+    SafetyClock::time_point deadline{SafetyClock::time_point::max()};
+    bool policy_allowed{false};
+    bool enforced{false};
+};
+
+struct HardwareCheckResult {
+    bool safe{false};
+    SafetyReason reason{SafetyReason::SafetyStateMissing};
+    std::string detail;
+};
+
+struct RecoveryContext {
+    std::string recovery_id;
+    std::string reason;
+    std::uint64_t safety_epoch{0};
+    SafetyClock::time_point deadline{SafetyClock::time_point::max()};
+    bool verify_only{true};
+};
+
+struct RecoveryCheckResult {
+    bool reconciled{false};
+    SafetyReason reason{SafetyReason::None};
+    std::string detail;
+};
+
+struct SafetyEvent {
+    std::uint64_t sequence{0};
+    std::uint64_t safety_epoch{0};
+    std::uint64_t occurred_at_unix_ms{0};
+    std::string source_id;
+    std::string operation_id;
+    SafetyReason reason{SafetyReason::None};
+    std::string detail;
+};
+
+const char* toString(TriState value) noexcept;
+const char* toString(SafetyCondition value) noexcept;
+const char* toString(CommandIntent value) noexcept;
+const char* toString(SafetyPolicyFamily value) noexcept;
+const char* toString(SystemAdmissionState value) noexcept;
+const char* toString(DeviceAdmissionState value) noexcept;
+const char* toString(EnforcementMode value) noexcept;
+
+} // namespace cmvr::safety

cmvr-es/manager/safety_manager/src/command_ledger.cpp

@@ -0,0 +1,276 @@
+#include "manager/safety_manager/include/command_ledger.h"
+
+#include <algorithm>
+#include <cctype>
+#include <functional>
+#include <stdexcept>
+#include <utility>
+
+namespace cmvr::safety {
+
+namespace {
+
+constexpr std::size_t kMaxPrincipalIdLength = 256;
+constexpr std::size_t kMaxCommandIdLength = 128;
+
+bool validIdentifier(const std::string& value, const std::size_t maximum)
+{
+    if (value.empty() || value.size() > maximum) {
+        return false;
+    }
+    return std::all_of(
+        value.begin(), value.end(), [](const unsigned char character) {
+            return std::isalnum(character) || character == '-' ||
+                   character == '_' || character == '.' ||
+                   character == ':' || character == '/';
+        });
+}
+
+} // namespace
+
+CommandLedger::CommandLedger()
+    : CommandLedger(Config{})
+{
+}
+
+CommandLedger::CommandLedger(Config config)
+    : config_(config)
+{
+    if (config_.total_id_capacity == 0 ||
+        config_.result_capacity > config_.total_id_capacity) {
+        throw std::invalid_argument("invalid CommandLedger capacity");
+    }
+}
+
+std::size_t CommandLedger::KeyHash::operator()(
+    const CommandKey& key) const noexcept
+{
+    const auto first = std::hash<std::string>{}(key.effective_principal_id);
+    const auto second = std::hash<std::string>{}(key.command_id);
+    return first ^ (second + 0x9e3779b9U + (first << 6U) + (first >> 2U));
+}
+
+bool CommandLedger::validKey_(const CommandKey& key) noexcept
+{
+    return validIdentifier(
+               key.effective_principal_id, kMaxPrincipalIdLength) &&
+           validIdentifier(key.command_id, kMaxCommandIdLength);
+}
+
+CommandLedger::Reservation CommandLedger::reserve(
+    CommandKey key,
+    std::string payload_hash)
+{
+    if (!validKey_(key) || payload_hash.empty()) {
+        return {};
+    }
+
+    std::lock_guard lock(mutex_);
+    const auto live = records_.find(key);
+    if (live != records_.end()) {
+        const auto& state = live->second;
+        std::lock_guard state_lock(state->mutex);
+        if (state->payload_hash != payload_hash) {
+            return {CommandReservationStatus::CommandIdConflict, {}, {}};
+        }
+        if (state->terminal && state->outcome.has_value()) {
+            return {
+                CommandReservationStatus::CachedResult,
+                Ticket(state),
+                state->outcome};
+        }
+        return {
+            CommandReservationStatus::JoinedInFlight,
+            Ticket(state),
+            {}};
+    }
+
+    const auto retired = retired_ids_.find(key);
+    if (retired != retired_ids_.end()) {
+        return {
+            retired->second == payload_hash
+                ? CommandReservationStatus::ResultEvicted
+                : CommandReservationStatus::CommandIdConflict,
+            {},
+            {}};
+    }
+
+    if (records_.size() + retired_ids_.size() >=
+        config_.total_id_capacity) {
+        return {CommandReservationStatus::LedgerExhausted, {}, {}};
+    }
+
+    auto state = std::make_shared<State>();
+    state->key = std::move(key);
+    state->payload_hash = std::move(payload_hash);
+    const auto inserted = records_.emplace(state->key, state);
+    if (!inserted.second) {
+        throw std::logic_error("CommandLedger duplicate insertion");
+    }
+    return {
+        CommandReservationStatus::AcceptedNew,
+        Ticket(std::move(state)),
+        {}};
+}
+
+bool CommandLedger::setLifecycle(
+    const Ticket& ticket,
+    const CommandLifecycle lifecycle,
+    const std::uint64_t safety_epoch,
+    const std::uint64_t device_generation,
+    const bool hardware_submission_possible)
+{
+    if (!ticket.valid()) {
+        return false;
+    }
+    std::lock_guard ledger_lock(mutex_);
+    const auto found = records_.find(ticket.state_->key);
+    if (found == records_.end() || found->second != ticket.state_) {
+        return false;
+    }
+    std::lock_guard state_lock(ticket.state_->mutex);
+    if (ticket.state_->terminal) {
+        return false;
+    }
+    ticket.state_->lifecycle = lifecycle;
+    ticket.state_->safety_epoch = safety_epoch;
+    ticket.state_->device_generation = device_generation;
+    ticket.state_->hardware_submission_possible =
+        ticket.state_->hardware_submission_possible ||
+        hardware_submission_possible;
+    return true;
+}
+
+bool CommandLedger::complete(const Ticket& ticket, CommandOutcome outcome)
+{
+    if (!ticket.valid()) {
+        return false;
+    }
+
+    std::lock_guard ledger_lock(mutex_);
+    const auto found = records_.find(ticket.state_->key);
+    if (found == records_.end() || found->second != ticket.state_) {
+        return false;
+    }
+
+    {
+        std::lock_guard state_lock(ticket.state_->mutex);
+        if (ticket.state_->terminal) {
+            return false;
+        }
+        outcome.hardware_submission_possible =
+            outcome.hardware_submission_possible ||
+            ticket.state_->hardware_submission_possible;
+        if (outcome.safety_epoch == 0) {
+            outcome.safety_epoch = ticket.state_->safety_epoch;
+        }
+        if (outcome.device_generation == 0) {
+            outcome.device_generation = ticket.state_->device_generation;
+        }
+        ticket.state_->lifecycle = outcome.lifecycle;
+        ticket.state_->outcome = std::move(outcome);
+        ticket.state_->terminal = true;
+    }
+    ticket.state_->condition.notify_all();
+    terminal_order_.push_back(ticket.state_->key);
+    ++terminal_result_count_;
+    trimTerminalResultsLocked_();
+    return true;
+}
+
+void CommandLedger::trimTerminalResultsLocked_()
+{
+    std::size_t consumed = 0;
+    while (terminal_result_count_ > config_.result_capacity &&
+           consumed < terminal_order_.size()) {
+        const auto key = terminal_order_[consumed++];
+        const auto found = records_.find(key);
+        if (found == records_.end()) {
+            continue;
+        }
+        const auto& state = found->second;
+        std::lock_guard state_lock(state->mutex);
+        if (!state->terminal) {
+            continue;
+        }
+        retired_ids_.emplace(state->key, state->payload_hash);
+        records_.erase(found);
+        --terminal_result_count_;
+    }
+    if (consumed != 0) {
+        terminal_order_.erase(
+            terminal_order_.begin(),
+            terminal_order_.begin() + static_cast<std::ptrdiff_t>(consumed));
+    }
+}
+
+std::optional<CommandOutcome> CommandLedger::wait(
+    const Ticket& ticket,
+    const SafetyClock::time_point deadline) const
+{
+    if (!ticket.valid()) {
+        return std::nullopt;
+    }
+    std::unique_lock lock(ticket.state_->mutex);
+    if (deadline == SafetyClock::time_point::max()) {
+        ticket.state_->condition.wait(
+            lock, [&ticket] { return ticket.state_->terminal; });
+    } else if (!ticket.state_->condition.wait_until(
+                   lock, deadline,
+                   [&ticket] { return ticket.state_->terminal; })) {
+        return std::nullopt;
+    }
+    return ticket.state_->outcome;
+}
+
+std::optional<CommandOutcome> CommandLedger::lookup(
+    const CommandKey& key,
+    const std::string& payload_hash) const
+{
+    std::lock_guard lock(mutex_);
+    const auto found = records_.find(key);
+    if (found == records_.end()) {
+        return std::nullopt;
+    }
+    std::lock_guard state_lock(found->second->mutex);
+    if (found->second->payload_hash != payload_hash ||
+        !found->second->terminal) {
+        return std::nullopt;
+    }
+    return found->second->outcome;
+}
+
+std::size_t CommandLedger::acceptedIdCount() const
+{
+    std::lock_guard lock(mutex_);
+    return records_.size() + retired_ids_.size();
+}
+
+std::size_t CommandLedger::liveRecordCount() const
+{
+    std::lock_guard lock(mutex_);
+    return records_.size();
+}
+
+std::size_t CommandLedger::retiredIdCount() const
+{
+    std::lock_guard lock(mutex_);
+    return retired_ids_.size();
+}
+
+const char* toString(const CommandReservationStatus status) noexcept
+{
+    switch (status) {
+    case CommandReservationStatus::AcceptedNew: return "AcceptedNew";
+    case CommandReservationStatus::JoinedInFlight: return "JoinedInFlight";
+    case CommandReservationStatus::CachedResult: return "CachedResult";
+    case CommandReservationStatus::CommandIdConflict:
+        return "CommandIdConflict";
+    case CommandReservationStatus::ResultEvicted: return "ResultEvicted";
+    case CommandReservationStatus::LedgerExhausted: return "LedgerExhausted";
+    case CommandReservationStatus::Invalid: return "Invalid";
+    }
+    return "Invalid";
+}
+
+} // namespace cmvr::safety

cmvr-es/manager/safety_manager/src/safety_reason.cpp

@@ -0,0 +1,92 @@
+#include "manager/safety_manager/include/safety_reason.h"
+
+namespace cmvr::safety {
+
+const char* toString(const SafetyReason reason) noexcept
+{
+    switch (reason) {
+    case SafetyReason::None: return "NONE";
+    case SafetyReason::InvalidArgument: return "INVALID_ARGUMENT";
+    case SafetyReason::Unauthenticated: return "UNAUTHENTICATED";
+    case SafetyReason::PermissionDenied: return "PERMISSION_DENIED";
+    case SafetyReason::RecoveryRpcDisabled: return "RECOVERY_RPC_DISABLED";
+    case SafetyReason::DeviceNotFound: return "DEVICE_NOT_FOUND";
+    case SafetyReason::DeviceUnavailable: return "DEVICE_UNAVAILABLE";
+    case SafetyReason::UnsupportedCommand: return "UNSUPPORTED_COMMAND";
+    case SafetyReason::SystemStarting: return "SYSTEM_STARTING";
+    case SafetyReason::SystemStopping: return "SYSTEM_STOPPING";
+    case SafetyReason::SafetyLatched: return "SAFETY_LATCHED";
+    case SafetyReason::SafetyStateMissing: return "SAFETY_STATE_MISSING";
+    case SafetyReason::SafetyStateStale: return "SAFETY_STATE_STALE";
+    case SafetyReason::HardwareUnsafe: return "HARDWARE_UNSAFE";
+    case SafetyReason::EmergencyStopActive: return "EMERGENCY_STOP_ACTIVE";
+    case SafetyReason::ProtectiveStopActive: return "PROTECTIVE_STOP_ACTIVE";
+    case SafetyReason::DeviceDisconnected: return "DEVICE_DISCONNECTED";
+    case SafetyReason::DeviceFault: return "DEVICE_FAULT";
+    case SafetyReason::DeviceNotReady: return "DEVICE_NOT_READY";
+    case SafetyReason::DeviceStillMoving: return "DEVICE_STILL_MOVING";
+    case SafetyReason::ControlBusy: return "CONTROL_BUSY";
+    case SafetyReason::GenerationMismatch: return "GENERATION_MISMATCH";
+    case SafetyReason::CommandIdRequired: return "COMMAND_ID_REQUIRED";
+    case SafetyReason::CommandIdConflict: return "COMMAND_ID_CONFLICT";
+    case SafetyReason::ResultEvicted: return "RESULT_EVICTED";
+    case SafetyReason::LedgerExhausted: return "LEDGER_EXHAUSTED";
+    case SafetyReason::Backpressure: return "BACKPRESSURE";
+    case SafetyReason::DeadlineExceededBeforeDispatch:
+        return "DEADLINE_EXCEEDED_BEFORE_DISPATCH";
+    case SafetyReason::OutcomeUnknown: return "OUTCOME_UNKNOWN";
+    case SafetyReason::ParticipantTimeout: return "PARTICIPANT_TIMEOUT";
+    case SafetyReason::StopUnconfirmed: return "STOP_UNCONFIRMED";
+    case SafetyReason::RecoveryEpochMismatch: return "RECOVERY_EPOCH_MISMATCH";
+    case SafetyReason::RecoveryReasonRequired: return "RECOVERY_REASON_REQUIRED";
+    case SafetyReason::RecoveryAuditFailed: return "RECOVERY_AUDIT_FAILED";
+    case SafetyReason::InternalError: return "INTERNAL_ERROR";
+    }
+    return "INTERNAL_ERROR";
+}
+
+bool retryWithSameCommandId(const SafetyReason reason) noexcept
+{
+    switch (reason) {
+    case SafetyReason::RecoveryRpcDisabled:
+    case SafetyReason::SystemStopping:
+        return true;
+    case SafetyReason::None:
+    case SafetyReason::InvalidArgument:
+    case SafetyReason::Unauthenticated:
+    case SafetyReason::PermissionDenied:
+    case SafetyReason::DeviceNotFound:
+    case SafetyReason::DeviceUnavailable:
+    case SafetyReason::UnsupportedCommand:
+    case SafetyReason::SystemStarting:
+    case SafetyReason::SafetyLatched:
+    case SafetyReason::SafetyStateMissing:
+    case SafetyReason::SafetyStateStale:
+    case SafetyReason::HardwareUnsafe:
+    case SafetyReason::EmergencyStopActive:
+    case SafetyReason::ProtectiveStopActive:
+    case SafetyReason::DeviceDisconnected:
+    case SafetyReason::DeviceFault:
+    case SafetyReason::DeviceNotReady:
+    case SafetyReason::DeviceStillMoving:
+    case SafetyReason::ControlBusy:
+    case SafetyReason::GenerationMismatch:
+    case SafetyReason::CommandIdRequired:
+    case SafetyReason::CommandIdConflict:
+    case SafetyReason::ResultEvicted:
+    case SafetyReason::LedgerExhausted:
+    case SafetyReason::Backpressure:
+    case SafetyReason::DeadlineExceededBeforeDispatch:
+    case SafetyReason::OutcomeUnknown:
+    case SafetyReason::ParticipantTimeout:
+    case SafetyReason::StopUnconfirmed:
+    case SafetyReason::RecoveryEpochMismatch:
+    case SafetyReason::RecoveryReasonRequired:
+    case SafetyReason::RecoveryAuditFailed:
+    case SafetyReason::InternalError:
+        return false;
+    }
+    return false;
+}
+
+} // namespace cmvr::safety

cmvr-es/manager/safety_manager/src/safety_snapshot_store.cpp

@@ -0,0 +1,305 @@
+#include "manager/safety_manager/include/safety_snapshot_store.h"
+
+#include <algorithm>
+#include <limits>
+#include <mutex>
+#include <utility>
+
+namespace cmvr::safety {
+
+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;
+}
+
+} // namespace
+
+bool SafetySnapshotStore::registerDevice(
+    const DeviceSafetyDescriptor& descriptor,
+    const std::uint64_t initial_generation)
+{
+    if (descriptor.device_id.empty() ||
+        descriptor.maximum_snapshot_age <= std::chrono::milliseconds::zero() ||
+        initial_generation == 0) {
+        return false;
+    }
+
+    Slot slot;
+    slot.descriptor = descriptor;
+    slot.snapshot.device_id = descriptor.device_id;
+    slot.snapshot.device_generation = initial_generation;
+    slot.snapshot.condition = SafetyCondition::Unknown;
+
+    std::unique_lock lock(mutex_);
+    const auto inserted = slots_.emplace(descriptor.device_id, std::move(slot));
+    if (inserted.second) {
+        changed_.notify_all();
+    }
+    return inserted.second;
+}
+
+bool SafetySnapshotStore::unregisterDevice(const std::string& device_id)
+{
+    std::unique_lock lock(mutex_);
+    const bool removed = slots_.erase(device_id) != 0;
+    if (removed) {
+        changed_.notify_all();
+    }
+    return removed;
+}
+
+bool SafetySnapshotStore::publish(DeviceSafetySnapshot snapshot)
+{
+    if (snapshot.device_id.empty() || snapshot.device_generation == 0 ||
+        snapshot.sample_sequence == 0 ||
+        snapshot.observed_at == SafetyClock::time_point{}) {
+        return false;
+    }
+
+    std::unique_lock lock(mutex_);
+    const auto found = slots_.find(snapshot.device_id);
+    if (found == slots_.end()) {
+        return false;
+    }
+
+    auto& slot = found->second;
+    const auto current_generation = slot.snapshot.device_generation;
+    if (snapshot.device_generation < current_generation) {
+        return false;
+    }
+    if (snapshot.device_generation == current_generation &&
+        slot.has_sample &&
+        snapshot.sample_sequence <= slot.snapshot.sample_sequence) {
+        return false;
+    }
+    if (snapshot.observed_at_unix_ms == 0) {
+        snapshot.observed_at_unix_ms = unixTimeMs();
+    }
+    slot.snapshot = std::move(snapshot);
+    slot.has_sample = true;
+    changed_.notify_all();
+    return true;
+}
+
+bool SafetySnapshotStore::markUnknown(
+    const std::string& device_id,
+    const SafetyReason reason,
+    std::string source_id)
+{
+    std::unique_lock lock(mutex_);
+    const auto found = slots_.find(device_id);
+    if (found == slots_.end()) {
+        return false;
+    }
+
+    auto& slot = found->second;
+    DeviceSafetySnapshot snapshot;
+    snapshot.device_id = device_id;
+    snapshot.device_generation = slot.snapshot.device_generation;
+    snapshot.sample_sequence = slot.snapshot.sample_sequence + 1U;
+    if (snapshot.sample_sequence == 0) {
+        snapshot.sample_sequence = 1U;
+    }
+    snapshot.observed_at = SafetyClock::now();
+    snapshot.observed_at_unix_ms = unixTimeMs();
+    snapshot.condition = SafetyCondition::Unknown;
+    snapshot.blockers.push_back(SafetyBlocker{
+        reason,
+        BlockerScope::Device,
+        RecoveryRequirement::RefreshOnly,
+        source_id.empty() ? device_id : std::move(source_id),
+        {},
+        snapshot.observed_at_unix_ms,
+        snapshot.observed_at_unix_ms});
+    slot.snapshot = std::move(snapshot);
+    slot.has_sample = true;
+    changed_.notify_all();
+    return true;
+}
+
+std::optional<std::uint64_t> SafetySnapshotStore::bumpGeneration(
+    const std::string& device_id)
+{
+    std::unique_lock lock(mutex_);
+    const auto found = slots_.find(device_id);
+    if (found == slots_.end()) {
+        return std::nullopt;
+    }
+    auto& slot = found->second;
+    if (slot.snapshot.device_generation ==
+        std::numeric_limits<std::uint64_t>::max()) {
+        return std::nullopt;
+    }
+    ++slot.snapshot.device_generation;
+    slot.snapshot.sample_sequence = 0;
+    slot.snapshot.observed_at = {};
+    slot.snapshot.observed_at_unix_ms = 0;
+    slot.snapshot.condition = SafetyCondition::Unknown;
+    slot.snapshot.blockers.clear();
+    slot.has_sample = false;
+    changed_.notify_all();
+    return slot.snapshot.device_generation;
+}
+
+SafetySnapshotView SafetySnapshotStore::viewOf_(
+    const Slot& slot,
+    const SafetyClock::time_point now)
+{
+    SafetySnapshotView view;
+    view.descriptor = slot.descriptor;
+    view.snapshot = slot.snapshot;
+    view.registered = true;
+    view.has_sample = slot.has_sample;
+    if (!slot.has_sample ||
+        slot.snapshot.observed_at == SafetyClock::time_point{}) {
+        return view;
+    }
+
+    const auto elapsed = now <= slot.snapshot.observed_at
+        ? SafetyClock::duration::zero()
+        : now - slot.snapshot.observed_at;
+    view.sample_age = std::chrono::duration_cast<std::chrono::milliseconds>(
+        elapsed);
+    view.fresh = view.sample_age <= slot.descriptor.maximum_snapshot_age;
+    return view;
+}
+
+SafetySnapshotView SafetySnapshotStore::get(
+    const std::string& device_id,
+    const SafetyClock::time_point now) const
+{
+    std::shared_lock lock(mutex_);
+    const auto found = slots_.find(device_id);
+    if (found == slots_.end()) {
+        return {};
+    }
+    return viewOf_(found->second, now);
+}
+
+std::vector<SafetySnapshotView> SafetySnapshotStore::snapshot(
+    const SafetyClock::time_point now) const
+{
+    std::vector<SafetySnapshotView> result;
+    std::shared_lock lock(mutex_);
+    result.reserve(slots_.size());
+    for (const auto& [id, slot] : slots_) {
+        (void)id;
+        result.push_back(viewOf_(slot, now));
+    }
+    std::sort(result.begin(), result.end(), [](const auto& lhs, const auto& rhs) {
+        return lhs.descriptor.device_id < rhs.descriptor.device_id;
+    });
+    return result;
+}
+
+bool SafetySnapshotStore::waitForNewerSample(
+    const std::string& device_id,
+    const std::uint64_t previous_sequence,
+    const SafetyClock::time_point deadline,
+    SafetySnapshotView& result) const
+{
+    std::unique_lock lock(mutex_);
+    const auto ready = [&]() {
+        const auto found = slots_.find(device_id);
+        return found == slots_.end() ||
+               (found->second.has_sample &&
+                found->second.snapshot.sample_sequence > previous_sequence);
+    };
+    if (!changed_.wait_until(lock, deadline, ready)) {
+        return false;
+    }
+    const auto found = slots_.find(device_id);
+    if (found == slots_.end()) {
+        return false;
+    }
+    result = viewOf_(found->second, SafetyClock::now());
+    return result.has_sample &&
+           result.snapshot.sample_sequence > previous_sequence;
+}
+
+const char* toString(const TriState value) noexcept
+{
+    switch (value) {
+    case TriState::Unknown: return "Unknown";
+    case TriState::False: return "False";
+    case TriState::True: return "True";
+    }
+    return "Unknown";
+}
+
+const char* toString(const SafetyCondition value) noexcept
+{
+    switch (value) {
+    case SafetyCondition::Nominal: return "Nominal";
+    case SafetyCondition::Restricted: return "Restricted";
+    case SafetyCondition::Unsafe: return "Unsafe";
+    case SafetyCondition::Unknown: return "Unknown";
+    }
+    return "Unknown";
+}
+
+const char* toString(const CommandIntent value) noexcept
+{
+    switch (value) {
+    case CommandIntent::Observe: return "Observe";
+    case CommandIntent::StartActivity: return "StartActivity";
+    case CommandIntent::Configure: return "Configure";
+    case CommandIntent::Actuate: return "Actuate";
+    case CommandIntent::Stop: return "Stop";
+    case CommandIntent::ResetFault: return "ResetFault";
+    case CommandIntent::RecoverAdmission: return "RecoverAdmission";
+    }
+    return "Observe";
+}
+
+const char* toString(const SafetyPolicyFamily value) noexcept
+{
+    switch (value) {
+    case SafetyPolicyFamily::Sensor: return "Sensor";
+    case SafetyPolicyFamily::Control: return "Control";
+    }
+    return "Sensor";
+}
+
+const char* toString(const SystemAdmissionState value) noexcept
+{
+    switch (value) {
+    case SystemAdmissionState::Starting: return "Starting";
+    case SystemAdmissionState::Open: return "Open";
+    case SystemAdmissionState::Stopping: return "Stopping";
+    case SystemAdmissionState::Latched: return "Latched";
+    case SystemAdmissionState::Recovering: return "Recovering";
+    case SystemAdmissionState::ShuttingDown: return "ShuttingDown";
+    }
+    return "Starting";
+}
+
+const char* toString(const DeviceAdmissionState value) noexcept
+{
+    switch (value) {
+    case DeviceAdmissionState::Observing: return "Observing";
+    case DeviceAdmissionState::Open: return "Open";
+    case DeviceAdmissionState::Blocked: return "Blocked";
+    case DeviceAdmissionState::Quarantined: return "Quarantined";
+    case DeviceAdmissionState::Recovering: return "Recovering";
+    case DeviceAdmissionState::Removed: return "Removed";
+    }
+    return "Observing";
+}
+
+const char* toString(const EnforcementMode value) noexcept
+{
+    switch (value) {
+    case EnforcementMode::Legacy: return "Legacy";
+    case EnforcementMode::Shadow: return "Shadow";
+    case EnforcementMode::EnforceSelected: return "EnforceSelected";
+    case EnforcementMode::EnforceAll: return "EnforceAll";
+    }
+    return "Legacy";
+}
+
+} // namespace cmvr::safety

cmvr-es/manager/safety_manager/tests/command_ledger_test.cpp

@@ -0,0 +1,122 @@
+#include "manager/safety_manager/include/command_ledger.h"
+
+#include <atomic>
+#include <chrono>
+#include <thread>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+namespace cmvr::safety {
+namespace {
+
+CommandKey key(const std::string& id)
+{
+    return {"anonymous", id};
+}
+
+CommandOutcome completedOutcome()
+{
+    CommandOutcome outcome;
+    outcome.lifecycle = CommandLifecycle::Completed;
+    outcome.reason = SafetyReason::None;
+    outcome.serialized_response = "done";
+    return outcome;
+}
+
+TEST(CommandLedgerTest, SameIdJoinsAndDifferentPayloadConflicts)
+{
+    CommandLedger ledger;
+    const auto first = ledger.reserve(key("command-1"), "payload-a");
+    ASSERT_EQ(first.status, CommandReservationStatus::AcceptedNew);
+    EXPECT_EQ(
+        ledger.reserve(key("command-1"), "payload-a").status,
+        CommandReservationStatus::JoinedInFlight);
+    EXPECT_EQ(
+        ledger.reserve(key("command-1"), "payload-b").status,
+        CommandReservationStatus::CommandIdConflict);
+
+    ASSERT_TRUE(ledger.complete(first.ticket, completedOutcome()));
+    const auto cached = ledger.reserve(key("command-1"), "payload-a");
+    ASSERT_EQ(cached.status, CommandReservationStatus::CachedResult);
+    ASSERT_TRUE(cached.cached_outcome.has_value());
+    EXPECT_EQ(cached.cached_outcome->serialized_response, "done");
+}
+
+TEST(CommandLedgerTest, ConcurrentCallersNeverCreateASecondReservation)
+{
+    CommandLedger ledger;
+    std::atomic<int> accepted{0};
+    std::vector<std::thread> workers;
+    for (int index = 0; index < 16; ++index) {
+        workers.emplace_back([&] {
+            const auto result = ledger.reserve(key("shared"), "payload");
+            if (result.status == CommandReservationStatus::AcceptedNew) {
+                accepted.fetch_add(1);
+            }
+        });
+    }
+    for (auto& worker : workers) {
+        worker.join();
+    }
+    EXPECT_EQ(accepted.load(), 1);
+    EXPECT_EQ(ledger.acceptedIdCount(), 1U);
+}
+
+TEST(CommandLedgerTest, OutcomeUnknownIsCachedAndNeverReservedAgain)
+{
+    CommandLedger ledger;
+    const auto first = ledger.reserve(key("uncertain"), "payload");
+    ASSERT_EQ(first.status, CommandReservationStatus::AcceptedNew);
+
+    CommandOutcome outcome;
+    outcome.lifecycle = CommandLifecycle::OutcomeUnknown;
+    outcome.reason = SafetyReason::OutcomeUnknown;
+    outcome.hardware_submission_possible = true;
+    ASSERT_TRUE(ledger.complete(first.ticket, outcome));
+
+    const auto retry = ledger.reserve(key("uncertain"), "payload");
+    ASSERT_EQ(retry.status, CommandReservationStatus::CachedResult);
+    ASSERT_TRUE(retry.cached_outcome.has_value());
+    EXPECT_EQ(
+        retry.cached_outcome->lifecycle, CommandLifecycle::OutcomeUnknown);
+    EXPECT_TRUE(retry.cached_outcome->hardware_submission_possible);
+}
+
+TEST(CommandLedgerTest, EvictedResultLeavesAnExactTombstone)
+{
+    CommandLedger ledger({1, 3});
+    auto first = ledger.reserve(key("first"), "payload-1");
+    ASSERT_TRUE(ledger.complete(first.ticket, completedOutcome()));
+    auto second = ledger.reserve(key("second"), "payload-2");
+    ASSERT_TRUE(ledger.complete(second.ticket, completedOutcome()));
+
+    EXPECT_EQ(
+        ledger.reserve(key("first"), "payload-1").status,
+        CommandReservationStatus::ResultEvicted);
+    EXPECT_EQ(
+        ledger.reserve(key("first"), "different").status,
+        CommandReservationStatus::CommandIdConflict);
+
+    auto third = ledger.reserve(key("third"), "payload-3");
+    ASSERT_EQ(third.status, CommandReservationStatus::AcceptedNew);
+    EXPECT_EQ(
+        ledger.reserve(key("fourth"), "payload-4").status,
+        CommandReservationStatus::LedgerExhausted);
+}
+
+TEST(CommandLedgerTest, WaitTimesOutWithoutChangingExecution)
+{
+    CommandLedger ledger;
+    const auto reservation = ledger.reserve(key("slow"), "payload");
+    ASSERT_EQ(reservation.status, CommandReservationStatus::AcceptedNew);
+    EXPECT_FALSE(ledger.wait(
+        reservation.ticket,
+        SafetyClock::now() + std::chrono::milliseconds(5)).has_value());
+    EXPECT_EQ(
+        ledger.reserve(key("slow"), "payload").status,
+        CommandReservationStatus::JoinedInFlight);
+}
+
+} // namespace
+} // namespace cmvr::safety

cmvr-es/manager/safety_manager/tests/safety_manager_test.cpp

@@ -0,0 +1,507 @@
+#include "manager/safety_manager/include/safety_manager.h"
+
+#include <atomic>
+#include <chrono>
+#include <memory>
+#include <string>
+
+#include <gtest/gtest.h>
+
+namespace cmvr::safety {
+namespace {
+
+DeviceSafetyDescriptor controlDescriptor()
+{
+    DeviceSafetyDescriptor descriptor;
+    descriptor.device_id = "arm";
+    descriptor.kind = device::DeviceKind::Arm;
+    descriptor.default_policy = SafetyPolicyFamily::Control;
+    descriptor.maximum_snapshot_age = std::chrono::seconds(1);
+    descriptor.requires_safe_stop = true;
+    descriptor.supports_active_refresh = true;
+    return descriptor;
+}
+
+class FakeEndpoint final : public DeviceSafetyEndpoint {
+public:
+    explicit FakeEndpoint(DeviceSafetyDescriptor descriptor)
+        : descriptor_(std::move(descriptor))
+    {
+    }
+
+    DeviceSafetyDescriptor descriptor() const override
+    {
+        return descriptor_;
+    }
+
+    void bindPublisher(SafetySnapshotPublisher publisher) override
+    {
+        publisher_ = std::move(publisher);
+    }
+
+    void requestSafetyRefresh() noexcept override
+    {
+        if (!publisher_ || !publish_on_refresh) {
+            return;
+        }
+        DeviceSafetySnapshot snapshot;
+        snapshot.device_id = descriptor_.device_id;
+        snapshot.condition = condition;
+        snapshot.device_generation = generation;
+        snapshot.sample_sequence = ++sequence;
+        snapshot.observed_at = SafetyClock::now();
+        snapshot.connected = connected;
+        snapshot.operational_ready = ready;
+        snapshot.quiescent = quiescent;
+        snapshot.motion_active =
+            quiescent == TriState::True ? TriState::False : TriState::Unknown;
+        snapshot.emergency_stop_active = emergency_stop;
+        snapshot.protective_stop_active = protective_stop;
+        snapshot.fault_active = fault;
+        (void)publisher_(std::move(snapshot));
+    }
+
+    HardwareCheckResult validateBeforeDispatch(
+        const AdmissionPermit&) override
+    {
+        ++hardware_checks;
+        return final_check;
+    }
+
+    RecoveryCheckResult reconcileAdmissionState(
+        const RecoveryContext&) override
+    {
+        ++recoveries;
+        return recovery_check;
+    }
+
+    DeviceSafetyDescriptor descriptor_;
+    SafetySnapshotPublisher publisher_;
+    SafetyCondition condition{SafetyCondition::Nominal};
+    TriState connected{TriState::True};
+    TriState ready{TriState::True};
+    TriState quiescent{TriState::True};
+    TriState emergency_stop{TriState::False};
+    TriState protective_stop{TriState::False};
+    TriState fault{TriState::False};
+    HardwareCheckResult final_check{true, SafetyReason::None, {}};
+    RecoveryCheckResult recovery_check{true, SafetyReason::None, {}};
+    std::uint64_t generation{1};
+    std::uint64_t sequence{0};
+    bool publish_on_refresh{true};
+    std::atomic<int> hardware_checks{0};
+    std::atomic<int> recoveries{0};
+};
+
+class FakeParticipant final : public SafetyParticipant {
+public:
+    ParticipantDescriptor descriptor() const override
+    {
+        return {"arm", ParticipantPhase::Actuator, true,
+                std::chrono::milliseconds(100)};
+    }
+
+    BarrierToken beginBarrier(
+        const SafetyOperationContext& context) override
+    {
+        ++barriers;
+        return {"arm", context.operation_id, context.safety_epoch,
+                static_cast<std::uint64_t>(barriers.load())};
+    }
+
+    ParticipantResult requestQuiesce(
+        const BarrierToken&,
+        const SafetyOperationContext&) override
+    {
+        ++stop_requests;
+        return stop_result;
+    }
+
+    ParticipantResult verifyQuiescent(
+        const BarrierToken&,
+        const SafetyOperationContext&) override
+    {
+        ++verifications;
+        return verify_result;
+    }
+
+    RecoveryCheckResult recoverAdmission(
+        const BarrierToken&,
+        const RecoveryContext&) override
+    {
+        ++recoveries;
+        return recovery_result;
+    }
+
+    ParticipantResult releaseBarrier(
+        const BarrierToken&) noexcept override
+    {
+        ++releases;
+        return release_result;
+    }
+
+    ParticipantResult stop_result{true, SafetyReason::None, {}};
+    ParticipantResult verify_result{true, SafetyReason::None, {}};
+    RecoveryCheckResult recovery_result{true, SafetyReason::None, {}};
+    ParticipantResult release_result{true, SafetyReason::None, {}};
+    std::atomic<int> barriers{0};
+    std::atomic<int> stop_requests{0};
+    std::atomic<int> verifications{0};
+    std::atomic<int> recoveries{0};
+    std::atomic<int> releases{0};
+};
+
+AdmissionRequest actuateRequest()
+{
+    AdmissionRequest request;
+    request.command = {
+        "/cmvr.api.ArmService/moveJ",
+        CommandIntent::Actuate,
+        SafetyPolicyFamily::Control,
+        true,
+        false};
+    request.device_id = "arm";
+    request.command_id = "command-1";
+    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
+    return request;
+}
+
+TEST(SafetyManagerTest, ShadowReportsDenyWithoutChangingLegacyBehavior)
+{
+    SafetyManager coordinator;
+    ASSERT_TRUE(coordinator.registerDevice({controlDescriptor(), {}, {}}));
+    coordinator.markStartupComplete();
+
+    const auto result = coordinator.admit(actuateRequest());
+    EXPECT_TRUE(result.decision.allowed);
+    EXPECT_FALSE(result.decision.policy_allowed);
+    EXPECT_FALSE(result.decision.enforced);
+    EXPECT_EQ(result.decision.reason, SafetyReason::SafetyStateMissing);
+    EXPECT_TRUE(result.permit.has_value());
+}
+
+TEST(SafetyManagerTest,
+     EnforceSelectedStartupRejectsEmptyOrUnknownCoverage)
+{
+    SafetyManagerConfig empty_config;
+    empty_config.enforcement_mode = EnforcementMode::EnforceSelected;
+    SafetyManager empty(empty_config);
+    const auto empty_result = empty.validateStartupCoverage(
+        SafetyClock::now() + std::chrono::milliseconds(10));
+    ASSERT_FALSE(empty_result.ready);
+    ASSERT_EQ(empty_result.issues.size(), 1U);
+    EXPECT_EQ(empty_result.issues.front().reason, SafetyReason::InvalidArgument);
+
+    SafetyManagerConfig missing_config;
+    missing_config.enforcement_mode = EnforcementMode::EnforceSelected;
+    missing_config.enforced_device_ids.insert("missing-arm");
+    SafetyManager missing(missing_config);
+    const auto missing_result = missing.validateStartupCoverage(
+        SafetyClock::now() + std::chrono::milliseconds(10));
+    ASSERT_FALSE(missing_result.ready);
+    ASSERT_EQ(missing_result.issues.size(), 1U);
+    EXPECT_EQ(missing_result.issues.front().target_id, "missing-arm");
+    EXPECT_EQ(missing_result.issues.front().reason, SafetyReason::DeviceNotFound);
+}
+
+TEST(SafetyManagerTest,
+     EnforceAllStartupRequiresEndpointParticipantAndFreshSnapshot)
+{
+    SafetyManagerConfig config;
+    config.enforcement_mode = EnforcementMode::EnforceAll;
+
+    SafetyManager missing_capability(config);
+    ASSERT_TRUE(missing_capability.registerDevice(
+        {controlDescriptor(), {}, {}}));
+    const auto structural = missing_capability.validateStartupCoverage(
+        SafetyClock::now() + std::chrono::milliseconds(10));
+    EXPECT_FALSE(structural.ready);
+    EXPECT_EQ(structural.issues.size(), 2U);
+
+    SafetyManager missing_sample(config);
+    auto silent_endpoint =
+        std::make_shared<FakeEndpoint>(controlDescriptor());
+    silent_endpoint->publish_on_refresh = false;
+    ASSERT_TRUE(missing_sample.registerDevice({
+        controlDescriptor(),
+        silent_endpoint,
+        std::make_shared<FakeParticipant>()}));
+    const auto stale = missing_sample.validateStartupCoverage(
+        SafetyClock::now() + std::chrono::milliseconds(10));
+    ASSERT_FALSE(stale.ready);
+    ASSERT_EQ(stale.issues.size(), 1U);
+    EXPECT_EQ(stale.issues.front().reason, SafetyReason::SafetyStateMissing);
+}
+
+TEST(SafetyManagerTest,
+     HardwareUnsafeSnapshotBlocksAdmissionButNotStructuralStartup)
+{
+    SafetyManagerConfig config;
+    config.enforcement_mode = EnforcementMode::EnforceAll;
+    SafetyManager coordinator(config);
+    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
+    endpoint->condition = SafetyCondition::Unsafe;
+    endpoint->emergency_stop = TriState::True;
+    ASSERT_TRUE(coordinator.registerDevice({
+        controlDescriptor(), endpoint, std::make_shared<FakeParticipant>()}));
+    coordinator.updateDeviceRuntimeState(
+        "arm", device::ManagedDeviceState::Running,
+        {device::DeviceHealthState::Healthy, {}});
+
+    const auto coverage = coordinator.validateStartupCoverage(
+        SafetyClock::now() + std::chrono::milliseconds(50));
+    EXPECT_TRUE(coverage.ready);
+    coordinator.markStartupComplete();
+    const auto admission = coordinator.admit(actuateRequest());
+    EXPECT_FALSE(admission.decision.allowed);
+    EXPECT_EQ(
+        admission.decision.reason, SafetyReason::EmergencyStopActive);
+}
+
+TEST(SafetyManagerTest, EnforceAllFailsClosedOnUnknownControlState)
+{
+    SafetyManagerConfig config;
+    config.enforcement_mode = EnforcementMode::EnforceAll;
+    SafetyManager coordinator(config);
+    ASSERT_TRUE(coordinator.registerDevice({controlDescriptor(), {}, {}}));
+    coordinator.markStartupComplete();
+
+    const auto result = coordinator.admit(actuateRequest());
+    EXPECT_FALSE(result.decision.allowed);
+    EXPECT_FALSE(result.decision.policy_allowed);
+    EXPECT_TRUE(result.decision.enforced);
+    EXPECT_FALSE(result.permit.has_value());
+}
+
+TEST(SafetyManagerTest, ControlSafetyBitsMustBeExplicitlyFalse)
+{
+    SafetyManagerConfig config;
+    config.enforcement_mode = EnforcementMode::EnforceAll;
+    SafetyManager coordinator(config);
+    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
+    endpoint->protective_stop = TriState::Unknown;
+    ASSERT_TRUE(coordinator.registerDevice(
+        {controlDescriptor(), endpoint, {}}));
+    coordinator.updateDeviceRuntimeState(
+        "arm", device::ManagedDeviceState::Running,
+        {device::DeviceHealthState::Healthy, {}});
+    coordinator.markStartupComplete();
+
+    const auto result = coordinator.admit(actuateRequest());
+    EXPECT_FALSE(result.decision.allowed);
+    EXPECT_EQ(result.decision.reason, SafetyReason::ProtectiveStopActive);
+    ASSERT_EQ(coordinator.snapshot().devices.size(), 1U);
+    EXPECT_EQ(
+        coordinator.snapshot().devices.front().admission_state,
+        DeviceAdmissionState::Blocked);
+}
+
+TEST(SafetyManagerTest, EnforcedDispatchRunsFinalHardwareCheck)
+{
+    SafetyManagerConfig config;
+    config.enforcement_mode = EnforcementMode::EnforceAll;
+    SafetyManager coordinator(config);
+    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
+    ASSERT_TRUE(coordinator.registerDevice(
+        {controlDescriptor(), endpoint, {}}));
+    coordinator.updateDeviceRuntimeState(
+        "arm", device::ManagedDeviceState::Running,
+        {device::DeviceHealthState::Healthy, {}});
+    coordinator.markStartupComplete();
+
+    auto admission = coordinator.admit(actuateRequest());
+    ASSERT_TRUE(admission.decision.allowed) << admission.decision.detail;
+    ASSERT_TRUE(admission.permit.has_value());
+    auto dispatch = coordinator.beginDispatch(*admission.permit);
+    EXPECT_TRUE(dispatch.acquired());
+    EXPECT_EQ(endpoint->hardware_checks.load(), 1);
+}
+
+TEST(SafetyManagerTest,
+     StartActivityMayEnterFromRestrictedButActuationMayNot)
+{
+    SafetyManagerConfig config;
+    config.enforcement_mode = EnforcementMode::EnforceAll;
+    SafetyManager coordinator(config);
+    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
+    endpoint->condition = SafetyCondition::Restricted;
+    endpoint->ready = TriState::False;
+    ASSERT_TRUE(coordinator.registerDevice(
+        {controlDescriptor(), endpoint, {}}));
+    coordinator.updateDeviceRuntimeState(
+        "arm", device::ManagedDeviceState::Running,
+        {device::DeviceHealthState::Healthy, {}});
+    coordinator.markStartupComplete();
+
+    auto start = actuateRequest();
+    start.command.intent = CommandIntent::StartActivity;
+    const auto start_result = coordinator.admit(start);
+    ASSERT_TRUE(start_result.decision.allowed)
+        << start_result.decision.detail;
+    ASSERT_TRUE(start_result.permit.has_value());
+    EXPECT_TRUE(coordinator.beginDispatch(*start_result.permit).acquired());
+
+    const auto actuation = coordinator.admit(actuateRequest());
+    EXPECT_FALSE(actuation.decision.allowed);
+    EXPECT_EQ(actuation.decision.reason, SafetyReason::HardwareUnsafe);
+}
+
+TEST(SafetyManagerTest, SuccessfulStopInvalidatesOldPermitAndReopens)
+{
+    SafetyManagerConfig config;
+    config.enforcement_mode = EnforcementMode::EnforceAll;
+    SafetyManager coordinator(config);
+    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
+    auto participant = std::make_shared<FakeParticipant>();
+    ASSERT_TRUE(coordinator.registerDevice(
+        {controlDescriptor(), endpoint, participant}));
+    coordinator.updateDeviceRuntimeState(
+        "arm", device::ManagedDeviceState::Running,
+        {device::DeviceHealthState::Healthy, {}});
+    coordinator.markStartupComplete();
+
+    auto admission = coordinator.admit(actuateRequest());
+    ASSERT_TRUE(admission.permit.has_value());
+    const auto old_epoch = admission.permit->safety_epoch;
+    const auto stopped = coordinator.stopAll(
+        "stop-1", SafetyClock::now() + std::chrono::seconds(1));
+    ASSERT_TRUE(stopped.success);
+    EXPECT_EQ(stopped.system_state, SystemAdmissionState::Open);
+    EXPECT_GT(stopped.current_safety_epoch, old_epoch);
+    const auto revalidated =
+        coordinator.revalidatePermit(*admission.permit);
+    EXPECT_FALSE(revalidated.safe);
+    EXPECT_EQ(revalidated.reason, SafetyReason::SafetyLatched);
+    EXPECT_FALSE(coordinator.beginDispatch(*admission.permit).acquired());
+    EXPECT_EQ(participant->stop_requests.load(), 1);
+    EXPECT_EQ(participant->verifications.load(), 1);
+    const auto snapshot = coordinator.snapshot();
+    ASSERT_EQ(snapshot.participants.size(), 1U);
+    EXPECT_EQ(snapshot.participants.front().descriptor.participant_id, "arm");
+    EXPECT_FALSE(snapshot.participants.front().barrier_active);
+    EXPECT_FALSE(snapshot.participants.front().barrier_retained);
+    EXPECT_TRUE(snapshot.participants.front().last_request.recorded);
+    EXPECT_TRUE(snapshot.participants.front().last_request.success);
+    EXPECT_TRUE(snapshot.participants.front().last_verify.recorded);
+    EXPECT_TRUE(snapshot.participants.front().last_verify.success);
+    EXPECT_TRUE(snapshot.participants.front().last_release.recorded);
+    EXPECT_TRUE(snapshot.participants.front().last_release.success);
+}
+
+TEST(SafetyManagerTest, FailedStopRequiresVerifiedRecovery)
+{
+    SafetyManagerConfig config;
+    config.enforcement_mode = EnforcementMode::EnforceAll;
+    SafetyManager coordinator(config);
+    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
+    auto participant = std::make_shared<FakeParticipant>();
+    participant->verify_result = {
+        false, SafetyReason::StopUnconfirmed, "motion not confirmed"};
+    ASSERT_TRUE(coordinator.registerDevice(
+        {controlDescriptor(), endpoint, participant}));
+    coordinator.updateDeviceRuntimeState(
+        "arm", device::ManagedDeviceState::Running,
+        {device::DeviceHealthState::Healthy, {}});
+    coordinator.markStartupComplete();
+
+    const auto stopped = coordinator.stopAll(
+        "stop-failed", SafetyClock::now() + std::chrono::seconds(1));
+    ASSERT_FALSE(stopped.success);
+    ASSERT_EQ(stopped.system_state, SystemAdmissionState::Latched);
+    const auto latched = coordinator.snapshot();
+    ASSERT_EQ(latched.participants.size(), 1U);
+    EXPECT_TRUE(latched.participants.front().barrier_active);
+    EXPECT_TRUE(latched.participants.front().barrier_retained);
+    EXPECT_TRUE(latched.participants.front().last_verify.recorded);
+    EXPECT_FALSE(latched.participants.front().last_verify.success);
+    EXPECT_FALSE(latched.participants.front().last_release.recorded);
+
+    participant->verify_result = {true, SafetyReason::None, {}};
+    RecoveryRequest request;
+    request.recovery_id = "recovery-1";
+    request.all_devices = true;
+    request.expected_safety_epoch = stopped.current_safety_epoch;
+    request.verify_only = false;
+    request.reason = "operator confirmed work cell is clear";
+    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
+    const auto recovered = coordinator.recover(request);
+    EXPECT_EQ(recovered.result, RecoveryResultCode::Recovered);
+    EXPECT_EQ(recovered.system_state, SystemAdmissionState::Open);
+    EXPECT_EQ(endpoint->recoveries.load(), 1);
+    EXPECT_EQ(participant->recoveries.load(), 1);
+    const auto reopened = coordinator.snapshot();
+    ASSERT_EQ(reopened.participants.size(), 1U);
+    EXPECT_FALSE(reopened.participants.front().barrier_active);
+    EXPECT_FALSE(reopened.participants.front().barrier_retained);
+    EXPECT_TRUE(reopened.participants.front().last_release.recorded);
+    EXPECT_TRUE(reopened.participants.front().last_release.success);
+}
+
+TEST(SafetyManagerTest, RecoveryCannotIgnoreEmergencyStop)
+{
+    SafetyManager coordinator;
+    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
+    auto participant = std::make_shared<FakeParticipant>();
+    ASSERT_TRUE(coordinator.registerDevice(
+        {controlDescriptor(), endpoint, participant}));
+    coordinator.markStartupComplete();
+    coordinator.quarantineDevice("arm", SafetyReason::OutcomeUnknown);
+    const auto before = coordinator.snapshot();
+
+    endpoint->emergency_stop = TriState::True;
+    RecoveryRequest request;
+    request.recovery_id = "recovery-estop";
+    request.all_devices = true;
+    request.expected_safety_epoch = before.safety_epoch;
+    request.verify_only = false;
+    request.reason = "test";
+    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
+    const auto recovered = coordinator.recover(request);
+    EXPECT_EQ(recovered.result, RecoveryResultCode::BlockerRemains);
+    EXPECT_EQ(recovered.system_state, SystemAdmissionState::Latched);
+    ASSERT_FALSE(recovered.targets.empty());
+    EXPECT_EQ(
+        recovered.targets.front().reason,
+        SafetyReason::EmergencyStopActive);
+    EXPECT_EQ(endpoint->recoveries.load(), 0);
+}
+
+TEST(SafetyManagerTest, RecoveryAuditFailureCannotReleaseLatch)
+{
+    SafetyManager coordinator;
+    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
+    auto participant = std::make_shared<FakeParticipant>();
+    participant->verify_result = {
+        false, SafetyReason::StopUnconfirmed, "motion not confirmed"};
+    ASSERT_TRUE(coordinator.registerDevice(
+        {controlDescriptor(), endpoint, participant}));
+    coordinator.markStartupComplete();
+
+    const auto stopped = coordinator.stopAll(
+        "stop-audit", SafetyClock::now() + std::chrono::seconds(1));
+    ASSERT_FALSE(stopped.success);
+    participant->verify_result = {true, SafetyReason::None, {}};
+
+    RecoveryRequest request;
+    request.recovery_id = "recovery-audit";
+    request.all_devices = true;
+    request.expected_safety_epoch = stopped.current_safety_epoch;
+    request.verify_only = false;
+    request.reason = "operator inspected the work cell";
+    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
+    request.authorize_clear = [] { return false; };
+    const auto recovered = coordinator.recover(request);
+
+    EXPECT_EQ(recovered.result, RecoveryResultCode::BlockerRemains);
+    EXPECT_EQ(recovered.system_state, SystemAdmissionState::Latched);
+    ASSERT_FALSE(recovered.targets.empty());
+    EXPECT_EQ(
+        recovered.targets.front().reason,
+        SafetyReason::RecoveryAuditFailed);
+    EXPECT_EQ(endpoint->recoveries.load(), 0);
+    EXPECT_EQ(participant->recoveries.load(), 0);
+    EXPECT_EQ(participant->releases.load(), 0);
+}
+
+} // namespace
+} // namespace cmvr::safety

cmvr-es/manager/safety_manager/tests/safety_snapshot_store_test.cpp

@@ -0,0 +1,111 @@
+#include "manager/safety_manager/include/safety_snapshot_store.h"
+
+#include <atomic>
+#include <chrono>
+#include <thread>
+
+#include <gtest/gtest.h>
+
+namespace cmvr::safety {
+namespace {
+
+DeviceSafetyDescriptor descriptor()
+{
+    DeviceSafetyDescriptor result;
+    result.device_id = "arm";
+    result.kind = device::DeviceKind::Arm;
+    result.default_policy = SafetyPolicyFamily::Control;
+    result.maximum_snapshot_age = std::chrono::milliseconds(50);
+    result.requires_safe_stop = true;
+    return result;
+}
+
+DeviceSafetySnapshot sample(
+    const std::uint64_t generation,
+    const std::uint64_t sequence,
+    const SafetyClock::time_point observed_at = SafetyClock::now())
+{
+    DeviceSafetySnapshot result;
+    result.device_id = "arm";
+    result.condition = SafetyCondition::Nominal;
+    result.device_generation = generation;
+    result.sample_sequence = sequence;
+    result.observed_at = observed_at;
+    result.connected = TriState::True;
+    result.operational_ready = TriState::True;
+    result.quiescent = TriState::True;
+    result.motion_active = TriState::False;
+    result.emergency_stop_active = TriState::False;
+    result.protective_stop_active = TriState::False;
+    result.fault_active = TriState::False;
+    return result;
+}
+
+TEST(SafetySnapshotStoreTest, RejectsOldGenerationAndSequence)
+{
+    SafetySnapshotStore store;
+    ASSERT_TRUE(store.registerDevice(descriptor(), 3));
+    EXPECT_FALSE(store.publish(sample(2, 1)));
+    ASSERT_TRUE(store.publish(sample(3, 2)));
+    EXPECT_FALSE(store.publish(sample(3, 2)));
+    EXPECT_FALSE(store.publish(sample(3, 1)));
+    EXPECT_TRUE(store.publish(sample(4, 1)));
+
+    const auto view = store.get("arm");
+    ASSERT_TRUE(view.registered);
+    EXPECT_EQ(view.snapshot.device_generation, 4U);
+    EXPECT_EQ(view.snapshot.sample_sequence, 1U);
+}
+
+TEST(SafetySnapshotStoreTest, FreshnessUsesMonotonicObservationTime)
+{
+    SafetySnapshotStore store;
+    ASSERT_TRUE(store.registerDevice(descriptor()));
+    const auto observed = SafetyClock::now();
+    ASSERT_TRUE(store.publish(sample(1, 1, observed)));
+
+    EXPECT_TRUE(store.get(
+        "arm", observed + std::chrono::milliseconds(49)).fresh);
+    const auto stale = store.get(
+        "arm", observed + std::chrono::milliseconds(51));
+    EXPECT_FALSE(stale.fresh);
+    EXPECT_EQ(stale.sample_age, std::chrono::milliseconds(51));
+}
+
+TEST(SafetySnapshotStoreTest, BumpGenerationRequiresANewSample)
+{
+    SafetySnapshotStore store;
+    ASSERT_TRUE(store.registerDevice(descriptor()));
+    ASSERT_TRUE(store.publish(sample(1, 8)));
+    ASSERT_EQ(store.bumpGeneration("arm"), 2U);
+
+    const auto view = store.get("arm");
+    EXPECT_FALSE(view.has_sample);
+    EXPECT_FALSE(view.fresh);
+    EXPECT_EQ(view.snapshot.device_generation, 2U);
+    EXPECT_FALSE(store.publish(sample(1, 9)));
+    EXPECT_TRUE(store.publish(sample(2, 1)));
+}
+
+TEST(SafetySnapshotStoreTest, WaiterObservesOnlyANewerSample)
+{
+    SafetySnapshotStore store;
+    ASSERT_TRUE(store.registerDevice(descriptor()));
+    ASSERT_TRUE(store.publish(sample(1, 1)));
+
+    std::atomic<bool> published{false};
+    std::thread writer([&] {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        published.store(store.publish(sample(1, 2)));
+    });
+
+    SafetySnapshotView view;
+    EXPECT_TRUE(store.waitForNewerSample(
+        "arm", 1, SafetyClock::now() + std::chrono::seconds(1), view));
+    writer.join();
+    EXPECT_TRUE(published.load());
+    EXPECT_EQ(view.snapshot.sample_sequence, 2U);
+}
+
+} // namespace
+} // namespace cmvr::safety

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)
@@ -22,6 +23,7 @@ if(BUILD_TESTING)
     )
     target_link_libraries(task_manager_lifecycle_test PRIVATE
             cmvr_es::task_manager
+            cmvr_es::stop_all_admission_gate
             gtest
             gtest_main
             pthread

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,6 +24,10 @@ 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();
 
@@ -34,6 +39,11 @@ namespace cmvr::task {
         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);