feat(grpc): persist RPC failures in edge logs

cmvr-es/CMakeLists.txt

@@ -8,6 +8,7 @@ add_subdirectory(simulate)
 add_subdirectory(devices)
 add_subdirectory(manager/control_authority)
 add_subdirectory(manager/device_manager)
+add_subdirectory(service/stop_all)
 add_subdirectory(manager/media_source_hub)
 add_subdirectory(service/quic_edge)
 add_subdirectory(service/arm_teleop_client)

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/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/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/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

@@ -100,6 +100,12 @@ public:
     bool busy() const override;
 
 private:
+    enum class TrajectoryExecutionResult {
+        Completed,
+        Canceled,
+        Failed,
+    };
+
     bool containsJoint_(const std::string& joint_name) const;
     bool validatePositionCommand_(const JointPositionCommand& cmd, std::string& error) const;
     bool validateVelocityCommand_(const JointVelocityCommand& cmd, std::string& error) const;
@@ -108,7 +114,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);
@@ -124,6 +133,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};
@@ -131,6 +141,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

@@ -29,6 +29,19 @@ struct BusyGuard {
     ~BusyGuard() { busy.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)
 {
@@ -124,6 +137,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()
@@ -172,6 +188,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;
 }
@@ -327,6 +353,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();
     }
@@ -355,6 +382,8 @@ Result MotorRobotArm::setSpeedScaling(const double scaling)
 
 Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOptions& options)
 {
+    const auto motion_generation =
+        motion_generation_.load(std::memory_order_acquire);
     std::string error;
     if (!validatePositionCommand_(target, error)) {
         return Result::failure(ArmErrorCode::InvalidArgument, error);
@@ -366,7 +395,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_);
+    }
 
     std::vector<JointTrajectorySample> samples;
     if (!joint_planner_->planMoveJ(readJointPosition_(), target, options, speed_scaling_, samples)) {
@@ -378,15 +412,31 @@ 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) {
-            motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+        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) {
+                motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+            }
+            motors.push_back(std::move(motor));
         }
-        motors.push_back(std::move(motor));
     }
 
     const auto t0 = std::chrono::steady_clock::now();
@@ -396,12 +446,32 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
         if (sample.position.size() != motors.size()) {
             return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch");
         }
-        for (std::size_t i = 0; i < motors.size(); ++i) {
-            const double qd = i < sample.velocity.size() ? sample.velocity[i] : 0.0;
-            if (!motors[i]->commandCyclicPosition(sample.position[i], qd)) {
-                return Result::failure(ArmErrorCode::CommandFailed,
-                                       "failed to command cyclic position for joint: " +
-                                           motors[i]->jointName());
+        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_);
+            }
+            for (std::size_t i = 0; i < motors.size(); ++i) {
+                const double qd =
+                    i < sample.velocity.size() ? sample.velocity[i] : 0.0;
+                if (!motors[i]->commandCyclicPosition(
+                        sample.position[i], qd)) {
+                    return Result::failure(
+                        ArmErrorCode::CommandFailed,
+                        "failed to command cyclic position for joint: " +
+                            motors[i]->jointName());
+                }
             }
         }
         if (k + 1 < samples.size()) {
@@ -452,6 +522,7 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity,
 
 Result MotorRobotArm::stopJ(const double acceleration)
 {
+    motion_generation_.fetch_add(1, std::memory_order_acq_rel);
     JointVelocityCommand zero;
     zero.velocity.assign(joint_names_.size(), 0.0);
     return speedJ(zero, acceleration, 0.0);
@@ -461,6 +532,8 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
                             const MotionOptions& options,
                             const FrameType frame)
 {
+    const auto motion_generation =
+        motion_generation_.load(std::memory_order_acquire);
     if (cartesian_velocity_controller_) {
         cartesian_velocity_controller_->shutdown();
     }
@@ -478,6 +551,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)) {
@@ -502,8 +581,22 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
                           << ", executable_path_m=" << trajectory.executable_path_length;
     }
 
-    return executeMoveLTrajectory_(trajectory) ? Result::success()
-                                               : Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed");
+    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:
+            return Result::failure(
+                ArmErrorCode::CommandFailed, "moveL execution failed");
+    }
+    return Result::failure(
+        ArmErrorCode::CommandFailed, "moveL execution failed");
 }
 
 Result MotorRobotArm::speedL(const CartesianVelocity& velocity,
@@ -530,16 +623,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)
@@ -816,25 +919,40 @@ 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) {
                 motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
@@ -849,11 +967,24 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj
         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;
         }
-        for (std::size_t j = 0; j < motors.size(); ++j) {
-            if (!motors[j]->commandCyclicPosition(position[j], velocity[j])) {
-                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;
+            }
+            for (std::size_t j = 0; j < motors.size(); ++j) {
+                if (!motors[j]->commandCyclicPosition(
+                        position[j], velocity[j])) {
+                    return TrajectoryExecutionResult::Failed;
+                }
             }
         }
         next_deadline += std::chrono::duration_cast<std::chrono::steady_clock::duration>(
@@ -861,7 +992,11 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj
         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/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,19 +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::lock_guard lock(ctrl_mtx_);
-    clear_error_();
-    if (!state_.is_opened || !state_.is_initialized) {
-        state_.is_opened = false;
-        return true;
+    std::lock_guard lifecycle_lock(stream_lifecycle_mtx_);
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        clear_error_();
+        if (!state_.is_opened || !state_.is_initialized) {
+            state_.is_opened = false;
+            return true;
+        }
+        state_.is_streaming = false;
+        stream_count_ = 0;
+        stream_requested_.store(false, std::memory_order_release);
     }
+
+    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_);
     try {
         pipe_.stop();
     } catch (const std::exception& e) {
@@ -387,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;
@@ -433,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;
@@ -485,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;
     }
 
@@ -516,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"; // 临时文件
@@ -604,63 +660,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_) {
@@ -682,14 +742,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() {
@@ -718,10 +790,11 @@ 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);
 
         // 处于流传输或者录像状态时就不退出线程
-        while (state_.is_streaming || state_.is_recording) {
+        while (stream_requested_.load(std::memory_order_acquire) ||
+               recording_requested_.load(std::memory_order_acquire)) {
             // 记录当前帧处理开始时间
             auto frame_start_time = std::chrono::high_resolution_clock::now();
 
@@ -730,15 +803,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;
             }
 
@@ -809,7 +880,7 @@ void RealsenseCamera::streaming_worker_() {
             }
         }
 
-        is_streaming_running = false;
+        markStreamingWorkerStopped_();
         // 线程结束时清空队列
         stream_frame_buffer_->clear();
         recordingIndex_ = 0;
@@ -820,20 +891,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));
@@ -898,11 +970,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) {
@@ -936,37 +1009,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::lock_guard lock(ctrl_mtx_);
-    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 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

@@ -5,6 +5,10 @@
 #ifndef CMVR_ES_UVC_CAMERA_H
 #define CMVR_ES_UVC_CAMERA_H
 
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+
 #include "common/base/ring_buffer.h"
 #include "camera/abstract_camera.h"
 #include "devices/camera/common/include/camera_stream_encoder.h"
@@ -42,9 +46,15 @@ namespace cmvr::device {
 
         bool startStreaming() override;
         void stopStreaming() override;
+        bool stopOperationalActivity() override;
     private:
         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;
 
         int fps_;
         int width_;
@@ -63,6 +73,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> recording_thread_;
@@ -88,8 +102,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

@@ -10,6 +10,10 @@ using namespace cmvr::device;
 
 #define USE_LIST_IMAGE 1
 
+namespace {
+constexpr auto kStreamStopTimeout = std::chrono::seconds(2);
+}
+
 UVCCamera::UVCCamera(const config::UVCCameraConfig& camera):camera_(camera)
 {
     id_ = camera_.id();
@@ -174,9 +178,17 @@ bool UVCCamera::start() {
 
 bool UVCCamera::stop() {
     //先停止录制再关闭摄像头
-    if (state_.is_recording) {
+    bool is_recording = false;
+    {
+        std::lock_guard lock(ctrl_mtx_);
+        is_recording = state_.is_recording;
+    }
+    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 {
@@ -186,18 +198,10 @@ bool UVCCamera::stop() {
         }
         if (mode_ == VIDEO_MODE){
             state_.is_streaming = false;
-            if (stream_thread_->joinable()) {
-                stream_thread_->join();
-                stream_thread_.reset();
-                stream_thread_ = nullptr;
-            }
+            stream_count_ = 0;
+            stream_requested_.store(false, std::memory_order_release);
+            collect_stream = static_cast<bool>(stream_thread_);
         }
-
-        if (cap_.isOpened()) {
-            cap_.release();
-        }
-        state_.is_opened = false;
-        return true;
     }
     catch (exception &e) {
         CMVR_LOG(ERROR) << "[UVCCamera] (stop): " << e.what();
@@ -205,6 +209,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;
+    }
+    std::lock_guard lock(ctrl_mtx_);
+    if (cap_.isOpened()) {
+        cap_.release();
+    }
+    state_.is_opened = false;
+    return true;
 }
 
 void UVCCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics)
@@ -240,6 +258,7 @@ void UVCCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics)
 }
 
 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;
@@ -247,6 +266,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;
@@ -255,24 +275,52 @@ void UVCCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& int
 }
 
 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;
+    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) << "[UVCCamera] (startRecording): " << state_.error_message;
+        state_.error_message = state_.is_recording
+            ? "already recording" : "camera not opened";
         return;
     }
 
@@ -286,8 +334,11 @@ void UVCCamera::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"; // 临时文件
@@ -374,64 +425,73 @@ 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_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;
-    }
-    if (!state_.is_recording) {
-        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_);
+        if (!state_.is_recording && !has_recording_worker) {
+            CMVR_LOG(WARNING) << "[UVCCamera] (stopRecording): not recording";
+            return;
+        }
+        recording_requested_.store(false, std::memory_order_release);
     }
 
-    // 1. 停止录像线程
-    state_.is_recording = false;
-    if (recording_thread_ && 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资源
     if (packet_) {
         av_packet_free(&packet_);
@@ -451,15 +511,29 @@ void UVCCamera::stopRecording() {
     }
     stream_ = nullptr;
 
-    // 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() {
@@ -477,17 +551,19 @@ 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);
         cv::Mat frame;
         int64_t frame_count = 0;
         // 处于流传输或者录像状态时就不退出线程
-        while (state_.is_streaming || state_.is_recording) {
+        while (stream_requested_.load(std::memory_order_acquire) ||
+               recording_requested_.load(std::memory_order_acquire)) {
             // 记录当前帧处理开始时间
             auto frame_start_time = std::chrono::high_resolution_clock::now();
             if (!cap_.read(frame) || frame.empty()) {
-                state_.is_error = true;
-                state_.error_message = "failed to read frame";
-                CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_: " << state_.error_message;
+                setWorkerError_("failed to read frame");
+                recording_requested_.store(false, std::memory_order_release);
+                CMVR_LOG(ERROR) <<
+                    "[UVCCamera]streaming_worker_: failed to read frame";
                 break;
             }
 
@@ -540,7 +616,7 @@ void UVCCamera::streaming_worker_() {
             }
         }
 
-        is_streaming_running = false;
+        markStreamingWorkerStopped_();
         // 线程结束时清空队列
         stream_frame_buffer_->clear();
         recordingIndex_ = 0;
@@ -551,20 +627,20 @@ 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();
     }
 }
 
 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;
     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));
@@ -629,11 +705,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) {
@@ -667,36 +744,215 @@ 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;
+    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;
+        }
+
+        // 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_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>(&UVCCamera::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>(&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_);
-    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;
+    }
+
+    std::lock_guard lock(ctrl_mtx_);
+    if (cap_.isOpened()) {
+        cap_.release();
+    }
+    state_.is_opened = false;
+    return !cap_.isOpened() && !state_.is_streaming &&
+        !state_.is_recording;
+}
+
+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

@@ -46,6 +46,15 @@ public:
     std::shared_ptr<AbstractMotor> getMotor(const std::string& joint_name) const;
     const std::unordered_map<std::string, std::shared_ptr<AbstractMotor>>& motorsMap() 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,
@@ -85,6 +94,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

@@ -239,6 +239,120 @@ const std::unordered_map<std::string, std::shared_ptr<AbstractMotor>>& MotorMana
     return motors_by_joint_;
 }
 
+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

@@ -30,7 +30,8 @@
 
 - DeviceManager 构造不会自动调用全部设备的 `start()`；
 - 当前主退出路径没有调用 `DeviceManager::stop()`；
-- `SystemService/StopAll` 会调用 DeviceManager stop；
+- `SystemService/StopAll` 只停止当前运动、控制和媒体活动，不调用
+  `DeviceManager::stop()`，成功返回后可继续接受新命令；
 - `DeviceManager::destroyInstance()` 不调用设备 stop，销毁前必须先显式停止；
 - `TaskManager::destroyInstance()` 会调用 `stopRunTask()`，但 manager 未处于 running 状态时该调用会直接返回；
 - DeviceManager 和 TaskManager 都是首次配置生效的单例，不支持热加载。

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

@@ -2,7 +2,9 @@
 #define CMVR_ES_CONTROL_AUTHORITY_MANAGER_H
 
 #include <chrono>
+#include <condition_variable>
 #include <cstdint>
+#include <memory>
 #include <mutex>
 #include <string>
 #include <unordered_map>
@@ -28,6 +30,8 @@ struct ControlAcquireResult {
     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.
@@ -58,13 +62,40 @@ public:
         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. Quarantine does not allocate and can only be removed by
-    // an explicit revoke/clear.
+    // 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
@@ -78,23 +109,62 @@ public:
     void clear() noexcept;
 
 private:
-    struct Entry {
+    friend class ControlDispatchGuard;
+
+    struct SafetyHolder {
         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;
+        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::unordered_map<std::string, Entry> entries_;
+    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/src/control_authority_manager.cpp

@@ -1,10 +1,65 @@
 #include "manager/control_authority/include/control_authority_manager.h"
 
-#include <type_traits>
 #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;
@@ -24,12 +79,26 @@ ControlAcquireResult ControlAuthorityManager::tryAcquire(
     std::lock_guard lock(mutex_);
     const auto existing = entries_.find(resource_id);
     if (existing != entries_.end()) {
-        if (!expired_(existing->second)) {
+        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 " +
-                    existing->second.owner_id};
+                    entry.owner_id};
+        }
+        if (entry.in_flight_dispatches != 0U) {
+            invalidateToDispatchFence_(entry);
+            return {
+                false,
+                {},
+                "control resource still has an in-flight dispatch"};
         }
         entries_.erase(existing);
     }
@@ -38,15 +107,13 @@ ControlAcquireResult ControlAuthorityManager::tryAcquire(
     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,
-            {}});
+
+    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), {}};
 }
 
@@ -62,47 +129,71 @@ ControlAcquireResult ControlAuthorityManager::preemptAcquire(
 
     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), {}};
-        }
+    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 has_existing = existing != entries_.end();
     const bool replacing_normal =
-        has_existing && existing->second.preemptible;
+        existing != entries_.end() && 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);
+        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 {
-            entries_.emplace(resource_id, std::move(replacement));
+            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);
+        if (replacing_normal && existing->second->preemptible) {
+            quarantine_(*existing->second);
         }
         throw;
     }
@@ -121,10 +212,10 @@ ControlAcquireResult ControlAuthorityManager::preemptAcquireIfCurrent(
     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) {
+        expired_(*existing->second) ||
+        !existing->second->preemptible ||
+        existing->second->owner_id != expected_token.owner_id ||
+        existing->second->generation != expected_token.generation) {
         return {
             false,
             {},
@@ -136,27 +227,70 @@ ControlAcquireResult ControlAuthorityManager::preemptAcquireIfCurrent(
         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);
+        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);
+        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
 {
@@ -165,16 +299,76 @@ bool ControlAuthorityManager::quarantineIfCurrent(
     }
     try {
         std::lock_guard lock(mutex_);
-        const auto existing =
-            entries_.find(expected_token.resource_id);
+        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) {
+            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);
+        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;
@@ -190,24 +384,29 @@ bool ControlAuthorityManager::renew(
     }
     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)) {
+    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 == token.owner_id;
+    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) {
+    if (found->second->owner_id != token.owner_id ||
+        found->second->generation != token.generation) {
         return false;
     }
-    found->second.deadline =
-        std::chrono::steady_clock::now() + ttl;
+    found->second->deadline = std::chrono::steady_clock::now() + ttl;
     return true;
 }
 
@@ -222,18 +421,51 @@ bool ControlAuthorityManager::validate(
     if (found == entries_.end()) {
         return false;
     }
-    if (expired_(found->second)) {
-        entries_.erase(found);
+    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 == token.owner_id;
+    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;
+    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(
@@ -248,21 +480,54 @@ void ControlAuthorityManager::release(
         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) {
+        auto& entry = *found->second;
+        if (!entry.preemptible) {
+            if (entry.dispatch_fence_only) {
                 return;
             }
-            found->second.safety_holders.erase(holder);
-            if (found->second.safety_holders.empty() &&
-                !found->second.quarantined) {
+            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);
             }
-        } else if (found->second.owner_id == token.owner_id &&
-                   found->second.generation == token.generation) {
-            entries_.erase(found);
+            release_cv_.notify_all();
         }
     } catch (...) {
     }
@@ -279,7 +544,11 @@ bool ControlAuthorityManager::isLeased(
     if (found == entries_.end()) {
         return false;
     }
-    if (expired_(found->second)) {
+    if (expired_(*found->second)) {
+        if (found->second->in_flight_dispatches != 0U) {
+            invalidateToDispatchFence_(*found->second);
+            return true;
+        }
         entries_.erase(found);
         return false;
     }
@@ -291,7 +560,16 @@ void ControlAuthorityManager::revoke(
 {
     try {
         std::lock_guard lock(mutex_);
-        entries_.erase(resource_id);
+        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 (...) {
     }
 }
@@ -300,24 +578,105 @@ void ControlAuthorityManager::clear() noexcept
 {
     try {
         std::lock_guard lock(mutex_);
-        entries_.clear();
+        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
+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.deadline =
-        std::chrono::steady_clock::time_point::max();
+    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/tests/control_authority_manager_test.cpp

@@ -1,6 +1,7 @@
 #include "manager/control_authority/include/control_authority_manager.h"
 
 #include <chrono>
+#include <future>
 #include <thread>
 
 #include <gtest/gtest.h>
@@ -93,6 +94,536 @@ TEST_F(ControlAuthorityManagerTest,
     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)
 {
@@ -127,6 +658,9 @@ TEST_F(ControlAuthorityManagerTest,
         "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);
@@ -163,6 +697,8 @@ TEST_F(ControlAuthorityManagerTest,
     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)
@@ -226,6 +762,137 @@ TEST_F(ControlAuthorityManagerTest,
     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();

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

@@ -18,6 +18,12 @@
 
 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,6 +42,9 @@ 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;
 
         std::string version() const;

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

@@ -377,6 +377,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_);

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,54 @@ bool testConcurrentSnapshotAndRegistration()
     return true;
 }
 
+bool testInventorySnapshotDoesNotWaitForDeviceHealth()
+{
+    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(blocking_device);
+    manager.registerDevice(other_device);
+
+    auto health_future = std::async(std::launch::async, [&manager] {
+        return manager.snapshot();
+    });
+    if (!blocking_device->waitForHealthCall(std::chrono::seconds(2))) {
+        blocking_device->releaseHealthCall();
+        health_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();
+        health_future.wait();
+        return false;
+    }
+
+    const auto inventory = inventory_future.get();
+    const bool inventory_valid =
+        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();
+    health_future.get();
+    return inventory_valid && blocking_device->health_calls.load() == 1;
+}
+
 } // namespace
 
 int main()
@@ -382,7 +490,8 @@ int main()
     const bool success =
         testCategoryHealthAdapters() &&
         testConfiguredAndDynamicSnapshots() &&
-        testConcurrentSnapshotAndRegistration();
+        testConcurrentSnapshotAndRegistration() &&
+        testInventorySnapshotDoesNotWaitForDeviceHealth();
     DeviceManager::destroyInstance();
     return success ? 0 : 1;
 }

cmvr-es/manager/media_source_hub/CMakeLists.txt

@@ -2,6 +2,10 @@ if(CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR)
     cmake_minimum_required(VERSION 3.22)
     project(cmvr_media_source_hub LANGUAGES CXX)
     enable_testing()
+    add_subdirectory(
+            ${CMAKE_CURRENT_SOURCE_DIR}/../../service/stop_all
+            ${CMAKE_CURRENT_BINARY_DIR}/stop_all
+    )
 endif()
 
 add_library(media_source_hub STATIC
@@ -13,6 +17,10 @@ target_include_directories(media_source_hub
         PUBLIC
         ${CMAKE_CURRENT_SOURCE_DIR}/../..
 )
+target_link_libraries(media_source_hub
+        PUBLIC
+        cmvr_es::stop_all_admission_gate
+)
 
 add_library(cmvr_es::media_source_hub ALIAS media_source_hub)
 

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

@@ -15,6 +15,10 @@
 #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
@@ -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;
     };
 
@@ -84,7 +92,10 @@ public:
         bool active_{false};
     };
 
-    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 MediaSourceHub(
+        service::StopAllAdmissionGate* admission_gate = nullptr);
     ~MediaSourceHub();
 
     MediaSourceHub(const MediaSourceHub&) = delete;
@@ -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,14 +127,32 @@ 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_;
 };

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

@@ -1,5 +1,7 @@
 #include "manager/media_source_hub/include/device_media_source_adapter.h"
 
+#include "service/stop_all/include/stop_all_admission_gate.h"
+
 #include <algorithm>
 #include <atomic>
 #include <chrono>
@@ -123,7 +125,7 @@ struct PumpState : public std::enable_shared_from_this<PumpState<DeviceT>> {
         : device(std::move(device_ptr)) {}
 
     virtual ~PumpState() {
-        stop();
+        (void)stop();
     }
 
     bool begin(
@@ -152,7 +154,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();
         }
 
@@ -213,7 +217,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;
         {
@@ -224,24 +228,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();
@@ -253,22 +261,25 @@ 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;
@@ -282,7 +293,7 @@ struct PumpState : public std::enable_shared_from_this<PumpState<DeviceT>> {
 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;
@@ -459,7 +470,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;
@@ -607,7 +618,7 @@ TrackDescriptorPtr initialTrack(
 } // namespace
 
 MediaSourceHub& globalMediaSourceHub() {
-    static MediaSourceHub hub;
+    static MediaSourceHub hub(&service::globalStopAllAdmissionGate());
     return hub;
 }
 
@@ -638,7 +649,7 @@ bool ensureCameraMediaSource(
                           const MediaSourceHub::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),
@@ -670,7 +681,7 @@ bool ensureMicrophoneMediaSource(
                           const MediaSourceHub::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_hub/src/media_source_hub.cpp

@@ -6,8 +6,11 @@
 #include <mutex>
 #include <thread>
 #include <unordered_map>
+#include <unordered_set>
 #include <utility>
 
+#include "service/stop_all/include/stop_all_admission_gate.h"
+
 namespace cmvr::media {
 
 struct MediaSourceHub::SourceState final : public std::enable_shared_from_this<SourceState> {
@@ -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,12 +402,22 @@ 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 {
+    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(
@@ -425,8 +493,9 @@ void MediaSourceHub::Subscription::reset() {
     source_.reset();
 }
 
-MediaSourceHub::MediaSourceHub()
-    : impl_(std::make_shared<Impl>()) {}
+MediaSourceHub::MediaSourceHub(
+    service::StopAllAdmissionGate* admission_gate)
+    : impl_(std::make_shared<Impl>(admission_gate)) {}
 
 MediaSourceHub::~MediaSourceHub() {
     shutdown();
@@ -444,13 +513,47 @@ 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) {
@@ -475,7 +578,13 @@ 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;
@@ -516,6 +625,25 @@ std::vector<TrackDescriptorPtr> MediaSourceHub::listTracks() const {
     return descriptors;
 }
 
+std::vector<std::string> MediaSourceHub::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 MediaSourceHub::subscriberCount(const std::string& track_id) const {
     if (!impl_) {
         return 0;
@@ -573,24 +701,103 @@ MediaSourceHub::Subscription MediaSourceHub::subscribe(
     return Subscription(std::move(source), std::move(cursor));
 }
 
-void MediaSourceHub::shutdown() {
+bool MediaSourceHub::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 MediaSourceHub::stopAllSources(std::vector<std::string>* failures) {
+    return stopSources(std::nullopt, failures);
+}
+
+bool MediaSourceHub::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 MediaSourceHub::shutdown() {
+    (void)stopAllSources();
 }
 
 } // namespace cmvr::media

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

@@ -1,4 +1,5 @@
 #include "manager/media_source_hub/include/media_source_hub.h"
+#include "service/stop_all/include/stop_all_admission_gate.h"
 
 #include <atomic>
 #include <chrono>
@@ -43,10 +44,12 @@ int failures = 0;
 TrackDescriptorPtr makeVideoDescriptor(
     const Codec codec,
     const uint64_t generation,
-    std::vector<uint8_t> codec_config = {}) {
+    std::vector<uint8_t> codec_config = {},
+    std::string track_id = "camera.front.video",
+    std::string source_id = "camera.front") {
     TrackDescriptor::Config config;
-    config.id = "camera.front.video";
-    config.source_id = "camera.front";
+    config.id = std::move(track_id);
+    config.source_id = std::move(source_id);
     config.kind = MediaKind::VIDEO;
     config.codec = codec;
     config.payload_format = codec == Codec::UNKNOWN ? PayloadFormat::UNKNOWN : PayloadFormat::ANNEX_B;
@@ -504,6 +507,648 @@ void testHubFailedStartAndShutdown() {
     CHECK_TRUE(!live.waitRead(50ms).has_value());
 }
 
+void testHubStopAllSourcesAllowsReregistration() {
+    MediaSourceHub hub;
+    const auto first_descriptor = makeVideoDescriptor(Codec::H264, 1);
+    std::atomic<int> first_stop_count{0};
+
+    MediaSourceHub::SourceCallbacks first_callbacks;
+    first_callbacks.start = [](const MediaSourceHub::FrameSink&,
+                               const MediaSourceHub::CancelPredicate&) {
+        return true;
+    };
+    first_callbacks.stop = [&] { ++first_stop_count; };
+    CHECK_TRUE(hub.registerSource(first_descriptor, std::move(first_callbacks), 2));
+    auto old_subscription = hub.subscribe(first_descriptor->id);
+    CHECK_TRUE(old_subscription.valid());
+
+    auto blocked_read = std::async(std::launch::async, [&] {
+        return old_subscription.waitRead(2s);
+    });
+    CHECK_TRUE(hub.stopAllSources());
+
+    CHECK_TRUE(first_stop_count.load(std::memory_order_acquire) == 1);
+    CHECK_TRUE(!hub.hasSource(first_descriptor->id));
+    CHECK_TRUE(!old_subscription.valid());
+    CHECK_TRUE(blocked_read.wait_for(500ms) == std::future_status::ready);
+    if (blocked_read.wait_for(0ms) == std::future_status::ready) {
+        CHECK_TRUE(!blocked_read.get().has_value());
+    }
+
+    const auto second_descriptor = makeVideoDescriptor(Codec::H264, 2);
+    std::atomic<int> second_start_count{0};
+    std::atomic<int> second_stop_count{0};
+    MediaSourceHub::SourceCallbacks second_callbacks;
+    second_callbacks.start = [&](const MediaSourceHub::FrameSink&,
+                                 const MediaSourceHub::CancelPredicate&) {
+        ++second_start_count;
+        return true;
+    };
+    second_callbacks.stop = [&] { ++second_stop_count; };
+
+    CHECK_TRUE(hub.registerSource(second_descriptor, std::move(second_callbacks), 2));
+    auto new_subscription = hub.subscribe(second_descriptor->id);
+    CHECK_TRUE(new_subscription.valid());
+    CHECK_TRUE(new_subscription.descriptor()->generation == 2);
+    CHECK_TRUE(second_start_count.load(std::memory_order_acquire) == 1);
+
+    old_subscription.reset();
+    CHECK_TRUE(second_stop_count.load(std::memory_order_acquire) == 0);
+    new_subscription.reset();
+    CHECK_TRUE(second_stop_count.load(std::memory_order_acquire) == 1);
+}
+
+void testStopAllSourcesReportsAndRetriesUnconfirmedStop() {
+    MediaSourceHub hub;
+    const auto descriptor = makeVideoDescriptor(Codec::H264, 1);
+    std::atomic<int> stop_attempts{0};
+
+    MediaSourceHub::SourceCallbacks callbacks;
+    callbacks.start = [](const MediaSourceHub::FrameSink&,
+                         const MediaSourceHub::CancelPredicate&) {
+        return true;
+    };
+    callbacks.stop_confirmed = [&] {
+        return ++stop_attempts >= 2;
+    };
+    CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 2));
+    auto subscription = hub.subscribe(descriptor->id);
+    CHECK_TRUE(subscription.valid());
+
+    std::vector<std::string> stop_failures;
+    CHECK_TRUE(!hub.stopAllSources(&stop_failures));
+    CHECK_TRUE(stop_failures.size() == 1);
+    CHECK_TRUE(stop_failures.front() == descriptor->id);
+    CHECK_TRUE(hub.hasSource(descriptor->id));
+    CHECK_TRUE(!subscription.valid());
+
+    stop_failures.clear();
+    CHECK_TRUE(hub.stopAllSources(&stop_failures));
+    CHECK_TRUE(stop_failures.empty());
+    CHECK_TRUE(!hub.hasSource(descriptor->id));
+    CHECK_TRUE(stop_attempts.load(std::memory_order_acquire) == 2);
+}
+
+void testStopSourcesForDeviceIsSelectiveAndRetriesFailures() {
+    MediaSourceHub hub;
+    const auto front_video = makeVideoDescriptor(
+        Codec::H264, 1, {}, "front.video", "camera.front");
+    const auto front_depth = makeVideoDescriptor(
+        Codec::H264, 1, {}, "front.depth", "camera.front");
+    const auto rear_video = makeVideoDescriptor(
+        Codec::H264, 1, {}, "rear.video", "camera.rear");
+
+    std::atomic<int> front_video_stops{0};
+    std::atomic<int> front_depth_stops{0};
+    std::atomic<int> rear_stops{0};
+    auto register_source = [&](
+        const TrackDescriptorPtr& descriptor,
+        std::function<bool()> stop_confirmed) {
+        MediaSourceHub::SourceCallbacks callbacks;
+        callbacks.start = [](
+            const MediaSourceHub::FrameSink&,
+            const MediaSourceHub::CancelPredicate&) { return true; };
+        callbacks.stop_confirmed = std::move(stop_confirmed);
+        return hub.registerSource(descriptor, std::move(callbacks), 2);
+    };
+
+    CHECK_TRUE(register_source(front_video, [&] {
+        return ++front_video_stops >= 2;
+    }));
+    CHECK_TRUE(register_source(front_depth, [&] {
+        ++front_depth_stops;
+        return true;
+    }));
+    CHECK_TRUE(register_source(rear_video, [&] {
+        ++rear_stops;
+        return true;
+    }));
+    CHECK_TRUE(
+        hub.trackedSourceIds() ==
+        (std::vector<std::string>{"camera.front", "camera.rear"}));
+
+    auto front_video_subscription = hub.subscribe(front_video->id);
+    auto front_depth_subscription = hub.subscribe(front_depth->id);
+    auto rear_subscription = hub.subscribe(rear_video->id);
+    CHECK_TRUE(front_video_subscription.valid());
+    CHECK_TRUE(front_depth_subscription.valid());
+    CHECK_TRUE(rear_subscription.valid());
+
+    std::vector<std::string> stop_failures;
+    CHECK_TRUE(!hub.stopSourcesForDevice("camera.front", &stop_failures));
+    CHECK_TRUE(stop_failures.size() == 1U);
+    CHECK_TRUE(stop_failures.front() == front_video->id);
+    CHECK_TRUE(hub.hasSource(front_video->id));
+    CHECK_TRUE(!hub.hasSource(front_depth->id));
+    CHECK_TRUE(hub.hasSource(rear_video->id));
+    CHECK_TRUE(
+        hub.trackedSourceIds() ==
+        (std::vector<std::string>{"camera.front", "camera.rear"}));
+    CHECK_TRUE(!front_video_subscription.valid());
+    CHECK_TRUE(!front_depth_subscription.valid());
+    CHECK_TRUE(rear_subscription.valid());
+    CHECK_TRUE(rear_stops.load(std::memory_order_acquire) == 0);
+
+    CHECK_TRUE(hub.stopSourcesForDevice("camera.front", &stop_failures));
+    CHECK_TRUE(stop_failures.empty());
+    CHECK_TRUE(!hub.hasSource(front_video->id));
+    CHECK_TRUE(hub.hasSource(rear_video->id));
+    CHECK_TRUE(
+        hub.trackedSourceIds() ==
+        (std::vector<std::string>{"camera.rear"}));
+    CHECK_TRUE(front_video_stops.load(std::memory_order_acquire) == 2);
+    CHECK_TRUE(front_depth_stops.load(std::memory_order_acquire) == 1);
+    CHECK_TRUE(rear_stops.load(std::memory_order_acquire) == 0);
+
+    rear_subscription.reset();
+    CHECK_TRUE(rear_stops.load(std::memory_order_acquire) == 1);
+}
+
+void testDeviceStopsRunConcurrentlyAndSerializeMatchingRegistration() {
+    MediaSourceHub hub;
+    const auto first = makeVideoDescriptor(
+        Codec::H264, 1, {}, "first.video", "camera.first");
+    const auto second = makeVideoDescriptor(
+        Codec::H264, 1, {}, "second.video", "camera.second");
+
+    std::atomic<bool> first_stop_entered{false};
+    std::atomic<bool> second_stop_entered{false};
+    std::atomic<bool> release_stops{false};
+    auto register_blocking_source = [&](
+        const TrackDescriptorPtr& descriptor,
+        std::atomic<bool>& entered) {
+        MediaSourceHub::SourceCallbacks callbacks;
+        callbacks.start = [](
+            const MediaSourceHub::FrameSink&,
+            const MediaSourceHub::CancelPredicate&) { return true; };
+        callbacks.stop = [&entered, &release_stops] {
+            entered.store(true, std::memory_order_release);
+            while (!release_stops.load(std::memory_order_acquire)) {
+                std::this_thread::sleep_for(1ms);
+            }
+        };
+        return hub.registerSource(descriptor, std::move(callbacks), 2);
+    };
+
+    CHECK_TRUE(register_blocking_source(first, first_stop_entered));
+    CHECK_TRUE(register_blocking_source(second, second_stop_entered));
+    auto first_subscription = hub.subscribe(first->id);
+    auto second_subscription = hub.subscribe(second->id);
+    CHECK_TRUE(first_subscription.valid());
+    CHECK_TRUE(second_subscription.valid());
+
+    auto first_stop = std::async(std::launch::async, [&] {
+        return hub.stopSourcesForDevice(first->source_id);
+    });
+    const auto first_deadline = std::chrono::steady_clock::now() + 500ms;
+    while (!first_stop_entered.load(std::memory_order_acquire) &&
+           std::chrono::steady_clock::now() < first_deadline) {
+        std::this_thread::sleep_for(1ms);
+    }
+    CHECK_TRUE(first_stop_entered.load(std::memory_order_acquire));
+    CHECK_TRUE(
+        hub.trackedSourceIds() ==
+        (std::vector<std::string>{"camera.first", "camera.second"}));
+
+    auto second_stop = std::async(std::launch::async, [&] {
+        return hub.stopSourcesForDevice(second->source_id);
+    });
+    const auto second_deadline = std::chrono::steady_clock::now() + 500ms;
+    while (!second_stop_entered.load(std::memory_order_acquire) &&
+           std::chrono::steady_clock::now() < second_deadline) {
+        std::this_thread::sleep_for(1ms);
+    }
+    CHECK_TRUE(second_stop_entered.load(std::memory_order_acquire));
+
+    MediaSourceHub::SourceCallbacks replacement_callbacks;
+    replacement_callbacks.start = [](
+        const MediaSourceHub::FrameSink&,
+        const MediaSourceHub::CancelPredicate&) { return true; };
+    replacement_callbacks.stop = [] {};
+    auto matching_registration = std::async(std::launch::async, [&] {
+        return hub.registerSource(
+            makeVideoDescriptor(
+                Codec::H264,
+                2,
+                {},
+                "first.replacement",
+                "camera.first"),
+            std::move(replacement_callbacks),
+            2);
+    });
+    CHECK_TRUE(
+        matching_registration.wait_for(20ms) == std::future_status::timeout);
+
+    release_stops.store(true, std::memory_order_release);
+    CHECK_TRUE(first_stop.wait_for(500ms) == std::future_status::ready);
+    CHECK_TRUE(second_stop.wait_for(500ms) == std::future_status::ready);
+    if (first_stop.wait_for(0ms) == std::future_status::ready) {
+        CHECK_TRUE(first_stop.get());
+    }
+    if (second_stop.wait_for(0ms) == std::future_status::ready) {
+        CHECK_TRUE(second_stop.get());
+    }
+    CHECK_TRUE(
+        matching_registration.wait_for(500ms) == std::future_status::ready);
+    if (matching_registration.wait_for(0ms) == std::future_status::ready) {
+        CHECK_TRUE(matching_registration.get());
+    }
+    CHECK_TRUE(hub.hasSource("first.replacement"));
+    CHECK_TRUE(
+        hub.trackedSourceIds() ==
+        (std::vector<std::string>{"camera.first"}));
+}
+
+void testStopAllWaitsForDeviceStopAndRetainsItsConcurrentRegistrationRule() {
+    MediaSourceHub hub;
+    const auto first = makeVideoDescriptor(
+        Codec::H264, 1, {}, "first.video", "camera.first");
+    const auto other = makeVideoDescriptor(
+        Codec::H264, 1, {}, "other.video", "camera.other");
+    std::atomic<bool> first_stop_entered{false};
+    std::atomic<bool> release_first_stop{false};
+    std::atomic<int> other_stops{0};
+
+    MediaSourceHub::SourceCallbacks first_callbacks;
+    first_callbacks.start = [](
+        const MediaSourceHub::FrameSink&,
+        const MediaSourceHub::CancelPredicate&) { return true; };
+    first_callbacks.stop = [&] {
+        first_stop_entered.store(true, std::memory_order_release);
+        while (!release_first_stop.load(std::memory_order_acquire)) {
+            std::this_thread::sleep_for(1ms);
+        }
+    };
+    CHECK_TRUE(hub.registerSource(first, std::move(first_callbacks), 2));
+    auto first_subscription = hub.subscribe(first->id);
+    CHECK_TRUE(first_subscription.valid());
+
+    auto device_stop = std::async(std::launch::async, [&] {
+        return hub.stopSourcesForDevice(first->source_id);
+    });
+    const auto stop_deadline = std::chrono::steady_clock::now() + 500ms;
+    while (!first_stop_entered.load(std::memory_order_acquire) &&
+           std::chrono::steady_clock::now() < stop_deadline) {
+        std::this_thread::sleep_for(1ms);
+    }
+    CHECK_TRUE(first_stop_entered.load(std::memory_order_acquire));
+
+    auto stop_all = std::async(std::launch::async, [&] {
+        return hub.stopAllSources();
+    });
+    CHECK_TRUE(stop_all.wait_for(20ms) == std::future_status::timeout);
+
+    MediaSourceHub::SourceCallbacks other_callbacks;
+    other_callbacks.start = [](
+        const MediaSourceHub::FrameSink&,
+        const MediaSourceHub::CancelPredicate&) { return true; };
+    other_callbacks.stop = [&] { ++other_stops; };
+    CHECK_TRUE(hub.registerSource(other, std::move(other_callbacks), 2));
+    auto other_subscription = hub.subscribe(other->id);
+    CHECK_TRUE(other_subscription.valid());
+
+    release_first_stop.store(true, std::memory_order_release);
+    CHECK_TRUE(device_stop.wait_for(500ms) == std::future_status::ready);
+    if (device_stop.wait_for(0ms) == std::future_status::ready) {
+        CHECK_TRUE(device_stop.get());
+    }
+    CHECK_TRUE(stop_all.wait_for(500ms) == std::future_status::ready);
+    if (stop_all.wait_for(0ms) == std::future_status::ready) {
+        CHECK_TRUE(stop_all.get());
+    }
+    CHECK_TRUE(!hub.hasSource(first->id));
+    CHECK_TRUE(!hub.hasSource(other->id));
+    CHECK_TRUE(hub.trackedSourceIds().empty());
+    CHECK_TRUE(!other_subscription.valid());
+    CHECK_TRUE(other_stops.load(std::memory_order_acquire) == 1);
+}
+
+void testConcurrentRegistrationWaitsForStopAllSources() {
+    MediaSourceHub hub;
+    const auto descriptor = makeVideoDescriptor(Codec::H264, 1);
+    std::atomic<bool> stop_entered{false};
+    std::atomic<bool> release_stop{false};
+    std::atomic<int> old_stop_count{0};
+
+    MediaSourceHub::SourceCallbacks old_callbacks;
+    old_callbacks.start = [](const MediaSourceHub::FrameSink&,
+                             const MediaSourceHub::CancelPredicate&) {
+        return true;
+    };
+    old_callbacks.stop = [&] {
+        stop_entered.store(true, std::memory_order_release);
+        while (!release_stop.load(std::memory_order_acquire)) {
+            std::this_thread::sleep_for(1ms);
+        }
+        ++old_stop_count;
+    };
+    CHECK_TRUE(hub.registerSource(descriptor, std::move(old_callbacks), 2));
+    auto old_subscription = hub.subscribe(descriptor->id);
+    CHECK_TRUE(old_subscription.valid());
+
+    auto stop_all = std::async(
+        std::launch::async, [&] { return hub.stopAllSources(); });
+    const auto stop_deadline = std::chrono::steady_clock::now() + 500ms;
+    while (!stop_entered.load(std::memory_order_acquire) &&
+           std::chrono::steady_clock::now() < stop_deadline) {
+        std::this_thread::sleep_for(1ms);
+    }
+    CHECK_TRUE(stop_entered.load(std::memory_order_acquire));
+
+    std::atomic<int> new_start_count{0};
+    MediaSourceHub::SourceCallbacks new_callbacks;
+    new_callbacks.start = [&](const MediaSourceHub::FrameSink&,
+                              const MediaSourceHub::CancelPredicate&) {
+        ++new_start_count;
+        return true;
+    };
+    new_callbacks.stop = [] {};
+    auto registration = std::async(std::launch::async, [&] {
+        return hub.registerSource(
+            makeVideoDescriptor(Codec::H264, 2), std::move(new_callbacks), 2);
+    });
+
+    CHECK_TRUE(registration.wait_for(20ms) == std::future_status::timeout);
+    release_stop.store(true, std::memory_order_release);
+    CHECK_TRUE(stop_all.wait_for(500ms) == std::future_status::ready);
+    if (stop_all.wait_for(0ms) == std::future_status::ready) {
+        CHECK_TRUE(stop_all.get());
+    }
+    CHECK_TRUE(registration.wait_for(500ms) == std::future_status::ready);
+    const bool registered = registration.wait_for(0ms) == std::future_status::ready &&
+                            registration.get();
+    CHECK_TRUE(registered);
+    CHECK_TRUE(old_stop_count.load(std::memory_order_acquire) == 1);
+    CHECK_TRUE(hub.hasSource(descriptor->id));
+
+    auto new_subscription = hub.subscribe(descriptor->id);
+    CHECK_TRUE(new_subscription.valid());
+    CHECK_TRUE(new_subscription.descriptor()->generation == 2);
+    CHECK_TRUE(new_start_count.load(std::memory_order_acquire) == 1);
+}
+
+void testSystemStopAllAdmissionFencesRegistrationAndStartup() {
+    cmvr::service::StopAllAdmissionGate admission_gate;
+    MediaSourceHub hub(&admission_gate);
+    const auto dormant = makeVideoDescriptor(
+        Codec::H264, 1, {}, "dormant.video", "camera.dormant");
+    const auto new_source = makeVideoDescriptor(
+        Codec::H264, 1, {}, "new.video", "camera.new");
+    std::atomic<int> dormant_starts{0};
+    std::atomic<int> new_starts{0};
+
+    MediaSourceHub::SourceCallbacks dormant_callbacks;
+    dormant_callbacks.start = [&](
+        const MediaSourceHub::FrameSink&,
+        const MediaSourceHub::CancelPredicate&) {
+        ++dormant_starts;
+        return true;
+    };
+    dormant_callbacks.stop = [] {};
+    CHECK_TRUE(hub.registerSource(
+        dormant, std::move(dormant_callbacks), 2));
+
+    const auto stop_ticket = admission_gate.beginStopAll();
+    CHECK_TRUE(stop_ticket.valid());
+
+    MediaSourceHub::SourceCallbacks rejected_callbacks;
+    rejected_callbacks.start = [&](
+        const MediaSourceHub::FrameSink&,
+        const MediaSourceHub::CancelPredicate&) {
+        ++new_starts;
+        return true;
+    };
+    rejected_callbacks.stop = [] {};
+    CHECK_TRUE(!hub.registerSource(
+        new_source, std::move(rejected_callbacks), 2));
+    auto rejected_subscription = hub.subscribe(dormant->id);
+    CHECK_TRUE(!rejected_subscription.valid());
+    CHECK_TRUE(dormant_starts.load(std::memory_order_acquire) == 0);
+    CHECK_TRUE(new_starts.load(std::memory_order_acquire) == 0);
+
+    CHECK_TRUE(admission_gate.finishStopAll(stop_ticket, true));
+
+    MediaSourceHub::SourceCallbacks recovered_callbacks;
+    recovered_callbacks.start = [&](
+        const MediaSourceHub::FrameSink&,
+        const MediaSourceHub::CancelPredicate&) {
+        ++new_starts;
+        return true;
+    };
+    recovered_callbacks.stop = [] {};
+    CHECK_TRUE(hub.registerSource(
+        new_source, std::move(recovered_callbacks), 2));
+
+    auto dormant_subscription = hub.subscribe(dormant->id);
+    auto new_subscription = hub.subscribe(new_source->id);
+    CHECK_TRUE(dormant_subscription.valid());
+    CHECK_TRUE(new_subscription.valid());
+    CHECK_TRUE(dormant_starts.load(std::memory_order_acquire) == 1);
+    CHECK_TRUE(new_starts.load(std::memory_order_acquire) == 1);
+}
+
+void testSystemStopAllRejectsRegistrationWaitingForLocalStop() {
+    cmvr::service::StopAllAdmissionGate admission_gate;
+    MediaSourceHub hub(&admission_gate);
+    const auto old_source = makeVideoDescriptor(
+        Codec::H264, 1, {}, "old.video", "camera.shared");
+    const auto replacement = makeVideoDescriptor(
+        Codec::H264, 2, {}, "replacement.video", "camera.shared");
+    std::atomic<bool> stop_entered{false};
+    std::atomic<bool> release_stop{false};
+
+    MediaSourceHub::SourceCallbacks old_callbacks;
+    old_callbacks.start = [](
+        const MediaSourceHub::FrameSink&,
+        const MediaSourceHub::CancelPredicate&) { return true; };
+    old_callbacks.stop = [&] {
+        stop_entered.store(true, std::memory_order_release);
+        while (!release_stop.load(std::memory_order_acquire)) {
+            std::this_thread::sleep_for(1ms);
+        }
+    };
+    CHECK_TRUE(hub.registerSource(
+        old_source, std::move(old_callbacks), 2));
+    auto old_subscription = hub.subscribe(old_source->id);
+    CHECK_TRUE(old_subscription.valid());
+
+    auto local_stop = std::async(std::launch::async, [&] {
+        return hub.stopSourcesForDevice(old_source->source_id);
+    });
+    const auto stop_deadline = std::chrono::steady_clock::now() + 500ms;
+    while (!stop_entered.load(std::memory_order_acquire) &&
+           std::chrono::steady_clock::now() < stop_deadline) {
+        std::this_thread::sleep_for(1ms);
+    }
+    CHECK_TRUE(stop_entered.load(std::memory_order_acquire));
+
+    auto make_replacement_callbacks = [] {
+        MediaSourceHub::SourceCallbacks callbacks;
+        callbacks.start = [](
+            const MediaSourceHub::FrameSink&,
+            const MediaSourceHub::CancelPredicate&) { return true; };
+        callbacks.stop = [] {};
+        return callbacks;
+    };
+    auto waiting_registration = std::async(std::launch::async, [&] {
+        return hub.registerSource(
+            replacement, make_replacement_callbacks(), 2);
+    });
+    CHECK_TRUE(
+        waiting_registration.wait_for(20ms) ==
+        std::future_status::timeout);
+
+    const auto stop_ticket = admission_gate.beginStopAll();
+    release_stop.store(true, std::memory_order_release);
+    CHECK_TRUE(local_stop.wait_for(500ms) == std::future_status::ready);
+    if (local_stop.wait_for(0ms) == std::future_status::ready) {
+        CHECK_TRUE(local_stop.get());
+    }
+    CHECK_TRUE(
+        waiting_registration.wait_for(500ms) ==
+        std::future_status::ready);
+    if (waiting_registration.wait_for(0ms) == std::future_status::ready) {
+        CHECK_TRUE(!waiting_registration.get());
+    }
+    CHECK_TRUE(!hub.hasSource(replacement->id));
+
+    CHECK_TRUE(admission_gate.finishStopAll(stop_ticket, true));
+    CHECK_TRUE(hub.registerSource(
+        replacement, make_replacement_callbacks(), 2));
+    auto recovered = hub.subscribe(replacement->id);
+    CHECK_TRUE(recovered.valid());
+}
+
+void testSystemStopAllRejectsSubscriptionWaitingForLocalStop() {
+    cmvr::service::StopAllAdmissionGate admission_gate;
+    MediaSourceHub hub(&admission_gate);
+    const auto descriptor = makeVideoDescriptor(
+        Codec::H264, 1, {}, "waiting.video", "camera.waiting");
+    std::atomic<bool> stop_entered{false};
+    std::atomic<bool> release_stop{false};
+    std::atomic<int> starts{0};
+
+    MediaSourceHub::SourceCallbacks callbacks;
+    callbacks.start = [&](
+        const MediaSourceHub::FrameSink&,
+        const MediaSourceHub::CancelPredicate&) {
+        ++starts;
+        return true;
+    };
+    callbacks.stop = [&] {
+        stop_entered.store(true, std::memory_order_release);
+        while (!release_stop.load(std::memory_order_acquire)) {
+            std::this_thread::sleep_for(1ms);
+        }
+    };
+    CHECK_TRUE(hub.registerSource(
+        descriptor, std::move(callbacks), 2));
+    auto active = hub.subscribe(descriptor->id);
+    CHECK_TRUE(active.valid());
+
+    auto local_stop = std::async(std::launch::async, [&] {
+        active.reset();
+    });
+    const auto stop_deadline = std::chrono::steady_clock::now() + 500ms;
+    while (!stop_entered.load(std::memory_order_acquire) &&
+           std::chrono::steady_clock::now() < stop_deadline) {
+        std::this_thread::sleep_for(1ms);
+    }
+    CHECK_TRUE(stop_entered.load(std::memory_order_acquire));
+
+    auto waiting_subscription = std::async(std::launch::async, [&] {
+        return hub.subscribe(descriptor->id);
+    });
+    CHECK_TRUE(
+        waiting_subscription.wait_for(20ms) ==
+        std::future_status::timeout);
+
+    const auto stop_ticket = admission_gate.beginStopAll();
+    release_stop.store(true, std::memory_order_release);
+    CHECK_TRUE(local_stop.wait_for(500ms) == std::future_status::ready);
+    if (local_stop.wait_for(0ms) == std::future_status::ready) {
+        local_stop.get();
+    }
+    CHECK_TRUE(
+        waiting_subscription.wait_for(500ms) ==
+        std::future_status::ready);
+    if (waiting_subscription.wait_for(0ms) == std::future_status::ready) {
+        CHECK_TRUE(!waiting_subscription.get().valid());
+    }
+    CHECK_TRUE(starts.load(std::memory_order_acquire) == 1);
+
+    CHECK_TRUE(admission_gate.finishStopAll(stop_ticket, true));
+    auto recovered = hub.subscribe(descriptor->id);
+    CHECK_TRUE(recovered.valid());
+    CHECK_TRUE(starts.load(std::memory_order_acquire) == 2);
+}
+
+void testStopAllSourcesCancelsStartingSourceBeforeReuse() {
+    MediaSourceHub hub;
+    const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1);
+    std::atomic<bool> old_start_entered{false};
+    std::atomic<bool> release_old_start{false};
+    std::atomic<int> old_stop_count{0};
+
+    MediaSourceHub::SourceCallbacks old_callbacks;
+    old_callbacks.start = [&](const MediaSourceHub::FrameSink&,
+                              const MediaSourceHub::CancelPredicate& cancelled) {
+        old_start_entered.store(true, std::memory_order_release);
+        while (!release_old_start.load(std::memory_order_acquire)) {
+            if (cancelled()) {
+                break;
+            }
+            std::this_thread::sleep_for(1ms);
+        }
+        // Deliberately report a late success to exercise the abandoned-start
+        // stop path after stopAllSources has unregistered this source.
+        return true;
+    };
+    old_callbacks.stop = [&] { ++old_stop_count; };
+    CHECK_TRUE(hub.registerSource(descriptor, std::move(old_callbacks), 2));
+
+    auto old_subscription = std::async(std::launch::async, [&] {
+        return hub.subscribe(descriptor->id);
+    });
+    const auto start_deadline = std::chrono::steady_clock::now() + 500ms;
+    while (!old_start_entered.load(std::memory_order_acquire) &&
+           std::chrono::steady_clock::now() < start_deadline) {
+        std::this_thread::sleep_for(1ms);
+    }
+    CHECK_TRUE(old_start_entered.load(std::memory_order_acquire));
+
+    CHECK_TRUE(!hub.stopAllSources());
+    CHECK_TRUE(hub.hasSource(descriptor->id));
+    CHECK_TRUE(old_subscription.wait_for(500ms) == std::future_status::ready);
+    if (old_subscription.wait_for(0ms) == std::future_status::ready) {
+        CHECK_TRUE(!old_subscription.get().valid());
+    }
+
+    release_old_start.store(true, std::memory_order_release);
+    const auto old_stop_deadline = std::chrono::steady_clock::now() + 500ms;
+    while (old_stop_count.load(std::memory_order_acquire) != 1 &&
+           std::chrono::steady_clock::now() < old_stop_deadline) {
+        std::this_thread::sleep_for(1ms);
+    }
+    CHECK_TRUE(old_stop_count.load(std::memory_order_acquire) == 1);
+    CHECK_TRUE(hub.stopAllSources());
+
+    std::atomic<int> new_start_count{0};
+    MediaSourceHub::SourceCallbacks new_callbacks;
+    new_callbacks.start = [&](const MediaSourceHub::FrameSink&,
+                              const MediaSourceHub::CancelPredicate&) {
+        ++new_start_count;
+        return true;
+    };
+    new_callbacks.stop = [] {};
+    CHECK_TRUE(hub.registerSource(
+        makeVideoDescriptor(Codec::H264, 2), std::move(new_callbacks), 2));
+    auto new_subscription = hub.subscribe(descriptor->id);
+    CHECK_TRUE(new_subscription.valid());
+    CHECK_TRUE(new_start_count.load(std::memory_order_acquire) == 1);
+    CHECK_TRUE(new_subscription.valid());
+}
+
 void testKeyFrameRequestIsOrderedBeforeStop() {
     MediaSourceHub hub;
     const auto descriptor = makeVideoDescriptor(Codec::H264, 1);
@@ -670,6 +1315,16 @@ int main() {
     testHubLifecycleAndDescriptorRefresh();
     testSubscriptionDiscardPending();
     testHubFailedStartAndShutdown();
+    testHubStopAllSourcesAllowsReregistration();
+    testStopAllSourcesReportsAndRetriesUnconfirmedStop();
+    testStopSourcesForDeviceIsSelectiveAndRetriesFailures();
+    testDeviceStopsRunConcurrentlyAndSerializeMatchingRegistration();
+    testStopAllWaitsForDeviceStopAndRetainsItsConcurrentRegistrationRule();
+    testConcurrentRegistrationWaitsForStopAllSources();
+    testSystemStopAllAdmissionFencesRegistrationAndStartup();
+    testSystemStopAllRejectsRegistrationWaitingForLocalStop();
+    testSystemStopAllRejectsSubscriptionWaitingForLocalStop();
+    testStopAllSourcesCancelsStartingSourceBeforeReuse();
     testKeyFrameRequestIsOrderedBeforeStop();
     testHubCancelsBlockedStartWithoutBlockingShutdown();
     testHubQuarantinesNonCooperativeStart();

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

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

@@ -1,5 +1,6 @@
 #include "manager/task_manager/include/task_manager.h"
 
+#include <algorithm>
 #include <chrono>
 #include <cmath>
 #include <stdexcept>
@@ -8,6 +9,7 @@
 
 #include "common/base/logging/logger.h"
 #include "common/config/config_files.h"
+#include "service/stop_all/include/stop_all_admission_gate.h"
 #include "task/task_factory.h"
 
 using namespace cmvr;
@@ -119,11 +121,46 @@ TaskManager& TaskManager::getInstance()
 
 void TaskManager::destroyInstance()
 {
-    std::lock_guard lock(init_mutex_);
-    if (instance_) {
-        instance_->stopRunTask();
+    std::shared_ptr<TaskManager> instance;
+    {
+        std::lock_guard lock(init_mutex_);
+        instance = instance_;
     }
-    instance_.reset();
+    if (instance) {
+        // Task shutdown may wait for an in-flight SystemService handler. That
+        // handler can query the process-wide task snapshot, so never retain
+        // init_mutex_ while stopping tasks or joining service workers.
+        instance->stopRunTask();
+    }
+    {
+        std::lock_guard lock(init_mutex_);
+        if (instance_ == instance) {
+            instance_.reset();
+        }
+    }
+}
+
+bool TaskManager::stopAllActivitiesIfInitialized(
+    std::vector<std::string>* failures)
+{
+    std::shared_ptr<TaskManager> manager;
+    {
+        std::lock_guard lock(init_mutex_);
+        manager = instance_;
+    }
+    return !manager || manager->stopAllActivities(failures);
+}
+
+std::vector<std::shared_ptr<Task>>
+TaskManager::activitySnapshotIfInitialized()
+{
+    std::shared_ptr<TaskManager> manager;
+    {
+        std::lock_guard lock(init_mutex_);
+        manager = instance_;
+    }
+    return manager ? manager->activitySnapshot()
+                   : std::vector<std::shared_ptr<Task>>{};
 }
 
 std::shared_ptr<TouchScreenTask> TaskManager::getTouchScreenTask(const std::string& task_id) const
@@ -147,9 +184,73 @@ std::shared_ptr<Task> TaskManager::getTask(const std::string& task_id) const
     return it->second;
 }
 
+bool TaskManager::stopAllActivities(std::vector<std::string>* failures)
+{
+    const auto tasks = activitySnapshot();
+
+    bool all_stopped = true;
+    for (const auto& task : tasks) {
+        bool stopped = false;
+        try {
+            stopped = task->stopActivity();
+        } catch (const std::exception& error) {
+            if (failures) {
+                failures->push_back(
+                    task->id() + ": stop threw: " + error.what());
+            }
+        } catch (...) {
+            if (failures) {
+                failures->push_back(
+                    task->id() + ": stop threw an unknown exception");
+            }
+        }
+        if (!stopped) {
+            all_stopped = false;
+            if (failures && (failures->empty() ||
+                failures->back().compare(0, task->id().size(), task->id()) != 0)) {
+                failures->push_back(
+                    task->id() +
+                    ": operational stop was not confirmed");
+            }
+        }
+    }
+    return all_stopped;
+}
+
+std::vector<std::shared_ptr<Task>> TaskManager::activitySnapshot() const
+{
+    std::vector<std::shared_ptr<Task>> tasks;
+    std::lock_guard lock(tasks_mutex_);
+    tasks.reserve(tasks_.size());
+    for (const auto& [id, task] : tasks_) {
+        (void)id;
+        if (task) {
+            tasks.push_back(task);
+        }
+    }
+    return tasks;
+}
+
 bool TaskManager::startRunTask(const double control_period_s)
 {
+    auto& admission_gate = service::globalStopAllAdmissionGate();
+    std::uint64_t admission_generation = 0U;
+    {
+        auto admission = admission_gate.lockAdmission();
+        if (!admission.accepting()) {
+            CMVR_LOG(WARNING) << "[TaskManager] task startup is paused by "
+                                 "System StopAll";
+            return false;
+        }
+        admission_generation = admission.generation();
+    }
+
     std::lock_guard lifecycle_lock(lifecycle_mutex_);
+    const auto admission_current = [&] {
+        auto admission = admission_gate.lockAdmission();
+        return admission.accepting() &&
+            admission.generation() == admission_generation;
+    };
     if (!initialized_) {
         CMVR_LOG(ERROR) << "[TaskManager] refusing to start because "
                            "initialization did not complete";
@@ -160,7 +261,7 @@ bool TaskManager::startRunTask(const double control_period_s)
         return false;
     }
     if (running_.load()) {
-        return true;
+        return admission_current();
     }
 
     std::vector<std::shared_ptr<Task>> tasks;
@@ -176,6 +277,17 @@ bool TaskManager::startRunTask(const double control_period_s)
 
     std::vector<std::shared_ptr<Task>> started_tasks;
     for (const auto& task : tasks) {
+        if (!admission_current()) {
+            CMVR_LOG(WARNING) << "[TaskManager] task startup was interrupted "
+                                 "by System StopAll";
+            for (auto it = started_tasks.rbegin();
+                 it != started_tasks.rend(); ++it) {
+                stopTaskNoThrow(*it);
+            }
+            running_.store(false);
+            return false;
+        }
+
         bool started = false;
         try {
             started = task->start();
@@ -197,11 +309,31 @@ bool TaskManager::startRunTask(const double control_period_s)
             return false;
         }
         started_tasks.push_back(task);
+        if (!admission_current()) {
+            CMVR_LOG(WARNING) << "[TaskManager] task startup crossed a System "
+                                 "StopAll boundary: " << task->id();
+            for (auto it = started_tasks.rbegin();
+                 it != started_tasks.rend(); ++it) {
+                stopTaskNoThrow(*it);
+            }
+            running_.store(false);
+            return false;
+        }
     }
 
-    running_.store(true);
+    bool admission_changed = false;
     try {
-        run_thread_ = std::thread(&TaskManager::runTaskLoop, this, control_period_s);
+        // Publish the scheduler under a short admission guard. No task/device
+        // call or rollback is made while the global StopAll mutex is held.
+        auto admission = admission_gate.lockAdmission();
+        if (!admission.accepting() ||
+            admission.generation() != admission_generation) {
+            admission_changed = true;
+        } else {
+            running_.store(true);
+            run_thread_ = std::thread(
+                &TaskManager::runTaskLoop, this, control_period_s);
+        }
     } catch (const std::exception& e) {
         CMVR_LOG(ERROR) << "[TaskManager] failed to start run thread: " << e.what();
         running_.store(false);
@@ -220,6 +352,16 @@ bool TaskManager::startRunTask(const double control_period_s)
         }
         return false;
     }
+    if (admission_changed) {
+        CMVR_LOG(WARNING) << "[TaskManager] scheduler startup was "
+                             "interrupted by System StopAll";
+        for (auto it = started_tasks.rbegin();
+             it != started_tasks.rend(); ++it) {
+            stopTaskNoThrow(*it);
+        }
+        running_.store(false);
+        return false;
+    }
     return true;
 }
 
@@ -244,6 +386,9 @@ void TaskManager::stopRunTask()
             }
         }
     }
+    std::sort(tasks.begin(), tasks.end(), [](const auto& lhs, const auto& rhs) {
+        return lhs->shutdownPhase() < rhs->shutdownPhase();
+    });
     for (const auto& task : tasks) {
         stopTaskNoThrow(task);
     }

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

@@ -1,10 +1,18 @@
 #include "manager/task_manager/include/task_manager.h"
 
+#include <atomic>
+#include <condition_variable>
+#include <future>
 #include <memory>
+#include <mutex>
 #include <stdexcept>
+#include <string>
+#include <thread>
+#include <vector>
 
 #include <gtest/gtest.h>
 
+#include "service/stop_all/include/stop_all_admission_gate.h"
 #include "task/task_factory.h"
 
 namespace {
@@ -13,14 +21,22 @@ struct TaskBehavior {
     bool init_result{true};
     bool start_result{true};
     bool throw_on_start{false};
+    bool stop_activity_result{true};
+    bool block_start{false};
+    bool query_snapshot_on_stop{false};
 };
 
 TaskBehavior task_behavior;
+std::vector<std::string> stop_order;
 
 class LifecycleTask final : public cmvr::task::Task {
 public:
-    explicit LifecycleTask(std::string id)
-        : id_(std::move(id))
+    explicit LifecycleTask(
+        std::string id,
+        const cmvr::task::TaskShutdownPhase shutdown_phase =
+            cmvr::task::TaskShutdownPhase::DEPENDENT_ACTIVITY)
+        : id_(std::move(id)),
+          shutdown_phase_(shutdown_phase)
     {
     }
 
@@ -29,6 +45,10 @@ public:
     {
         return cmvr::task::TaskRunMode::BLOCKING_SERVICE;
     }
+    cmvr::task::TaskShutdownPhase shutdownPhase() const override
+    {
+        return shutdown_phase_;
+    }
 
     bool init() override
     {
@@ -42,6 +62,14 @@ public:
     bool start() override
     {
         ++start_calls;
+        {
+            std::unique_lock lock(start_mutex);
+            start_entered = true;
+            start_condition.notify_all();
+            start_condition.wait(lock, [] {
+                return !task_behavior.block_start;
+            });
+        }
         if (task_behavior.throw_on_start) {
             throw std::runtime_error("start failure");
         }
@@ -56,9 +84,19 @@ public:
     void stop() override
     {
         ++stop_calls;
+        if (task_behavior.query_snapshot_on_stop) {
+            (void)cmvr::task::TaskManager::activitySnapshotIfInitialized();
+        }
+        stop_order.push_back(id_);
         state_ = cmvr::task::TaskState::STOPPED;
     }
 
+    bool stopActivity() override
+    {
+        ++stop_activity_calls;
+        return task_behavior.stop_activity_result;
+    }
+
     cmvr::task::TaskState state() const override { return state_; }
     bool isBusy() const override
     {
@@ -84,14 +122,20 @@ public:
     int init_calls{0};
     int start_calls{0};
     int stop_calls{0};
+    int stop_activity_calls{0};
+    std::mutex start_mutex;
+    std::condition_variable start_condition;
+    bool start_entered{false};
 
 private:
     std::string id_;
+    cmvr::task::TaskShutdownPhase shutdown_phase_;
     cmvr::task::TaskState state_{
         cmvr::task::TaskState::UNINITIALIZED};
 };
 
 std::shared_ptr<LifecycleTask> created_task;
+std::shared_ptr<LifecycleTask> created_ingress_task;
 
 cmvr::config::TaskManagerConfig enabledTaskConfig()
 {
@@ -112,8 +156,11 @@ protected:
     void SetUp() override
     {
         cmvr::task::TaskManager::destroyInstance();
+        cmvr::service::globalStopAllAdmissionGate().clearForTesting();
         task_behavior = {};
         created_task.reset();
+        created_ingress_task.reset();
+        stop_order.clear();
         cmvr::task::TaskFactory::registerCreator(
             cmvr::config::TaskConfigEntry::TASK_TYPE_UME_TELEOP,
             [](const cmvr::config::TaskConfigEntry& entry) {
@@ -125,8 +172,18 @@ protected:
 
     void TearDown() override
     {
+        if (created_task) {
+            {
+                std::lock_guard lock(created_task->start_mutex);
+                task_behavior.block_start = false;
+            }
+            created_task->start_condition.notify_all();
+        }
         cmvr::task::TaskManager::destroyInstance();
+        cmvr::service::globalStopAllAdmissionGate().clearForTesting();
         created_task.reset();
+        created_ingress_task.reset();
+        stop_order.clear();
     }
 };
 
@@ -191,4 +248,181 @@ TEST_F(TaskManagerLifecycleTest, SuccessfulStartAndStopAreReported)
     EXPECT_EQ(created_task->stop_calls, 1);
 }
 
+TEST_F(TaskManagerLifecycleTest,
+       CommandIngressStopsBeforeDependentTaskActivity)
+{
+    cmvr::task::TaskFactory::registerCreator(
+        cmvr::config::TaskConfigEntry::TASK_TYPE_GRPC_SERVER,
+        [](const cmvr::config::TaskConfigEntry& entry) {
+            created_ingress_task = std::make_shared<LifecycleTask>(
+                entry.id(),
+                cmvr::task::TaskShutdownPhase::COMMAND_INGRESS);
+            return created_ingress_task;
+        });
+
+    auto config = enabledTaskConfig();
+    auto* ingress = config.add_tasks();
+    ingress->set_id("control_ingress");
+    ingress->set_type(
+        cmvr::config::TaskConfigEntry::TASK_TYPE_GRPC_SERVER);
+    ingress->set_enable(true);
+    ingress->set_run_mode(
+        cmvr::config::TaskConfigEntry::TASK_RUN_MODE_BLOCKING_SERVICE);
+
+    auto& manager = cmvr::task::TaskManager::getInstance(config);
+    ASSERT_TRUE(manager.initialized());
+    ASSERT_TRUE(manager.startRunTask());
+    manager.stopRunTask();
+
+    ASSERT_EQ(stop_order.size(), 2U);
+    EXPECT_EQ(stop_order[0], "control_ingress");
+    EXPECT_EQ(stop_order[1], "lifecycle_task");
+}
+
+TEST_F(TaskManagerLifecycleTest,
+       DestroyDoesNotHoldSingletonLockWhileStoppingTasks)
+{
+    task_behavior.query_snapshot_on_stop = true;
+    auto& manager =
+        cmvr::task::TaskManager::getInstance(enabledTaskConfig());
+    ASSERT_TRUE(manager.initialized());
+    ASSERT_TRUE(manager.startRunTask());
+
+    auto destroy = std::async(std::launch::async, [] {
+        cmvr::task::TaskManager::destroyInstance();
+    });
+    ASSERT_EQ(
+        destroy.wait_for(std::chrono::seconds(1)),
+        std::future_status::ready);
+    destroy.get();
+
+    ASSERT_NE(created_task, nullptr);
+    EXPECT_EQ(created_task->stop_calls, 1);
+}
+
+TEST_F(TaskManagerLifecycleTest,
+       StopAllActivitiesDoesNotStopSchedulerOrTaskLifecycle)
+{
+    auto& manager =
+        cmvr::task::TaskManager::getInstance(enabledTaskConfig());
+
+    ASSERT_TRUE(manager.initialized());
+    ASSERT_TRUE(manager.startRunTask());
+    ASSERT_NE(created_task, nullptr);
+    EXPECT_TRUE(manager.stopAllActivities());
+    EXPECT_TRUE(manager.running());
+    EXPECT_EQ(created_task->stop_activity_calls, 1);
+    EXPECT_EQ(created_task->stop_calls, 0);
+
+    manager.stopRunTask();
+    EXPECT_EQ(created_task->stop_calls, 1);
+}
+
+TEST_F(TaskManagerLifecycleTest,
+       StopAllActivitiesReportsUnconfirmedOperationalStop)
+{
+    task_behavior.stop_activity_result = false;
+    auto& manager =
+        cmvr::task::TaskManager::getInstance(enabledTaskConfig());
+
+    std::vector<std::string> failures;
+    EXPECT_FALSE(manager.stopAllActivities(&failures));
+    ASSERT_EQ(failures.size(), 1U);
+    EXPECT_NE(failures.front().find("lifecycle_task"), std::string::npos);
+    EXPECT_EQ(created_task->stop_calls, 0);
+}
+
+TEST_F(TaskManagerLifecycleTest,
+       StopAllActivitiesIsSuccessfulWhenManagerIsNotInitialized)
+{
+    cmvr::task::TaskManager::destroyInstance();
+    std::vector<std::string> failures;
+    EXPECT_TRUE(
+        cmvr::task::TaskManager::stopAllActivitiesIfInitialized(&failures));
+    EXPECT_TRUE(failures.empty());
+}
+
+TEST_F(TaskManagerLifecycleTest, StartIsRejectedWhileStopAllAdmissionIsClosed)
+{
+    auto& manager =
+        cmvr::task::TaskManager::getInstance(enabledTaskConfig());
+    auto& gate = cmvr::service::globalStopAllAdmissionGate();
+    const auto ticket = gate.beginStopAll();
+
+    EXPECT_FALSE(manager.startRunTask());
+    EXPECT_FALSE(manager.running());
+    ASSERT_NE(created_task, nullptr);
+    EXPECT_EQ(created_task->start_calls, 0);
+
+    EXPECT_TRUE(gate.finishStopAll(ticket, true));
+    EXPECT_TRUE(manager.startRunTask());
+    EXPECT_TRUE(manager.running());
+}
+
+TEST_F(TaskManagerLifecycleTest,
+       StartCrossingStopAllGenerationRollsBackStartedTasks)
+{
+    task_behavior.block_start = true;
+    auto& manager =
+        cmvr::task::TaskManager::getInstance(enabledTaskConfig());
+    ASSERT_NE(created_task, nullptr);
+
+    std::atomic<bool> start_result{true};
+    std::thread starter([&] {
+        start_result.store(manager.startRunTask());
+    });
+    bool start_entered = false;
+    {
+        std::unique_lock lock(created_task->start_mutex);
+        start_entered = created_task->start_condition.wait_for(
+            lock, std::chrono::seconds(2), [&] {
+                return created_task->start_entered;
+            });
+    }
+    if (!start_entered) {
+        {
+            std::lock_guard lock(created_task->start_mutex);
+            task_behavior.block_start = false;
+        }
+        created_task->start_condition.notify_all();
+        starter.join();
+        FAIL() << "task start did not reach the generation-race barrier";
+    }
+
+    auto& gate = cmvr::service::globalStopAllAdmissionGate();
+    const auto ticket = gate.beginStopAll();
+    {
+        std::lock_guard lock(created_task->start_mutex);
+        task_behavior.block_start = false;
+    }
+    created_task->start_condition.notify_all();
+    starter.join();
+
+    EXPECT_FALSE(start_result.load());
+    EXPECT_FALSE(manager.running());
+    EXPECT_EQ(created_task->start_calls, 1);
+    EXPECT_EQ(created_task->stop_calls, 1);
+
+    EXPECT_TRUE(gate.finishStopAll(ticket, true));
+    EXPECT_TRUE(manager.startRunTask());
+}
+
+TEST_F(TaskManagerLifecycleTest,
+       FailedStopAllKeepsTaskStartupRejectedUntilSuccessfulRound)
+{
+    auto& manager =
+        cmvr::task::TaskManager::getInstance(enabledTaskConfig());
+    auto& gate = cmvr::service::globalStopAllAdmissionGate();
+    auto ticket = gate.beginStopAll();
+    EXPECT_FALSE(gate.finishStopAll(ticket, false));
+
+    EXPECT_FALSE(manager.startRunTask());
+    ASSERT_NE(created_task, nullptr);
+    EXPECT_EQ(created_task->start_calls, 0);
+
+    ticket = gate.beginStopAll();
+    EXPECT_TRUE(gate.finishStopAll(ticket, true));
+    EXPECT_TRUE(manager.startRunTask());
+}
+
 } // namespace

cmvr-es/service/CMakeLists.txt

@@ -1,7 +1,12 @@
 
 add_library(service
+        stop_all/src/stop_operation_dispatcher.cpp
         action/src/action_queue_executor.cpp
+        grpc/src/camera_ptz_activity_registry.cpp
+        grpc/src/media_activity_coordinator.cpp
+        grpc/src/motor_activity_coordinator.cpp
         grpc/src/grpc_camera_service.cpp
+        grpc/src/grpc_error_logging_interceptor.cpp
         grpc/src/grpc_system_service.cpp
         grpc/src/grpc_speaker_service.cpp
         grpc/src/grpc_microphone_service.cpp
@@ -20,6 +25,8 @@ target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 target_link_libraries(service PRIVATE
         cmvr_es::proto
+        cmvr_es::stop_all_admission_gate
+        cmvr_es::camera_operational_activity_registry
         osqp
         cmvr_es::control_authority
         cmvr_es::device_manager
@@ -35,6 +42,136 @@ add_library(cmvr_es::service ALIAS service)
 install(TARGETS service LIBRARY DESTINATION lib)
 
 if(BUILD_TESTING)
+    add_executable(stop_all_admission_gate_test
+            stop_all/tests/stop_all_admission_gate_test.cpp
+            stop_all/src/stop_all_admission_gate.cpp
+    )
+    target_include_directories(stop_all_admission_gate_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+    )
+    target_link_libraries(stop_all_admission_gate_test PRIVATE
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME stop_all_admission_gate_test
+            COMMAND stop_all_admission_gate_test
+    )
+    set_tests_properties(stop_all_admission_gate_test PROPERTIES TIMEOUT 10)
+
+    add_executable(stop_operation_dispatcher_test
+            stop_all/tests/stop_operation_dispatcher_test.cpp
+            stop_all/src/stop_operation_dispatcher.cpp
+    )
+    target_include_directories(stop_operation_dispatcher_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+    )
+    target_link_libraries(stop_operation_dispatcher_test PRIVATE
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME stop_operation_dispatcher_test
+            COMMAND stop_operation_dispatcher_test
+    )
+    set_tests_properties(stop_operation_dispatcher_test PROPERTIES TIMEOUT 10)
+
+    add_executable(camera_operational_activity_registry_test
+            grpc/tests/camera_operational_activity_registry_test.cpp
+    )
+    target_include_directories(camera_operational_activity_registry_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+    )
+    target_link_libraries(camera_operational_activity_registry_test PRIVATE
+            cmvr_es::proto
+            cmvr_es::camera_operational_activity_registry
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME camera_operational_activity_registry_test
+            COMMAND camera_operational_activity_registry_test
+    )
+    set_tests_properties(camera_operational_activity_registry_test PROPERTIES
+            TIMEOUT 10)
+
+    add_executable(camera_ptz_activity_registry_test
+            grpc/tests/camera_ptz_activity_registry_test.cpp
+            grpc/src/camera_ptz_activity_registry.cpp
+            stop_all/src/stop_all_admission_gate.cpp
+    )
+    target_include_directories(camera_ptz_activity_registry_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+    )
+    target_link_libraries(camera_ptz_activity_registry_test PRIVATE
+            cmvr_es::proto
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME camera_ptz_activity_registry_test
+            COMMAND camera_ptz_activity_registry_test
+    )
+    set_tests_properties(camera_ptz_activity_registry_test PROPERTIES TIMEOUT 10)
+
+    add_executable(media_activity_coordinator_test
+            grpc/tests/media_activity_coordinator_test.cpp
+            grpc/src/media_activity_coordinator.cpp
+            stop_all/src/stop_all_admission_gate.cpp
+    )
+    target_include_directories(media_activity_coordinator_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+    )
+    target_link_libraries(media_activity_coordinator_test PRIVATE
+            cmvr_es::logging
+            pthread
+    )
+    add_test(
+            NAME media_activity_coordinator_test
+            COMMAND media_activity_coordinator_test
+    )
+    set(_grpc_media_test_environment
+            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
+    )
+    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+        list(APPEND _grpc_media_test_environment
+                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+    endif()
+    set_tests_properties(media_activity_coordinator_test PROPERTIES
+            TIMEOUT 10
+            ENVIRONMENT "${_grpc_media_test_environment}"
+    )
+
+    add_executable(motor_activity_coordinator_test
+            grpc/tests/motor_activity_coordinator_test.cpp
+            grpc/src/motor_activity_coordinator.cpp
+    )
+    target_include_directories(motor_activity_coordinator_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+    )
+    target_link_libraries(motor_activity_coordinator_test PRIVATE
+            cmvr_es::logging
+            pthread
+    )
+    add_test(
+            NAME motor_activity_coordinator_test
+            COMMAND motor_activity_coordinator_test
+    )
+    set_tests_properties(motor_activity_coordinator_test PROPERTIES
+            TIMEOUT 10
+            ENVIRONMENT "${_grpc_media_test_environment}"
+    )
+
     add_executable(grpc_camera_stream_policy_test
             grpc/tests/grpc_camera_stream_policy_test.cpp
     )
@@ -80,6 +217,31 @@ if(BUILD_TESTING)
             ENVIRONMENT "${_grpc_system_test_environment}"
     )
 
+    add_executable(grpc_error_logging_interceptor_test
+            grpc/tests/grpc_error_logging_interceptor_test.cpp
+            grpc/src/grpc_error_logging_interceptor.cpp
+    )
+    target_include_directories(grpc_error_logging_interceptor_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+    )
+    target_link_libraries(grpc_error_logging_interceptor_test
+            PRIVATE
+            cmvr_es::logging
+            cmvr_es::proto
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME grpc_error_logging_interceptor_test
+            COMMAND grpc_error_logging_interceptor_test
+    )
+    set_tests_properties(grpc_error_logging_interceptor_test PROPERTIES
+            TIMEOUT 10
+            ENVIRONMENT "${_grpc_system_test_environment}"
+    )
+
     add_executable(grpc_arm_service_test
             grpc/tests/grpc_arm_service_test.cpp
     )
@@ -222,6 +384,56 @@ if(BUILD_TESTING)
             ENVIRONMENT "${_grpc_agv_test_environment}"
     )
 
+    add_executable(grpc_head_service_test
+            grpc/tests/grpc_head_service_test.cpp
+    )
+    target_include_directories(grpc_head_service_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+            ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager
+    )
+    target_link_libraries(grpc_head_service_test
+            PRIVATE
+            service
+            cmvr_es::proto
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME grpc_head_service_test
+            COMMAND grpc_head_service_test
+    )
+    set_tests_properties(grpc_head_service_test PROPERTIES
+            TIMEOUT 15
+            ENVIRONMENT "${_grpc_system_test_environment}"
+    )
+
+    add_executable(grpc_dexhand_service_test
+            grpc/tests/grpc_dexhand_service_test.cpp
+    )
+    target_include_directories(grpc_dexhand_service_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+            ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager
+    )
+    target_link_libraries(grpc_dexhand_service_test
+            PRIVATE
+            service
+            cmvr_es::proto
+            gtest
+            gtest_main
+            pthread
+    )
+    add_test(
+            NAME grpc_dexhand_service_test
+            COMMAND grpc_dexhand_service_test
+    )
+    set_tests_properties(grpc_dexhand_service_test PROPERTIES
+            TIMEOUT 15
+            ENVIRONMENT "${_grpc_system_test_environment}"
+    )
+
 endif()
 
 # --------------------------------------------------------

cmvr-es/service/action/include/action_queue_executor.h

@@ -3,6 +3,7 @@
 
 #include <chrono>
 #include <cstddef>
+#include <cstdint>
 #include <functional>
 #include <memory>
 #include <string>
@@ -30,6 +31,20 @@ public:
         CanceledAfterAdmission,
     };
 
+    // Identifies one participant in a StopAll round. Multiple concurrent
+    // StopAll callers join the same round; ActionQueue admission resumes only
+    // after every ticket in that round has been completed successfully.
+    struct StopAllTicket {
+        std::uint64_t generation{0};
+        std::uint64_t ticket_id{0};
+        bool active_action_stop_confirmed{false};
+
+        bool valid() const noexcept
+        {
+            return generation != 0U && ticket_id != 0U;
+        }
+    };
+
     explicit ActionQueueExecutor(
         device::DeviceManager& device_manager,
         std::size_t max_accepted_action_ids =
@@ -46,11 +61,31 @@ public:
         api::ActionQueueCommand_Feedback& feedback,
         const std::function<bool()>& waiter_canceled = {});
 
-    // StopAll uses this fail-closed transition. It rejects future submissions,
-    // cancels pending actions, and requests a typed stop for the active action.
-    // Returns true when every active Action device reported a confirmed stop.
-    // False means at least one resource remains fail-closed quarantined.
-    bool cancelAllAndDisable();
+    // Starts (or joins) a temporary StopAll round. New action IDs are rejected
+    // and queued/active actions are canceled. By default the executor also
+    // requests a typed stop for the active action. SystemService delegates that
+    // stop to its whole-machine sweep so one slow Action backend cannot delay
+    // stop requests for every other device.
+    // Existing action IDs remain queryable for idempotent reconciliation. An
+    // invalid ticket means permanent shutdown has already started.
+    StopAllTicket beginStopAll(bool delegate_active_stop = false);
+
+    // Completes a StopAll participant after the caller has stopped and
+    // confirmed all other devices. The queue resumes only when every ticket in
+    // the current round reports success and the worker is idle. True means this
+    // ticket was completed successfully; another concurrent ticket may still
+    // keep the queue paused. False is fail-closed: the ticket was stale,
+    // shutdown won the race, a stop was not confirmed, or the executor was not
+    // idle when the last ticket completed.
+    bool finishStopAll(
+        const StopAllTicket& ticket,
+        bool all_devices_stop_confirmed);
+
+    // Permanently rejects new actions, cancels queued/active work, requests a
+    // typed stop, and tells the worker to exit after canceled work is drained.
+    // This transition is irreversible for this executor instance. Returns true
+    // when the active Action devices reported a confirmed stop.
+    bool disableForShutdown();
 
     bool waitForIdle(std::chrono::milliseconds timeout);
     const std::string& instanceId() const noexcept;

cmvr-es/service/action/src/action_queue_executor.cpp

@@ -38,6 +38,7 @@
 #include "devices/arm/robot_arm.h"
 #include "manager/control_authority/include/control_authority_manager.h"
 #include "manager/device_manager/include/device_manager.h"
+#include "service/stop_all/include/stop_all_admission_gate.h"
 
 namespace cmvr::service {
 namespace {
@@ -732,6 +733,12 @@ device::AgvActionKind toAgvActionKind(
 } // namespace
 
 struct ActionQueueExecutor::Impl {
+    enum class RunState {
+        Accepting,
+        PausedForStopAll,
+        ShuttingDown,
+    };
+
     struct Record {
         api::ActionQueueCommand_Request request;
         RequestFingerprint fingerprint;
@@ -840,53 +847,131 @@ struct ActionQueueExecutor::Impl {
 
     void shutdown()
     {
-        std::shared_ptr<Record> active_record;
-        {
-            std::lock_guard lock(mutex);
-            if (joined) {
-                return;
-            }
-            accepting = false;
-            stopping = true;
-            for (const auto& record : queue) {
-                record->cancel_requested.store(
-                    true, std::memory_order_release);
-                record->condition.notify_all();
-            }
-            active_record = active;
-            if (active_record) {
-                active_record->cancel_requested.store(
-                    true, std::memory_order_release);
-                active_record->condition.notify_all();
-            }
+        if (joined) {
+            return;
         }
-        (void)requestTypedStop(active_record);
-        queue_condition.notify_all();
+        (void)disableForShutdown();
         if (worker.joinable()) {
             worker.join();
         }
         joined = true;
     }
 
-    bool cancelAllAndDisable()
+    void cancelAllLocked(std::shared_ptr<Record>& active_record)
+    {
+        for (const auto& record : queue) {
+            record->cancel_requested.store(
+                true, std::memory_order_release);
+            record->condition.notify_all();
+        }
+        active_record = active;
+        if (active_record) {
+            active_record->cancel_requested.store(
+                true, std::memory_order_release);
+            active_record->condition.notify_all();
+        }
+    }
+
+    ActionQueueExecutor::StopAllTicket beginStopAll(
+        const bool delegate_active_stop)
+    {
+        ActionQueueExecutor::StopAllTicket ticket;
+        std::shared_ptr<Record> active_record;
+        {
+            std::lock_guard lock(mutex);
+            if (run_state == RunState::ShuttingDown) {
+                return ticket;
+            }
+            if (run_state == RunState::Accepting ||
+                outstanding_stop_all_tickets.empty()) {
+                run_state = RunState::PausedForStopAll;
+                ++admission_generation;
+                ++stop_all_generation;
+                stop_all_failed = false;
+            }
+            ticket.generation = stop_all_generation;
+            ticket.ticket_id = ++next_stop_all_ticket_id;
+            outstanding_stop_all_tickets.emplace(ticket.ticket_id);
+            cancelAllLocked(active_record);
+        }
+
+        if (delegate_active_stop) {
+            // finishStopAll(all_devices_stop_confirmed=true) is the external
+            // confirmation for the active Action device in this mode.
+            ticket.active_action_stop_confirmed = true;
+        } else {
+            try {
+                ticket.active_action_stop_confirmed =
+                    requestTypedStop(active_record);
+            } catch (const std::exception& error) {
+                CMVR_LOG(ERROR)
+                    << "[ActionQueueExecutor] StopAll typed stop threw, error="
+                    << error.what();
+            } catch (...) {
+                CMVR_LOG(ERROR)
+                    << "[ActionQueueExecutor] StopAll typed stop threw";
+            }
+        }
+        queue_condition.notify_all();
+        return ticket;
+    }
+
+    bool finishStopAll(
+        const ActionQueueExecutor::StopAllTicket& ticket,
+        const bool all_devices_stop_confirmed)
+    {
+        std::lock_guard lock(mutex);
+        if (run_state != RunState::PausedForStopAll ||
+            !ticket.valid() ||
+            ticket.generation != stop_all_generation ||
+            outstanding_stop_all_tickets.erase(ticket.ticket_id) == 0U) {
+            return false;
+        }
+
+        const bool caller_confirmed =
+            ticket.active_action_stop_confirmed &&
+            all_devices_stop_confirmed;
+        if (!caller_confirmed) {
+            stop_all_failed = true;
+        }
+        if (!outstanding_stop_all_tickets.empty()) {
+            return caller_confirmed;
+        }
+        if (stop_all_failed || !queue.empty() || active) {
+            stop_all_failed = true;
+            return false;
+        }
+
+        run_state = RunState::Accepting;
+        return true;
+    }
+
+    bool disableForShutdown()
     {
         std::shared_ptr<Record> active_record;
         {
             std::lock_guard lock(mutex);
-            accepting = false;
-            for (const auto& record : queue) {
-                record->cancel_requested.store(
-                    true, std::memory_order_release);
-                record->condition.notify_all();
-            }
-            active_record = active;
-            if (active_record) {
-                active_record->cancel_requested.store(
-                    true, std::memory_order_release);
-                active_record->condition.notify_all();
+            if (run_state != RunState::ShuttingDown) {
+                run_state = RunState::ShuttingDown;
+                ++admission_generation;
+                ++stop_all_generation;
+                outstanding_stop_all_tickets.clear();
+                stop_all_failed = true;
             }
+            cancelAllLocked(active_record);
+        }
+
+        bool stopped = false;
+        try {
+            stopped = requestTypedStop(active_record);
+        } catch (const std::exception& error) {
+            CMVR_LOG(ERROR)
+                << "[ActionQueueExecutor] shutdown typed stop threw, error="
+                << error.what();
+        } catch (...) {
+            CMVR_LOG(ERROR)
+                << "[ActionQueueExecutor] shutdown typed stop threw";
         }
-        const bool stopped = requestTypedStop(active_record);
         queue_condition.notify_all();
         return stopped;
     }
@@ -914,6 +999,37 @@ struct ActionQueueExecutor::Impl {
         }
     }
 
+    enum class FailClosedResult {
+        Quarantined,
+        AlreadyFenced,
+        LeaseRetained,
+    };
+
+    static FailClosedResult quarantineOrRetainLease(
+        const std::shared_ptr<Record>& record,
+        const control::ControlLeaseToken& expected_token) noexcept
+    {
+        auto& authority =
+            control::ControlAuthorityManager::instance();
+        if (authority.quarantineIfCurrent(expected_token)) {
+            // LeaseSet must release the displaced handler token when it exits;
+            // that release is what makes this quarantine recoverable.
+            return FailClosedResult::Quarantined;
+        }
+        try {
+            if (!authority.validate(expected_token)) {
+                return FailClosedResult::AlreadyFenced;
+            }
+        } catch (...) {
+        }
+
+        // No independently managed barrier could be confirmed. Retaining the
+        // exact Action lease is the last fail-closed fallback.
+        record->retain_control_leases.store(
+            true, std::memory_order_release);
+        return FailClosedResult::LeaseRetained;
+    }
+
     bool requestTypedStop(const std::shared_ptr<Record>& record)
     {
         if (!record) {
@@ -991,9 +1107,7 @@ struct ActionQueueExecutor::Impl {
                     *expected_token, owner, ttl);
             } catch (const std::exception& error) {
                 all_stopped = false;
-                (void)authority.quarantineIfCurrent(*expected_token);
-                record->retain_control_leases.store(
-                    true, std::memory_order_release);
+                (void)quarantineOrRetainLease(record, *expected_token);
                 CMVR_LOG(ERROR)
                     << "[ActionQueueExecutor] could not establish typed stop "
                        "barrier; control remains quarantined, id="
@@ -1001,9 +1115,7 @@ struct ActionQueueExecutor::Impl {
                 continue;
             } catch (...) {
                 all_stopped = false;
-                (void)authority.quarantineIfCurrent(*expected_token);
-                record->retain_control_leases.store(
-                    true, std::memory_order_release);
+                (void)quarantineOrRetainLease(record, *expected_token);
                 CMVR_LOG(ERROR)
                     << "[ActionQueueExecutor] could not establish typed stop "
                        "barrier; control remains quarantined, id="
@@ -1011,10 +1123,10 @@ struct ActionQueueExecutor::Impl {
                 continue;
             }
             if (!barrier.acquired) {
-                if (authority.quarantineIfCurrent(*expected_token)) {
+                const auto fail_closed =
+                    quarantineOrRetainLease(record, *expected_token);
+                if (fail_closed != FailClosedResult::AlreadyFenced) {
                     all_stopped = false;
-                    record->retain_control_leases.store(
-                        true, std::memory_order_release);
                     CMVR_LOG(ERROR)
                         << "[ActionQueueExecutor] exact typed stop barrier was "
                            "not established while the Action lease remained "
@@ -1079,6 +1191,12 @@ struct ActionQueueExecutor::Impl {
                 authority.release(barrier.token);
             } else {
                 all_stopped = false;
+                if (!authority.retireSafetyHolder(barrier.token)) {
+                    CMVR_LOG(ERROR)
+                        << "[ActionQueueExecutor] failed to retire an "
+                           "unconfirmed typed-stop barrier, id="
+                        << step.device_id;
+                }
                 CMVR_LOG(ERROR)
                     << "[ActionQueueExecutor] typed stop was not confirmed; "
                        "control remains quarantined, id="
@@ -1206,10 +1324,30 @@ struct ActionQueueExecutor::Impl {
         };
 
         bool handled = false;
+        std::uint64_t observed_admission_generation = 0U;
+        std::uint64_t observed_system_admission_generation = 0U;
         const bool initially_canceled = waiterCanceled(waiter_canceled);
         {
+            auto system_admission =
+                globalStopAllAdmissionGate().lockAdmission();
             std::lock_guard lock(mutex);
             handled = lookup_existing_locked();
+            if (!handled && !record) {
+                if (!system_admission.accepting() ||
+                    run_state != RunState::Accepting) {
+                    fillTerminalFeedback(
+                        feedback, request.action_id(),
+                        api::ACTION_RESULT_CODE_REJECTED, 0,
+                        run_state == RunState::ShuttingDown
+                            ? "ActionQueue is disabled because the service is shutting down"
+                            : "ActionQueue is temporarily paused by StopAll");
+                    handled = true;
+                } else {
+                    observed_admission_generation = admission_generation;
+                    observed_system_admission_generation =
+                        system_admission.generation();
+                }
+            }
         }
         if (handled) {
             return ActionQueueExecutor::WaitResult::Terminal;
@@ -1243,15 +1381,25 @@ struct ActionQueueExecutor::Impl {
             waiterCanceled(waiter_canceled);
         bool canceled_without_record = false;
         {
+            auto system_admission =
+                globalStopAllAdmissionGate().lockAdmission();
             std::lock_guard lock(mutex);
             handled = lookup_existing_locked();
             if (!handled && !record && canceled_before_admission) {
                 canceled_without_record = true;
-            } else if (!handled && !record && (!accepting || stopping)) {
+            } else if (!handled && !record &&
+                       (!system_admission.accepting() ||
+                        system_admission.generation() !=
+                            observed_system_admission_generation ||
+                        run_state != RunState::Accepting ||
+                        admission_generation !=
+                            observed_admission_generation)) {
                 fillTerminalFeedback(
                     feedback, request.action_id(),
                     api::ACTION_RESULT_CODE_REJECTED, 0,
-                    "ActionQueue is disabled by a system stop");
+                    run_state == RunState::ShuttingDown
+                        ? "ActionQueue is disabled because the service is shutting down"
+                        : "ActionQueue admission was interrupted by StopAll; retry after StopAll completes");
                 handled = true;
             } else if (!handled && !record &&
                        queue.size() + (active ? 1U : 0U) >=
@@ -1353,9 +1501,10 @@ struct ActionQueueExecutor::Impl {
             {
                 std::unique_lock lock(mutex);
                 queue_condition.wait(lock, [this]() {
-                    return stopping || !queue.empty();
+                    return run_state == RunState::ShuttingDown ||
+                        !queue.empty();
                 });
-                if (stopping && queue.empty()) {
+                if (run_state == RunState::ShuttingDown && queue.empty()) {
                     return;
                 }
                 record = queue.front();
@@ -1646,18 +1795,16 @@ struct ActionQueueExecutor::Impl {
             barrier = authority.preemptAcquireIfCurrent(
                 *expected_token, owner, ttl);
         } catch (...) {
-            (void)authority.quarantineIfCurrent(*expected_token);
-            record->retain_control_leases.store(
-                true, std::memory_order_release);
+            (void)quarantineOrRetainLease(record, *expected_token);
             throw;
         }
         if (!barrier.acquired) {
             // A direct Stop or another Action stop may already have converted
             // our lease. Do not join that barrier and, critically, do not
             // preempt a successor which acquired control after it completed.
-            if (authority.quarantineIfCurrent(*expected_token)) {
-                record->retain_control_leases.store(
-                    true, std::memory_order_release);
+            const auto fail_closed =
+                quarantineOrRetainLease(record, *expected_token);
+            if (fail_closed != FailClosedResult::AlreadyFenced) {
                 const std::string detail =
                     "could not establish timed-out RobotArm stop barrier "
                     "while the Action lease remained current: " +
@@ -1710,8 +1857,10 @@ struct ActionQueueExecutor::Impl {
         } else {
             // Deliberately retain the safety barrier when idle was not
             // confirmed. Releasing it would allow a new command to overlap an
-            // unknown physical outcome. Recovery requires an explicit device
-            // safety procedure or process restart.
+            // unknown physical outcome. Retiring the token keeps the barrier
+            // fail-closed while allowing a later confirmed System StopAll
+            // recovery round to clear it.
+            (void)authority.retireSafetyHolder(barrier.token);
             std::lock_guard lock(record->mutex);
             record->stop_error =
                 "RobotArm stop was not confirmed; control remains quarantined: " +
@@ -2156,8 +2305,12 @@ struct ActionQueueExecutor::Impl {
     std::deque<std::string> terminal_result_order;
     std::unordered_set<std::string> retired_action_ids;
     std::shared_ptr<Record> active;
-    bool accepting{true};
-    bool stopping{false};
+    RunState run_state{RunState::Accepting};
+    std::uint64_t admission_generation{1U};
+    std::uint64_t stop_all_generation{0U};
+    std::uint64_t next_stop_all_ticket_id{0U};
+    std::unordered_set<std::uint64_t> outstanding_stop_all_tickets;
+    bool stop_all_failed{false};
     bool joined{false};
     std::atomic<std::uint64_t> sequence{0};
     std::atomic<std::size_t> concurrent_submitters{0};
@@ -2185,9 +2338,23 @@ ActionQueueExecutor::WaitResult ActionQueueExecutor::submitAndWait(
     return result;
 }
 
-bool ActionQueueExecutor::cancelAllAndDisable()
+ActionQueueExecutor::StopAllTicket ActionQueueExecutor::beginStopAll(
+    const bool delegate_active_stop)
 {
-    return impl_->cancelAllAndDisable();
+    return impl_->beginStopAll(delegate_active_stop);
+}
+
+bool ActionQueueExecutor::finishStopAll(
+    const StopAllTicket& ticket,
+    const bool all_devices_stop_confirmed)
+{
+    return impl_->finishStopAll(
+        ticket, all_devices_stop_confirmed);
+}
+
+bool ActionQueueExecutor::disableForShutdown()
+{
+    return impl_->disableForShutdown();
 }
 
 bool ActionQueueExecutor::waitForIdle(

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

@@ -0,0 +1,108 @@
+#ifndef CMVR_ES_CAMERA_OPERATIONAL_ACTIVITY_REGISTRY_H
+#define CMVR_ES_CAMERA_OPERATIONAL_ACTIVITY_REGISTRY_H
+
+#include <atomic>
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "devices/camera/abstract_camera.h"
+
+namespace cmvr::service {
+
+// Tracks successful CameraService::StartCamera calls. StopAll uses this
+// registry to stop the corresponding operational pipelines without invoking
+// AbstractDevice::stop().
+class CameraOperationalActivityRegistry final {
+public:
+    enum class DispatchResult {
+        Success,
+        RejectedByStopAll,
+        DeviceFailure,
+    };
+
+    struct ActivityToken {
+        std::string device_id;
+        std::uint64_t activity_generation{0U};
+        bool owns_start{false};
+
+        bool valid() const noexcept
+        {
+            return !device_id.empty() && activity_generation != 0U;
+        }
+    };
+
+    DispatchResult start(
+        const std::string& device_id,
+        const std::shared_ptr<device::AbstractCamera>& camera,
+        ActivityToken* token = nullptr);
+
+    // Rolls back only the exact activity created by start(). A newer start for
+    // the same device is never stopped by an older request finishing late.
+    bool stopIfCurrent(const ActivityToken& token);
+
+    // Explicit StopCamera retains its legacy lifecycle behavior, but is
+    // serialized here so it cannot race an operational StopAll stop.
+    DispatchResult stopLifecycle(
+        const std::string& device_id,
+        const std::shared_ptr<device::AbstractCamera>& camera);
+
+    // Reconciles a lifecycle stop performed outside CameraService.
+    void markCameraStopped(const std::string& device_id);
+
+    // StopAll must close the process-wide admission gate first. Successful
+    // entries are removed. Failed entries remain quarantined for a later
+    // StopAll retry.
+    bool stopAllActivities(std::vector<std::string>* failures = nullptr);
+
+    // StopAll must close the process-wide admission gate first. Stops only the
+    // operational pipeline tracked for device_id and never invokes the camera
+    // lifecycle stop().
+    bool stopActivitiesForDevice(
+        const std::string& device_id,
+        std::vector<std::string>* failures = nullptr);
+
+    // If no StartCamera activity is tracked, StopAll can still quiesce the
+    // camera currently present in DeviceManager's inventory. A tracked camera
+    // takes precedence over the fallback. Repeated calls in one StopAll round
+    // do not stop the same instance twice.
+    bool stopActivitiesForDevice(
+        const std::string& device_id,
+        const std::shared_ptr<device::AbstractCamera>& fallback_camera,
+        std::vector<std::string>* failures = nullptr);
+
+    std::size_t activeCameraCount() const;
+    // Does not wait for per-device driver I/O. This conservative snapshot
+    // includes devices with an admitted or historical dispatch state, allowing
+    // StopAll to cover activity absent from the DeviceManager snapshot.
+    std::vector<std::string> trackedDeviceIds() const;
+    std::vector<std::string> activeDeviceIds() const;
+    void clearForTesting();
+
+private:
+    struct DeviceState {
+        mutable std::mutex mutex;
+        std::shared_ptr<device::AbstractCamera> active_camera;
+        std::weak_ptr<device::AbstractCamera> last_stopped_camera;
+        std::uint64_t last_stopped_generation{0U};
+        std::uint64_t activity_generation{0U};
+        std::atomic<bool> active{false};
+    };
+
+    std::shared_ptr<DeviceState> stateForDevice(
+        const std::string& device_id,
+        bool create);
+
+    mutable std::mutex states_mutex_;
+    std::unordered_map<std::string, std::shared_ptr<DeviceState>> states_;
+};
+
+CameraOperationalActivityRegistry&
+globalCameraOperationalActivityRegistry();
+
+} // namespace cmvr::service
+
+#endif // CMVR_ES_CAMERA_OPERATIONAL_ACTIVITY_REGISTRY_H

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

@@ -0,0 +1,91 @@
+#ifndef CMVR_ES_CAMERA_PTZ_ACTIVITY_REGISTRY_H
+#define CMVR_ES_CAMERA_PTZ_ACTIVITY_REGISTRY_H
+
+#include <atomic>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "devices/camera/abstract_camera.h"
+
+namespace cmvr::service {
+
+// Tracks PTZ commands whose START has not yet been paired with a successful
+// STOP. The registry is an operational control boundary; it never invokes a
+// camera lifecycle method.
+class CameraPtzActivityRegistry final {
+public:
+    enum class DispatchResult {
+        Success,
+        RejectedByStopAll,
+        DeviceFailure,
+    };
+
+    DispatchResult control(
+        const std::string& device_id,
+        const std::shared_ptr<device::AbstractCamera>& camera,
+        device::PtzCommand command,
+        bool stop,
+        int speed);
+
+    // Reconciles externally stopped camera PTZ state with this registry. A
+    // camera backend can call this if it stops PTZ outside CameraService.
+    void markCameraStopped(const std::string& device_id);
+
+    // StopAll must close the process-wide admission gate before calling this.
+    // A true result means every tracked START received a successful matching
+    // STOP. Failed entries are retained so a later StopAll can retry them.
+    bool stopAllActivities(std::vector<std::string>* failures = nullptr);
+
+    // StopAll must close the process-wide admission gate first. Stops only PTZ
+    // commands tracked for device_id. Calls for different physical cameras may
+    // execute concurrently; calls for one camera remain ordered with control().
+    bool stopActivitiesForDevice(
+        const std::string& device_id,
+        std::vector<std::string>* failures = nullptr);
+
+    std::size_t activeCommandCount() const;
+    // Does not wait for per-device driver I/O. This conservative snapshot
+    // includes devices with an admitted or historical dispatch state, allowing
+    // StopAll to cover activity absent from the DeviceManager snapshot.
+    std::vector<std::string> trackedDeviceIds() const;
+    std::vector<std::string> activeDeviceIds() const;
+    void clearForTesting();
+
+private:
+    struct PtzCommandHash {
+        std::size_t operator()(device::PtzCommand command) const noexcept
+        {
+            return static_cast<std::size_t>(command);
+        }
+    };
+
+    struct ActiveCommand {
+        std::shared_ptr<device::AbstractCamera> camera;
+        int speed{0};
+    };
+
+    using CameraCommands = std::unordered_map<
+        device::PtzCommand, ActiveCommand, PtzCommandHash>;
+
+    struct DeviceState {
+        mutable std::mutex mutex;
+        CameraCommands commands;
+        std::atomic<std::size_t> active_command_count{0U};
+    };
+
+    std::shared_ptr<DeviceState> stateForDevice(
+        const std::string& device_id,
+        bool create);
+
+    mutable std::mutex states_mutex_;
+    std::unordered_map<std::string, std::shared_ptr<DeviceState>> states_;
+};
+
+CameraPtzActivityRegistry& globalCameraPtzActivityRegistry();
+
+} // namespace cmvr::service
+
+#endif // CMVR_ES_CAMERA_PTZ_ACTIVITY_REGISTRY_H

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

@@ -0,0 +1,37 @@
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include <grpcpp/support/server_interceptor.h>
+
+namespace cmvr::service {
+
+enum class GrpcFailureKind {
+    APPLICATION,
+    GRPC_STATUS,
+    MALFORMED_RESPONSE,
+};
+
+enum class GrpcFailureSeverity {
+    INFO,
+    WARNING,
+    ERROR,
+};
+
+struct GrpcFailureRecord {
+    GrpcFailureKind kind{GrpcFailureKind::APPLICATION};
+    std::string method;
+    std::string peer;
+    grpc::StatusCode status_code{grpc::StatusCode::OK};
+    GrpcFailureSeverity severity{GrpcFailureSeverity::ERROR};
+    std::string detail;
+};
+
+using GrpcFailureSink = std::function<void(const GrpcFailureRecord&)>;
+
+std::unique_ptr<grpc::experimental::ServerInterceptorFactoryInterface>
+makeGrpcErrorLoggingInterceptorFactory(GrpcFailureSink sink = {});
+
+} // namespace cmvr::service

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

@@ -10,6 +10,7 @@
 
 #include "cmvr/api/motor_service.grpc.pb.h"
 #include "devices/motor/abstract_motor.h"
+#include "service/grpc/include/motor_activity_coordinator.h"
 
 namespace cmvr::device {
 class DeviceManager;
@@ -89,9 +90,15 @@ private:
         std::shared_ptr<MotorControlState> control;
     };
 
+    enum class ResolveAccess {
+        Control,
+        Observe,
+    };
+
     struct MotorControlEntry {
         std::weak_ptr<device::AbstractMotor> owner;
         std::shared_ptr<MotorControlState> state;
+        MotorActivityCoordinator::Registration stop_all_registration;
     };
 
     class ControlLease {
@@ -111,9 +118,12 @@ private:
     };
 
     grpc::Status resolveMotor(const api::MotorTarget& target,
-                              ResolvedMotor& resolved) const;
+                              ResolvedMotor& resolved,
+                              ResolveAccess access = ResolveAccess::Control) const;
     std::shared_ptr<MotorControlState> stateFor(
         const std::shared_ptr<device::AbstractMotor>& motor) const;
+    std::shared_ptr<MotorControlState> existingStateFor(
+        const std::shared_ptr<device::AbstractMotor>& motor) const;
     std::unique_ptr<ControlLease> acquireControl(
         const ResolvedMotor& resolved,
         api::MotorControlType control,

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

@@ -14,6 +14,7 @@
 namespace cmvr::service
 {
     class ActionQueueExecutor;
+    class StopOperationDispatcher;
 
     class gRPCSystemServiceImpl: public api::SystemService::Service {
     public:
@@ -31,6 +32,10 @@ namespace cmvr::service
         grpc::Status ExecuteActionQueue(grpc::ServerContext* context, const cmvr::api::ActionQueueCommand_Request* request, cmvr::api::ActionQueueCommand_Feedback* response) override;
     private:
         device::DeviceManager& dmgr_;
+        // Outlives ActionQueueExecutor and every StopAll RPC stack. A stop
+        // backend which ignores the shared deadline can therefore finish in
+        // its owned worker without accessing destroyed RPC-local state.
+        std::unique_ptr<StopOperationDispatcher> stop_dispatcher_;
         std::unique_ptr<ActionQueueExecutor> action_queue_;
     };
 }

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

@@ -0,0 +1,119 @@
+#ifndef CMVR_ES_MEDIA_ACTIVITY_COORDINATOR_H
+#define CMVR_ES_MEDIA_ACTIVITY_COORDINATOR_H
+
+#include <chrono>
+#include <cstdint>
+#include <functional>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "service/stop_all/include/deferred_stop_operation.h"
+
+namespace cmvr::service {
+
+// Coordinates in-process media RPC activity with SystemService::StopAll.
+// StopAll invalidates the current generation and waits for the affected RPCs
+// to release their own device leases; it does not stop device lifecycles.
+class MediaActivityCoordinator final {
+private:
+    struct Impl;
+    struct SessionState;
+
+public:
+    using CancelCallback = std::function<void()>;
+
+    struct StopAllTicket {
+        std::uint64_t generation{0};
+        std::uint64_t ticket_id{0};
+
+        bool valid() const noexcept
+        {
+            return generation != 0U && ticket_id != 0U;
+        }
+    };
+
+    class Session final {
+    public:
+        Session() = default;
+        ~Session();
+
+        Session(Session&& other) noexcept;
+        Session& operator=(Session&& other) noexcept;
+
+        Session(const Session&) = delete;
+        Session& operator=(const Session&) = delete;
+
+        explicit operator bool() const noexcept;
+        bool cancelled() const noexcept;
+
+        // Linearizes a short device operation against beginStopAll(). If this
+        // returns false, StopAll won the race and the operation was not run.
+        bool runIfCurrent(const std::function<void()>& operation) const;
+
+        // Claims an optional process-wide resource for this session. This is
+        // used by speaker input because one device cannot safely have two RPCs
+        // feeding and independently stopping the same streaming pipeline.
+        bool claimExclusiveResource(const std::string& resource_key);
+
+        void reset() noexcept;
+
+    private:
+        friend class MediaActivityCoordinator;
+        Session(
+            std::shared_ptr<Impl> impl,
+            std::shared_ptr<SessionState> state);
+
+        std::shared_ptr<Impl> impl_;
+        std::shared_ptr<SessionState> state_;
+    };
+
+    MediaActivityCoordinator();
+    ~MediaActivityCoordinator() = default;
+
+    MediaActivityCoordinator(const MediaActivityCoordinator&) = delete;
+    MediaActivityCoordinator& operator=(const MediaActivityCoordinator&) = delete;
+
+    // Returns an invalid session while a StopAll round is in progress.
+    Session beginSession(CancelCallback cancel = {});
+
+    // Pauses new sessions and invalidates all sessions from the previous
+    // generation. Cancellation callbacks are normally invoked before this
+    // returns. SystemService defers them until whole-machine motion stop
+    // requests have been issued, so a callback cannot delay physical stops.
+    // Concurrent callers join the same StopAll round.
+    StopAllTicket beginStopAll(bool defer_cancellation = false);
+
+    // Collects one independently executable, at-most-once cancellation per
+    // invalidated session. Each operation captures SessionState ownership and
+    // can safely outlive this coordinator object without capturing `this`.
+    bool collectCancellationOperations(
+        const StopAllTicket& ticket,
+        std::vector<DeferredStopOperation>& operations,
+        std::string* error = nullptr) const;
+
+    // Legacy synchronous wrapper which serially executes the operations above.
+    // Returns false for a stale ticket or when any callback throws.
+    bool requestCancellation(const StopAllTicket& ticket);
+
+    // Waits until every session invalidated by this ticket has run its cleanup
+    // and unregistered. A timeout leaves admission paused (fail closed).
+    bool waitForStopped(
+        const StopAllTicket& ticket,
+        std::chrono::milliseconds timeout);
+
+    // Completes one StopAll participant. Admission resumes only after every
+    // participant succeeds and all invalidated sessions have exited.
+    bool finishStopAll(
+        const StopAllTicket& ticket,
+        bool all_media_stopped);
+
+private:
+    std::shared_ptr<Impl> impl_;
+};
+
+MediaActivityCoordinator& globalMediaActivityCoordinator();
+
+} // namespace cmvr::service
+
+#endif // CMVR_ES_MEDIA_ACTIVITY_COORDINATOR_H

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

@@ -0,0 +1,149 @@
+#ifndef CMVR_ES_MOTOR_ACTIVITY_COORDINATOR_H
+#define CMVR_ES_MOTOR_ACTIVITY_COORDINATOR_H
+
+#include <chrono>
+#include <cstdint>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include "service/stop_all/include/deferred_stop_operation.h"
+
+namespace cmvr::service {
+
+// Coordinates MotorService command dispatch with SystemService::StopAll.
+// Registered controls expose only operational cancellation and quick-stop;
+// this coordinator never invokes a MotorManager or device lifecycle method.
+class MotorActivityCoordinator final {
+private:
+    struct Impl;
+
+public:
+    using CancelCallback = std::function<void()>;
+    using QuickStopCallback = std::function<bool()>;
+    using IdleCallback = std::function<bool()>;
+
+    struct StopAllTicket {
+        std::uint64_t generation{0};
+        std::uint64_t ticket_id{0};
+
+        bool valid() const noexcept
+        {
+            return generation != 0U && ticket_id != 0U;
+        }
+    };
+
+    class Registration final {
+    public:
+        Registration() = default;
+        ~Registration();
+
+        Registration(Registration&& other) noexcept;
+        Registration& operator=(Registration&& other) noexcept;
+
+        Registration(const Registration&) = delete;
+        Registration& operator=(const Registration&) = delete;
+
+        explicit operator bool() const noexcept;
+        void reset() noexcept;
+
+    private:
+        friend class MotorActivityCoordinator;
+        Registration(std::shared_ptr<Impl> impl, std::uint64_t id) noexcept;
+
+        std::shared_ptr<Impl> impl_;
+        std::uint64_t id_{0};
+    };
+
+    class AdmissionGuard final {
+    public:
+        AdmissionGuard(AdmissionGuard&&) noexcept = default;
+        AdmissionGuard& operator=(AdmissionGuard&&) noexcept = default;
+
+        AdmissionGuard(const AdmissionGuard&) = delete;
+        AdmissionGuard& operator=(const AdmissionGuard&) = delete;
+
+        bool accepting() const noexcept { return accepting_; }
+
+    private:
+        friend class MotorActivityCoordinator;
+        AdmissionGuard(
+            std::unique_lock<std::mutex>&& lock,
+            bool accepting) noexcept;
+
+        std::unique_lock<std::mutex> lock_;
+        bool accepting_{false};
+    };
+
+    MotorActivityCoordinator();
+    ~MotorActivityCoordinator() = default;
+
+    MotorActivityCoordinator(const MotorActivityCoordinator&) = delete;
+    MotorActivityCoordinator& operator=(const MotorActivityCoordinator&) = delete;
+
+    Registration registerControl(
+        CancelCallback cancel,
+        QuickStopCallback quick_stop,
+        IdleCallback idle,
+        std::string description = {});
+
+    // Hold this guard until the MotorControlState has been marked busy. This
+    // makes final command admission atomic with beginStopAll().
+    AdmissionGuard lockAdmission();
+
+    // Invalidates admission for every command from the preceding generation.
+    // With defer_callbacks=false, cancellation callbacks retain their legacy
+    // synchronous behavior. With true, callers must collect and execute every
+    // target operation; each operation orders cancellation before quick-stop.
+    StopAllTicket beginStopAll(bool defer_callbacks = false);
+
+    // Collects one independently executable operation per registration that
+    // belonged to this StopAll round. Operations capture shared state rather
+    // than this coordinator and are idempotent, including their failure result.
+    bool collectStopOperations(
+        const StopAllTicket& ticket,
+        std::vector<DeferredStopOperation>& operations,
+        std::string* error = nullptr) const;
+
+    // Legacy synchronous wrapper which serially executes the operations above.
+    bool requestStop(
+        const StopAllTicket& ticket,
+        std::string* error = nullptr);
+
+    // Waits for in-flight RPC/stream ownership captured by the round to be
+    // released after requestStop().
+    bool waitForStopped(
+        const StopAllTicket& ticket,
+        std::chrono::milliseconds timeout,
+        std::string* error = nullptr);
+
+    // Convenience operation for callers that do not need split-phase stop.
+    bool stopAndWait(
+        const StopAllTicket& ticket,
+        std::chrono::milliseconds timeout,
+        std::string* error = nullptr);
+
+    // Admission resumes only when all registered controls confirmed their stop
+    // and every invalidated RPC/stream has exited. Failure remains fail-closed.
+    bool finishStopAll(
+        const StopAllTicket& ticket,
+        bool all_motors_stopped);
+
+    // Wakes StopAll after a MotorControlState releases or changes ownership.
+    void notifyStateChanged() noexcept;
+
+    // Test/process teardown hook. Runtime recovery must use another successful
+    // StopAll round instead of bypassing fail-closed state.
+    void clearForTesting() noexcept;
+
+private:
+    std::shared_ptr<Impl> impl_;
+};
+
+MotorActivityCoordinator& globalMotorActivityCoordinator();
+
+} // namespace cmvr::service
+
+#endif // CMVR_ES_MOTOR_ACTIVITY_COORDINATOR_H

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

@@ -0,0 +1,328 @@
+#include "service/grpc/include/camera_operational_activity_registry.h"
+
+#include <algorithm>
+#include <exception>
+
+#include "service/stop_all/include/stop_all_admission_gate.h"
+
+namespace cmvr::service {
+namespace {
+
+template <typename Operation>
+CameraOperationalActivityRegistry::DispatchResult dispatchIfAdmitted(
+    std::mutex& device_mutex,
+    Operation&& operation)
+{
+    std::uint64_t admitted_generation = 0U;
+    {
+        auto admission = globalStopAllAdmissionGate().lockAdmission();
+        if (!admission.accepting()) {
+            return CameraOperationalActivityRegistry::DispatchResult::
+                RejectedByStopAll;
+        }
+        admitted_generation = admission.generation();
+    }
+
+    std::lock_guard dispatch_lock(device_mutex);
+    {
+        auto admission = globalStopAllAdmissionGate().lockAdmission();
+        if (!admission.accepting() ||
+            admission.generation() != admitted_generation) {
+            return CameraOperationalActivityRegistry::DispatchResult::
+                RejectedByStopAll;
+        }
+    }
+
+    return operation();
+}
+
+} // namespace
+
+std::shared_ptr<CameraOperationalActivityRegistry::DeviceState>
+CameraOperationalActivityRegistry::stateForDevice(
+    const std::string& device_id,
+    const bool create)
+{
+    std::lock_guard lock(states_mutex_);
+    const auto existing = states_.find(device_id);
+    if (existing != states_.end()) {
+        return existing->second;
+    }
+    if (!create) {
+        return {};
+    }
+
+    auto state = std::make_shared<DeviceState>();
+    states_.emplace(device_id, state);
+    return state;
+}
+
+CameraOperationalActivityRegistry::DispatchResult
+CameraOperationalActivityRegistry::start(
+    const std::string& device_id,
+    const std::shared_ptr<device::AbstractCamera>& camera,
+    ActivityToken* token)
+{
+    if (token) {
+        *token = {};
+    }
+    if (device_id.empty() || !camera) {
+        return DispatchResult::DeviceFailure;
+    }
+
+    const auto state = stateForDevice(device_id, true);
+    return dispatchIfAdmitted(state->mutex, [&] {
+        const auto previous_camera = state->active_camera;
+        const bool was_active = state->active.load(std::memory_order_acquire);
+        if (was_active && previous_camera != camera) {
+            // A device id has one operational owner at a time. Replacing an
+            // active instance would make a token from either instance unable
+            // to roll back without risking the other camera.
+            return DispatchResult::DeviceFailure;
+        }
+        if (!previous_camera) {
+            // Allocate tracking before device I/O so a successful start always
+            // has a StopAll-visible owner.
+            state->active_camera = camera;
+        }
+
+        bool started = false;
+        try {
+            started = camera->startOperationalActivity();
+        } catch (...) {
+            state->active_camera = previous_camera;
+            state->active.store(was_active, std::memory_order_release);
+            throw;
+        }
+        if (!started) {
+            state->active_camera = previous_camera;
+            state->active.store(was_active, std::memory_order_release);
+            return DispatchResult::DeviceFailure;
+        }
+
+        state->active_camera = camera;
+        state->active.store(true, std::memory_order_release);
+        ++state->activity_generation;
+        if (state->activity_generation == 0U) {
+            ++state->activity_generation;
+        }
+        if (token) {
+            token->device_id = device_id;
+            token->activity_generation = state->activity_generation;
+            token->owns_start = !was_active;
+        }
+        return DispatchResult::Success;
+    });
+}
+
+bool CameraOperationalActivityRegistry::stopIfCurrent(
+    const ActivityToken& token)
+{
+    if (!token.valid() || !token.owns_start) {
+        return true;
+    }
+
+    const auto state = stateForDevice(token.device_id, false);
+    if (!state) {
+        return true;
+    }
+
+    std::lock_guard dispatch_lock(state->mutex);
+    if (!state->active.load(std::memory_order_acquire) ||
+        state->activity_generation != token.activity_generation ||
+        !state->active_camera) {
+        return true;
+    }
+
+    bool stopped = false;
+    try {
+        stopped = state->active_camera->stopOperationalActivity();
+    } catch (...) {
+        stopped = false;
+    }
+    if (!stopped) {
+        return false;
+    }
+
+    state->last_stopped_camera = state->active_camera;
+    state->active_camera.reset();
+    state->active.store(false, std::memory_order_release);
+    return true;
+}
+
+CameraOperationalActivityRegistry::DispatchResult
+CameraOperationalActivityRegistry::stopLifecycle(
+    const std::string& device_id,
+    const std::shared_ptr<device::AbstractCamera>& camera)
+{
+    if (device_id.empty() || !camera) {
+        return DispatchResult::DeviceFailure;
+    }
+
+    const auto state = stateForDevice(device_id, true);
+    return dispatchIfAdmitted(state->mutex, [&] {
+        if (!camera->stop()) {
+            return DispatchResult::DeviceFailure;
+        }
+        state->active_camera.reset();
+        state->active.store(false, std::memory_order_release);
+        return DispatchResult::Success;
+    });
+}
+
+void CameraOperationalActivityRegistry::markCameraStopped(
+    const std::string& device_id)
+{
+    const auto state = stateForDevice(device_id, false);
+    if (!state) {
+        return;
+    }
+    std::lock_guard lock(state->mutex);
+    state->active_camera.reset();
+    state->active.store(false, std::memory_order_release);
+}
+
+bool CameraOperationalActivityRegistry::stopActivitiesForDevice(
+    const std::string& device_id,
+    std::vector<std::string>* failures)
+{
+    return stopActivitiesForDevice(device_id, {}, failures);
+}
+
+bool CameraOperationalActivityRegistry::stopActivitiesForDevice(
+    const std::string& device_id,
+    const std::shared_ptr<device::AbstractCamera>& fallback_camera,
+    std::vector<std::string>* failures)
+{
+    const auto state = stateForDevice(
+        device_id, static_cast<bool>(fallback_camera));
+    if (!state) {
+        return true;
+    }
+
+    std::lock_guard dispatch_lock(state->mutex);
+    const auto active_camera = state->active_camera;
+    const auto camera = active_camera ? active_camera : fallback_camera;
+    if (!camera) {
+        return true;
+    }
+
+    std::uint64_t stop_generation = 0U;
+    {
+        const auto admission = globalStopAllAdmissionGate().lockAdmission();
+        stop_generation = admission.generation();
+    }
+    if (!active_camera &&
+        state->last_stopped_generation == stop_generation &&
+        state->last_stopped_camera.lock() == camera) {
+        return true;
+    }
+
+    bool stopped = false;
+    std::string detail;
+    try {
+        stopped = camera->stopOperationalActivity();
+        if (!stopped) {
+            detail = "operational camera stop was not confirmed";
+        }
+    } catch (const std::exception& error) {
+        detail = std::string("operational camera stop threw: ") + error.what();
+    } catch (...) {
+        detail = "operational camera stop threw an unknown exception";
+    }
+
+    if (stopped) {
+        if (active_camera) {
+            state->active_camera.reset();
+        }
+        state->last_stopped_camera = camera;
+        state->last_stopped_generation = stop_generation;
+        state->active.store(false, std::memory_order_release);
+        return true;
+    }
+    if (failures) {
+        failures->push_back(device_id + ": " + detail);
+    }
+    return false;
+}
+
+bool CameraOperationalActivityRegistry::stopAllActivities(
+    std::vector<std::string>* failures)
+{
+    std::vector<std::string> device_ids;
+    {
+        std::lock_guard lock(states_mutex_);
+        device_ids.reserve(states_.size());
+        for (const auto& [device_id, state] : states_) {
+            (void)state;
+            device_ids.push_back(device_id);
+        }
+    }
+
+    bool all_stopped = true;
+    for (const auto& device_id : device_ids) {
+        if (!stopActivitiesForDevice(device_id, failures)) {
+            all_stopped = false;
+        }
+    }
+    return all_stopped;
+}
+
+std::size_t CameraOperationalActivityRegistry::activeCameraCount() const
+{
+    return activeDeviceIds().size();
+}
+
+std::vector<std::string>
+CameraOperationalActivityRegistry::trackedDeviceIds() const
+{
+    std::vector<std::string> device_ids;
+    {
+        std::lock_guard lock(states_mutex_);
+        device_ids.reserve(states_.size());
+        for (const auto& [device_id, state] : states_) {
+            (void)state;
+            device_ids.push_back(device_id);
+        }
+    }
+    std::sort(device_ids.begin(), device_ids.end());
+    return device_ids;
+}
+
+std::vector<std::string>
+CameraOperationalActivityRegistry::activeDeviceIds() const
+{
+    std::vector<std::pair<std::string, std::shared_ptr<DeviceState>>> states;
+    {
+        std::lock_guard lock(states_mutex_);
+        states.reserve(states_.size());
+        for (const auto& entry : states_) {
+            states.push_back(entry);
+        }
+    }
+
+    std::vector<std::string> device_ids;
+    device_ids.reserve(states.size());
+    for (const auto& [device_id, state] : states) {
+        if (state->active.load(std::memory_order_acquire)) {
+            device_ids.push_back(device_id);
+        }
+    }
+    std::sort(device_ids.begin(), device_ids.end());
+    return device_ids;
+}
+
+void CameraOperationalActivityRegistry::clearForTesting()
+{
+    std::lock_guard lock(states_mutex_);
+    states_.clear();
+}
+
+CameraOperationalActivityRegistry&
+globalCameraOperationalActivityRegistry()
+{
+    static CameraOperationalActivityRegistry registry;
+    return registry;
+}
+
+} // namespace cmvr::service

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

@@ -0,0 +1,224 @@
+#include "service/grpc/include/camera_ptz_activity_registry.h"
+
+#include <algorithm>
+#include <exception>
+#include <utility>
+
+#include "service/stop_all/include/stop_all_admission_gate.h"
+
+namespace cmvr::service {
+
+std::shared_ptr<CameraPtzActivityRegistry::DeviceState>
+CameraPtzActivityRegistry::stateForDevice(
+    const std::string& device_id,
+    const bool create)
+{
+    std::lock_guard lock(states_mutex_);
+    const auto existing = states_.find(device_id);
+    if (existing != states_.end()) {
+        return existing->second;
+    }
+    if (!create) {
+        return {};
+    }
+
+    auto state = std::make_shared<DeviceState>();
+    states_.emplace(device_id, state);
+    return state;
+}
+
+CameraPtzActivityRegistry::DispatchResult
+CameraPtzActivityRegistry::control(
+    const std::string& device_id,
+    const std::shared_ptr<device::AbstractCamera>& camera,
+    const device::PtzCommand command,
+    const bool stop,
+    const int speed)
+{
+    if (device_id.empty() || !camera) {
+        return DispatchResult::DeviceFailure;
+    }
+
+    // Check admission on both sides of the per-device dispatch queue. A
+    // command admitted before StopAll but still queued is rejected; one already
+    // in device I/O is completed before that device's stop begins.
+    std::uint64_t admitted_generation = 0U;
+    {
+        auto admission = globalStopAllAdmissionGate().lockAdmission();
+        if (!admission.accepting()) {
+            return DispatchResult::RejectedByStopAll;
+        }
+        admitted_generation = admission.generation();
+    }
+
+    const auto state = stateForDevice(device_id, true);
+    std::lock_guard dispatch_lock(state->mutex);
+    {
+        auto admission = globalStopAllAdmissionGate().lockAdmission();
+        if (!admission.accepting() ||
+            admission.generation() != admitted_generation) {
+            return DispatchResult::RejectedByStopAll;
+        }
+    }
+
+    if (!camera->controlPtz(command, stop, speed)) {
+        return DispatchResult::DeviceFailure;
+    }
+
+    if (!stop) {
+        state->commands[command] = {camera, speed};
+    } else {
+        state->commands.erase(command);
+    }
+    state->active_command_count.store(
+        state->commands.size(), std::memory_order_release);
+    return DispatchResult::Success;
+}
+
+void CameraPtzActivityRegistry::markCameraStopped(
+    const std::string& device_id)
+{
+    const auto state = stateForDevice(device_id, false);
+    if (!state) {
+        return;
+    }
+    std::lock_guard lock(state->mutex);
+    state->commands.clear();
+    state->active_command_count.store(0U, std::memory_order_release);
+}
+
+bool CameraPtzActivityRegistry::stopActivitiesForDevice(
+    const std::string& device_id,
+    std::vector<std::string>* failures)
+{
+    const auto state = stateForDevice(device_id, false);
+    if (!state) {
+        return true;
+    }
+
+    std::lock_guard dispatch_lock(state->mutex);
+    bool all_stopped = true;
+    for (auto command_it = state->commands.begin();
+         command_it != state->commands.end();) {
+        bool stopped = false;
+        std::string detail;
+        try {
+            const auto& activity = command_it->second;
+            stopped = activity.camera && activity.camera->controlPtz(
+                command_it->first, true, activity.speed);
+            if (!stopped) {
+                detail = "PTZ stop was rejected by the camera";
+            }
+        } catch (const std::exception& error) {
+            detail = std::string("PTZ stop threw: ") + error.what();
+        } catch (...) {
+            detail = "PTZ stop threw an unknown exception";
+        }
+
+        if (stopped) {
+            command_it = state->commands.erase(command_it);
+            continue;
+        }
+
+        all_stopped = false;
+        if (failures) {
+            failures->push_back(device_id + ": " + detail);
+        }
+        ++command_it;
+    }
+    state->active_command_count.store(
+        state->commands.size(), std::memory_order_release);
+    return all_stopped;
+}
+
+bool CameraPtzActivityRegistry::stopAllActivities(
+    std::vector<std::string>* failures)
+{
+    std::vector<std::string> device_ids;
+    {
+        std::lock_guard lock(states_mutex_);
+        device_ids.reserve(states_.size());
+        for (const auto& [device_id, state] : states_) {
+            (void)state;
+            device_ids.push_back(device_id);
+        }
+    }
+
+    bool all_stopped = true;
+    for (const auto& device_id : device_ids) {
+        if (!stopActivitiesForDevice(device_id, failures)) {
+            all_stopped = false;
+        }
+    }
+    return all_stopped;
+}
+
+std::size_t CameraPtzActivityRegistry::activeCommandCount() const
+{
+    std::vector<std::shared_ptr<DeviceState>> states;
+    {
+        std::lock_guard lock(states_mutex_);
+        states.reserve(states_.size());
+        for (const auto& [device_id, state] : states_) {
+            (void)device_id;
+            states.push_back(state);
+        }
+    }
+
+    std::size_t count = 0U;
+    for (const auto& state : states) {
+        count += state->active_command_count.load(std::memory_order_acquire);
+    }
+    return count;
+}
+
+std::vector<std::string> CameraPtzActivityRegistry::trackedDeviceIds() const
+{
+    std::vector<std::string> device_ids;
+    {
+        std::lock_guard lock(states_mutex_);
+        device_ids.reserve(states_.size());
+        for (const auto& [device_id, state] : states_) {
+            (void)state;
+            device_ids.push_back(device_id);
+        }
+    }
+    std::sort(device_ids.begin(), device_ids.end());
+    return device_ids;
+}
+
+std::vector<std::string> CameraPtzActivityRegistry::activeDeviceIds() const
+{
+    std::vector<std::pair<std::string, std::shared_ptr<DeviceState>>> states;
+    {
+        std::lock_guard lock(states_mutex_);
+        states.reserve(states_.size());
+        for (const auto& entry : states_) {
+            states.push_back(entry);
+        }
+    }
+
+    std::vector<std::string> device_ids;
+    device_ids.reserve(states.size());
+    for (const auto& [device_id, state] : states) {
+        if (state->active_command_count.load(std::memory_order_acquire) != 0U) {
+            device_ids.push_back(device_id);
+        }
+    }
+    std::sort(device_ids.begin(), device_ids.end());
+    return device_ids;
+}
+
+void CameraPtzActivityRegistry::clearForTesting()
+{
+    std::lock_guard lock(states_mutex_);
+    states_.clear();
+}
+
+CameraPtzActivityRegistry& globalCameraPtzActivityRegistry()
+{
+    static CameraPtzActivityRegistry registry;
+    return registry;
+}
+
+} // namespace cmvr::service

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

@@ -12,6 +12,7 @@
 
 #include "common/base/logging/logger.h"
 #include "manager/control_authority/include/control_authority_manager.h"
+#include "service/stop_all/include/stop_all_admission_gate.h"
 
 using google::protobuf::util::TimeUtil;
 
@@ -108,6 +109,24 @@ grpc::Status setControlLeaseConflict(
         response->mutable_header(), device_id, detail);
 }
 
+grpc::Status setStopAllRejected(
+    api::CommandHeader_Feedback* response,
+    const std::string& device_id)
+{
+    const std::string message =
+        "AGV control is temporarily paused by StopAll: " + device_id;
+    fillFeedback(response, false, message);
+    return grpc::Status(grpc::StatusCode::UNAVAILABLE, message);
+}
+
+template <typename Response>
+grpc::Status setStopAllRejected(
+    Response* response,
+    const std::string& device_id)
+{
+    return setStopAllRejected(response->mutable_header(), device_id);
+}
+
 class ScopedUnaryAgvControlLease final {
 public:
     ScopedUnaryAgvControlLease(
@@ -129,27 +148,93 @@ public:
         const auto ttl = std::chrono::duration_cast<
             control::ControlAuthorityManager::Duration>(
                 std::chrono::hours(24));
-        auto acquired = preemptive
-            ? manager_.preemptAcquire(device_id, owner, ttl)
-            : manager_.tryAcquire(device_id, owner, ttl);
+        control::ControlAcquireResult acquired;
+        if (preemptive) {
+            acquired = manager_.preemptAcquire(device_id, owner, ttl);
+        } else {
+            auto admission =
+                globalStopAllAdmissionGate().lockAdmission();
+            if (!admission.accepting()) {
+                rejected_by_stop_all_ = true;
+                detail_ = "System StopAll admission is closed";
+                return;
+            }
+            admission_generation_ = admission.generation();
+            acquired = manager_.tryAcquire(device_id, owner, ttl);
+        }
         acquired_ = acquired.acquired;
         token_ = std::move(acquired.token);
         detail_ = std::move(acquired.detail);
+        release_on_destroy_ = !preemptive;
     }
 
     ~ScopedUnaryAgvControlLease()
     {
         if (release_on_destroy_) {
             manager_.release(token_);
+        } else if (acquired_) {
+            (void)manager_.retireSafetyHolder(token_);
         }
     }
 
     bool acquired() const noexcept { return acquired_; }
+    bool rejectedByStopAll() const noexcept
+    {
+        return rejected_by_stop_all_;
+    }
     const std::string& detail() const noexcept { return detail_; }
 
-    // Unknown physical outcomes stay fail-closed until an explicit device
-    // safety procedure or process restart clears the retained holder.
-    void quarantine() noexcept { release_on_destroy_ = false; }
+    bool waitForPreemptedRelease(
+        const control::ControlAuthorityManager::Duration timeout)
+    {
+        return manager_.waitForPreemptedRelease(token_, timeout);
+    }
+
+    bool admissionCurrent() const
+    {
+        auto admission = globalStopAllAdmissionGate().lockAdmission();
+        return admission.accepting() &&
+               admission.generation() == admission_generation_;
+    }
+
+    bool current() const
+    {
+        return manager_.validate(token_) && admissionCurrent();
+    }
+
+    std::function<bool()> cancellationRequested(
+        grpc::ServerContext* context) const
+    {
+        const auto token = token_;
+        const auto admission_generation = admission_generation_;
+        return [context, token, admission_generation]() {
+            try {
+                if ((context && context->IsCancelled()) ||
+                       !control::ControlAuthorityManager::instance()
+                            .validate(token)) {
+                    return true;
+                }
+                auto admission =
+                    globalStopAllAdmissionGate().lockAdmission();
+                return !admission.accepting() ||
+                       admission.generation() != admission_generation;
+            } catch (...) {
+                return true;
+            }
+        };
+    }
+
+    control::ControlDispatchGuard tryBeginDispatch()
+    {
+        auto admission = globalStopAllAdmissionGate().lockAdmission();
+        if (!admission.accepting() ||
+            admission.generation() != admission_generation_) {
+            return {};
+        }
+        return manager_.tryBeginDispatch(token_);
+    }
+
+    void confirmSafeToRelease() noexcept { release_on_destroy_ = true; }
 
 private:
     control::ControlAuthorityManager& manager_;
@@ -157,8 +242,38 @@ private:
     std::string detail_;
     bool acquired_{false};
     bool release_on_destroy_{true};
+    bool rejected_by_stop_all_{false};
+    std::uint64_t admission_generation_{0U};
 };
 
+template <typename Response>
+grpc::Status setControlAdmissionFailure(
+    Response* response,
+    const std::string& device_id,
+    const ScopedUnaryAgvControlLease& lease)
+{
+    return lease.rejectedByStopAll()
+        ? setStopAllRejected(response, device_id)
+        : setControlLeaseConflict(response, device_id, lease.detail());
+}
+
+template <typename Response>
+grpc::Status setControlDispatchFailure(
+    Response* response,
+    const std::string& device_id,
+    const ScopedUnaryAgvControlLease& lease,
+    const char* operation)
+{
+    if (!lease.admissionCurrent()) {
+        return setStopAllRejected(response, device_id);
+    }
+    return setControlLeaseConflict(
+        response,
+        device_id,
+        std::string("control lease was preempted before ") + operation +
+            " dispatch");
+}
+
 template <typename Response, typename Operation>
 grpc::Status executeConfirmedAgvStop(
     Response* response,
@@ -168,23 +283,41 @@ grpc::Status executeConfirmedAgvStop(
     Operation&& operation)
 {
     try {
-        const auto command_result = operation();
+        const auto initial_stop = operation();
+        if (!initial_stop.ok()) {
+            return setResponseResult(response, initial_stop);
+        }
+
+        constexpr auto handler_release_timeout = std::chrono::seconds(15);
+        if (!control_barrier.waitForPreemptedRelease(
+                std::chrono::duration_cast<
+                    control::ControlAuthorityManager::Duration>(
+                        handler_release_timeout))) {
+            return setResponseResult(
+                response,
+                device::AgvResult::failure(
+                    device::AgvErrorCode::Timeout,
+                    std::string(operation_name) +
+                        " timed out waiting for the preempted control handler to exit"));
+        }
+
+        const auto final_stop = operation();
+        if (!final_stop.ok()) {
+            return setResponseResult(response, final_stop);
+        }
+
         const auto stopped = agv->confirmMotionStopped();
         if (!stopped.ok()) {
-            control_barrier.quarantine();
             std::string message = std::string(operation_name) +
                 " did not reach a confirmed stopped state: " +
                 stopped.message;
-            if (!command_result.ok()) {
-                message += "; command_result=" + command_result.message;
-            }
             return setResponseResult(
                 response,
                 device::AgvResult::failure(stopped.code, message));
         }
-        return setResponseResult(response, command_result);
+        control_barrier.confirmSafeToRelease();
+        return setResponseResult(response, final_stop);
     } catch (...) {
-        control_barrier.quarantine();
         throw;
     }
 }
@@ -209,7 +342,7 @@ device::AgvAdapterParams toAdapterParams(const msgs::AgvAdapterParams& src)
 
 device::AgvMotionOptions toMotionOptions(
     const msgs::AgvMotionOptions& src,
-    grpc::ServerContext* context = nullptr)
+    std::function<bool()> cancellation_requested = {})
 {
     device::AgvMotionOptions dst;
     dst.max_speed = src.max_speed();
@@ -222,11 +355,7 @@ device::AgvMotionOptions toMotionOptions(
     dst.asynchronous = src.asynchronous();
     dst.wait_timeout_ms = src.wait_timeout_ms();
     dst.poll_interval_ms = src.poll_interval_ms();
-    if (context) {
-        dst.cancellation_requested = [context]() {
-            return context->IsCancelled();
-        };
-    }
+    dst.cancellation_requested = std::move(cancellation_requested);
     return dst;
 }
 
@@ -556,8 +685,13 @@ grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext*,
         ScopedUnaryAgvControlLease control_lease(
             device_id, "clearFault");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        auto dispatch = control_lease.tryBeginDispatch();
+        if (!dispatch.acquired()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease, "clearFault");
         }
         return setResponseResult(response, agv->clearFault());
     } catch (const std::exception& e) {
@@ -582,12 +716,18 @@ grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext* context,
         ScopedUnaryAgvControlLease control_lease(
             device_id, "navigateToPose");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        if (!control_lease.current()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease, "navigateToPose");
         }
         return setResponseResult(response, agv->navigateToPose(
             toPose2d(request->pose()),
-            toMotionOptions(request->options(), context),
+            toMotionOptions(
+                request->options(),
+                control_lease.cancellationRequested(context)),
             toAdapterParams(request->adapter_params())));
     } catch (const std::exception& e) {
         fillFeedback(response->mutable_header(), false, e.what());
@@ -611,12 +751,19 @@ grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext* context,
         ScopedUnaryAgvControlLease control_lease(
             device_id, "navigateToStation");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        if (!control_lease.current()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease,
+                "navigateToStation");
         }
         return setResponseResult(response, agv->navigateToStation(
             request->station_id(),
-            toMotionOptions(request->options(), context),
+            toMotionOptions(
+                request->options(),
+                control_lease.cancellationRequested(context)),
             toAdapterParams(request->adapter_params())));
     } catch (const std::exception& e) {
         fillFeedback(response->mutable_header(), false, e.what());
@@ -640,8 +787,12 @@ grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext* context,
         ScopedUnaryAgvControlLease control_lease(
             device_id, "followPath");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        if (!control_lease.current()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease, "followPath");
         }
         std::vector<device::AgvPathSegment> path;
         path.reserve(static_cast<std::size_t>(request->path_size()));
@@ -652,7 +803,9 @@ grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext* context,
             response,
             agv->followPath(
                 path,
-                toMotionOptions(request->options(), context)));
+                toMotionOptions(
+                    request->options(),
+                    control_lease.cancellationRequested(context))));
     } catch (const std::exception& e) {
         fillFeedback(response->mutable_header(), false, e.what());
         return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
@@ -678,8 +831,13 @@ grpc::Status gRPCAgvServiceImpl::translate(
         ScopedUnaryAgvControlLease control_lease(
             device_id, "translate");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        auto dispatch = control_lease.tryBeginDispatch();
+        if (!dispatch.acquired()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease, "translate");
         }
         return setResponseResult(
             response,
@@ -707,8 +865,14 @@ grpc::Status gRPCAgvServiceImpl::pauseNavigation(grpc::ServerContext*,
         ScopedUnaryAgvControlLease control_lease(
             device_id, "pauseNavigation");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        auto dispatch = control_lease.tryBeginDispatch();
+        if (!dispatch.acquired()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease,
+                "pauseNavigation");
         }
         return setResponseResult(response, agv->pauseNavigation());
     } catch (const std::exception& e) {
@@ -730,8 +894,14 @@ grpc::Status gRPCAgvServiceImpl::resumeNavigation(grpc::ServerContext*,
         ScopedUnaryAgvControlLease control_lease(
             device_id, "resumeNavigation");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        auto dispatch = control_lease.tryBeginDispatch();
+        if (!dispatch.acquired()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease,
+                "resumeNavigation");
         }
         return setResponseResult(response, agv->resumeNavigation());
     } catch (const std::exception& e) {
@@ -778,8 +948,13 @@ grpc::Status gRPCAgvServiceImpl::setVelocity(grpc::ServerContext*,
         ScopedUnaryAgvControlLease control_lease(
             device_id, "setVelocity");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        auto dispatch = control_lease.tryBeginDispatch();
+        if (!dispatch.acquired()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease, "setVelocity");
         }
         return setResponseResult(response, agv->setVelocity(toVelocity(request->velocity())));
     } catch (const std::exception& e) {
@@ -874,8 +1049,13 @@ grpc::Status gRPCAgvServiceImpl::switchMap(grpc::ServerContext*,
         ScopedUnaryAgvControlLease control_lease(
             device_id, "switchMap");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        auto dispatch = control_lease.tryBeginDispatch();
+        if (!dispatch.acquired()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease, "switchMap");
         }
         return setResponseResult(response, agv->switchMap(request->map_name()));
     } catch (const std::exception& e) {
@@ -897,8 +1077,13 @@ grpc::Status gRPCAgvServiceImpl::uploadMap(grpc::ServerContext*,
         ScopedUnaryAgvControlLease control_lease(
             device_id, "uploadMap");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        auto dispatch = control_lease.tryBeginDispatch();
+        if (!dispatch.acquired()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease, "uploadMap");
         }
         return setResponseResult(response, agv->uploadMap(request->map_name(), request->content()));
     } catch (const std::exception& e) {
@@ -942,8 +1127,13 @@ grpc::Status gRPCAgvServiceImpl::startMapping(grpc::ServerContext*,
         ScopedUnaryAgvControlLease control_lease(
             device_id, "startMapping");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        auto dispatch = control_lease.tryBeginDispatch();
+        if (!dispatch.acquired()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease, "startMapping");
         }
         device::AgvMappingOptions options;
         options.dimension = toMapDimension(request->dimension());
@@ -1043,8 +1233,13 @@ grpc::Status gRPCAgvServiceImpl::stopMapping(grpc::ServerContext*,
         ScopedUnaryAgvControlLease control_lease(
             device_id, "stopMapping");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        auto dispatch = control_lease.tryBeginDispatch();
+        if (!dispatch.acquired()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease, "stopMapping");
         }
         return setResponseResult(response, agv->stopMapping());
     } catch (const std::exception& e) {

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

@@ -8,6 +8,7 @@
 
 #include "common/base/logging/logger.h"
 #include "manager/control_authority/include/control_authority_manager.h"
+#include "service/stop_all/include/stop_all_admission_gate.h"
 
 using google::protobuf::util::TimeUtil;
 
@@ -67,7 +68,9 @@ device::JointVelocityCommand toJointVelocityCommand(const api::JointVelocityComm
     return dst;
 }
 
-device::MotionOptions toMotionOptions(const api::MotionOptions& src)
+device::MotionOptions toMotionOptions(
+    const api::MotionOptions& src,
+    std::function<bool()> cancellation_requested = {})
 {
     device::MotionOptions dst;
     dst.velocity = src.velocity();
@@ -77,6 +80,7 @@ device::MotionOptions toMotionOptions(const api::MotionOptions& src)
     dst.joint_velocity_limits.assign(src.joint_velocity_limits().begin(),
                                      src.joint_velocity_limits().end());
     dst.asynchronous = src.asynchronous();
+    dst.cancellation_requested = std::move(cancellation_requested);
     return dst;
 }
 
@@ -124,6 +128,24 @@ grpc::Status setDeviceNotFound(Response* response, const std::string& device_id)
     return grpc::Status(grpc::StatusCode::NOT_FOUND, message);
 }
 
+grpc::Status setStopAllRejected(
+    api::CommandHeader_Feedback* response,
+    const std::string& device_id)
+{
+    const std::string message =
+        "RobotArm control is temporarily paused by StopAll: " + device_id;
+    fillFeedback(response, false, message);
+    return grpc::Status(grpc::StatusCode::UNAVAILABLE, message);
+}
+
+template <typename Response>
+grpc::Status setStopAllRejected(
+    Response* response,
+    const std::string& device_id)
+{
+    return setStopAllRejected(response->mutable_header(), device_id);
+}
+
 grpc::Status setControlLeaseConflict(
     api::CommandHeader_Feedback* response,
     const std::string& device_id,
@@ -169,9 +191,20 @@ public:
         const auto ttl = std::chrono::duration_cast<
             control::ControlAuthorityManager::Duration>(
                 std::chrono::hours(24));
-        auto acquired = preemptive
-            ? manager_.preemptAcquire(device_id, owner, ttl)
-            : manager_.tryAcquire(device_id, owner, ttl);
+        control::ControlAcquireResult acquired;
+        if (preemptive) {
+            acquired = manager_.preemptAcquire(device_id, owner, ttl);
+        } else {
+            auto admission =
+                globalStopAllAdmissionGate().lockAdmission();
+            if (!admission.accepting()) {
+                rejected_by_stop_all_ = true;
+                detail_ = "System StopAll admission is closed";
+                return;
+            }
+            admission_generation_ = admission.generation();
+            acquired = manager_.tryAcquire(device_id, owner, ttl);
+        }
         acquired_ = acquired.acquired;
         token_ = std::move(acquired.token);
         detail_ = std::move(acquired.detail);
@@ -182,21 +215,136 @@ public:
     {
         if (release_on_destroy_) {
             manager_.release(token_);
+        } else if (acquired_) {
+            (void)manager_.retireSafetyHolder(token_);
         }
     }
 
     bool acquired() const noexcept { return acquired_; }
+    bool rejectedByStopAll() const noexcept
+    {
+        return rejected_by_stop_all_;
+    }
     const std::string& detail() const noexcept { return detail_; }
     void confirmSafeToRelease() noexcept { release_on_destroy_ = true; }
 
+    bool waitForPreemptedRelease(
+        const control::ControlAuthorityManager::Duration timeout)
+    {
+        return manager_.waitForPreemptedRelease(token_, timeout);
+    }
+
+    bool admissionCurrent() const
+    {
+        auto admission = globalStopAllAdmissionGate().lockAdmission();
+        return admission.accepting() &&
+               admission.generation() == admission_generation_;
+    }
+
+    bool current() const
+    {
+        return manager_.validate(token_) && admissionCurrent();
+    }
+
+    std::function<bool()> cancellationRequested(
+        grpc::ServerContext* context) const
+    {
+        const auto token = token_;
+        const auto admission_generation = admission_generation_;
+        return [context, token, admission_generation]() {
+            try {
+                if ((context && context->IsCancelled()) ||
+                       !control::ControlAuthorityManager::instance()
+                            .validate(token)) {
+                    return true;
+                }
+                auto admission =
+                    globalStopAllAdmissionGate().lockAdmission();
+                return !admission.accepting() ||
+                       admission.generation() != admission_generation;
+            } catch (...) {
+                return true;
+            }
+        };
+    }
+
+    control::ControlDispatchGuard tryBeginDispatch()
+    {
+        auto admission = globalStopAllAdmissionGate().lockAdmission();
+        if (!admission.accepting() ||
+            admission.generation() != admission_generation_) {
+            return {};
+        }
+        return manager_.tryBeginDispatch(token_);
+    }
+
 private:
     control::ControlAuthorityManager& manager_;
     control::ControlLeaseToken token_;
     std::string detail_;
     bool acquired_{false};
     bool release_on_destroy_{true};
+    bool rejected_by_stop_all_{false};
+    std::uint64_t admission_generation_{0U};
 };
 
+template <typename Operation>
+device::Result executeConfirmedArmStop(
+    ScopedUnaryControlLease& control_barrier,
+    const char* operation_name,
+    Operation&& operation)
+{
+    const auto initial_stop = operation();
+    if (!initial_stop.ok()) {
+        return initial_stop;
+    }
+
+    constexpr auto handler_release_timeout = std::chrono::seconds(15);
+    if (!control_barrier.waitForPreemptedRelease(
+            std::chrono::duration_cast<
+                control::ControlAuthorityManager::Duration>(
+                    handler_release_timeout))) {
+        return device::Result::failure(
+            device::ArmErrorCode::Timeout,
+            std::string(operation_name) +
+                " timed out waiting for the preempted control handler to exit");
+    }
+
+    const auto final_stop = operation();
+    if (final_stop.ok()) {
+        control_barrier.confirmSafeToRelease();
+    }
+    return final_stop;
+}
+
+template <typename Response>
+grpc::Status setControlAdmissionFailure(
+    Response* response,
+    const std::string& device_id,
+    const ScopedUnaryControlLease& lease)
+{
+    return lease.rejectedByStopAll()
+        ? setStopAllRejected(response, device_id)
+        : setControlLeaseConflict(response, device_id, lease.detail());
+}
+
+template <typename Response>
+grpc::Status setControlDispatchFailure(
+    Response* response,
+    const std::string& device_id,
+    const ScopedUnaryControlLease& lease,
+    const char* operation)
+{
+    if (!lease.admissionCurrent()) {
+        return setStopAllRejected(response, device_id);
+    }
+    return setControlLeaseConflict(
+        response,
+        device_id,
+        std::string("control lease was preempted before ") + operation +
+            " dispatch");
+}
+
 } // namespace
 
 gRPCArmServiceImpl::gRPCArmServiceImpl()
@@ -220,10 +368,10 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*,
             return setControlLeaseConflict(
                 response, device_id, control_barrier.detail());
         }
-        const auto result = arm->torqueOff();
-        if (result.ok()) {
-            control_barrier.confirmSafeToRelease();
-        }
+        const auto result = executeConfirmedArmStop(
+            control_barrier,
+            "torqueOff",
+            [&arm]() { return arm->torqueOff(); });
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
         if (result.ok()) {
             logRpcSuccess("torqueOff", device_id);
@@ -248,8 +396,13 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*,
         ScopedUnaryControlLease control_lease(
             device_id, "torqueOn");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        auto dispatch = control_lease.tryBeginDispatch();
+        if (!dispatch.acquired()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease, "torqueOn");
         }
         const auto result = arm->torqueOn();
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
@@ -263,7 +416,7 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*,
     }
 }
 
-grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*,
+grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext* context,
                                        const api::MoveJ_Request* request,
                                        api::MoveJ_Response* response)
 {
@@ -276,11 +429,18 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*,
         ScopedUnaryControlLease control_lease(
             device_id, "moveJ");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
         }
-        const auto result = arm->moveJ(toJointPositionCommand(request->target()),
-                                       toMotionOptions(request->options()));
+        if (!control_lease.current()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease, "moveJ");
+        }
+        auto options = toMotionOptions(
+            request->options(),
+            control_lease.cancellationRequested(context));
+        const auto result = arm->moveJ(
+            toJointPositionCommand(request->target()), options);
         if (result.ok()) {
             CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveJ): success, id=" << device_id
                             << ", positions=" << request->target().position_size();
@@ -292,7 +452,7 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*,
     }
 }
 
-grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*,
+grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext* context,
                                        const api::MoveL_Request* request,
                                        api::MoveL_Response* response)
 {
@@ -305,12 +465,20 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*,
         ScopedUnaryControlLease control_lease(
             device_id, "moveL");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
         }
-        const auto result = arm->moveL(toCartesianPose(request->target()),
-                                       toMotionOptions(request->options()),
-                                       toFrameType(request->frame()));
+        if (!control_lease.current()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease, "moveL");
+        }
+        auto options = toMotionOptions(
+            request->options(),
+            control_lease.cancellationRequested(context));
+        const auto result = arm->moveL(
+            toCartesianPose(request->target()),
+            options,
+            toFrameType(request->frame()));
         if (result.ok()) {
             CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveL): success, id=" << device_id
                             << ", frame=" << request->frame();
@@ -335,8 +503,12 @@ grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*,
         ScopedUnaryControlLease control_lease(
             device_id, "speedJ");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        if (!control_lease.current()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease, "speedJ");
         }
         const auto result = arm->speedJ(toJointVelocityCommand(request->velocity()),
                                         request->acceleration(),
@@ -367,8 +539,12 @@ grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*,
         ScopedUnaryControlLease control_lease(
             device_id, "speedL");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        if (!control_lease.current()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease, "speedL");
         }
         const auto result = arm->speedL(toCartesianVelocity(request->velocity()),
                                         request->acceleration(),
@@ -400,8 +576,13 @@ grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext*,
         ScopedUnaryControlLease control_lease(
             device_id, "servoJ");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        auto dispatch = control_lease.tryBeginDispatch();
+        if (!dispatch.acquired()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease, "servoJ");
         }
         const auto result = arm->servoJ(toJointPositionCommand(request->target()));
         if (result.ok()) {
@@ -431,10 +612,10 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*,
             return setControlLeaseConflict(
                 response, device_id, control_barrier.detail());
         }
-        const auto result = arm->stopMotion();
-        if (result.ok()) {
-            control_barrier.confirmSafeToRelease();
-        }
+        const auto result = executeConfirmedArmStop(
+            control_barrier,
+            "stopMotion",
+            [&arm]() { return arm->stopMotion(); });
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
         if (result.ok()) {
             logRpcSuccess("stopMotion", device_id);
@@ -514,8 +695,12 @@ grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*,
         ScopedUnaryControlLease control_lease(
             device_id, "calibrateZeroQ");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        if (!control_lease.current()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease, "calibrateZeroQ");
         }
         const auto result = arm->calibrateZeroQ(request->joint_name());
         if (result.ok()) {
@@ -561,6 +746,18 @@ grpc::Status gRPCArmServiceImpl::ExecuteJsonCommand(
             return grpc::Status::OK;
         }
 
+        ScopedUnaryControlLease control_lease(
+            device_id, "ExecuteJsonCommand");
+        if (!control_lease.acquired()) {
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        if (!control_lease.current()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease,
+                "ExecuteJsonCommand");
+        }
+
         std::string response_json;
         const bool success = arm->executeJsonCommand(
             request->request_json(), response_json);
@@ -592,8 +789,13 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context,
         ScopedUnaryControlLease control_lease(
             device_id, "clearFault");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(
-                response, device_id, control_lease.detail());
+            return setControlAdmissionFailure(
+                response, device_id, control_lease);
+        }
+        auto dispatch = control_lease.tryBeginDispatch();
+        if (!dispatch.acquired()) {
+            return setControlDispatchFailure(
+                response, device_id, control_lease, "clearFault");
         }
         const auto result = arm->clearFault();
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);

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

@@ -14,6 +14,8 @@
 #include <unordered_set>
 #include <utility>
 
+#include "service/stop_all/include/stop_all_admission_gate.h"
+
 namespace cmvr::service {
 
 namespace {
@@ -497,10 +499,22 @@ grpc::Status ArmTeleopServiceImpl::Teleoperate(
         }
 
         const std::string session_id = nextSessionId();
-        const auto acquired = authority_->tryAcquire(
-            backend_manifest.robot_id(),
-            session_id,
-            std::chrono::milliseconds(negotiated.lease_ms));
+        control::ControlAcquireResult acquired;
+        {
+            auto admission =
+                globalStopAllAdmissionGate().lockAdmission();
+            if (!admission.accepting()) {
+                const grpc::Status status(
+                    grpc::StatusCode::UNAVAILABLE,
+                    "arm teleoperation is temporarily paused by StopAll");
+                writeBareRejection(stream, status);
+                return status;
+            }
+            acquired = authority_->tryAcquire(
+                backend_manifest.robot_id(),
+                session_id,
+                std::chrono::milliseconds(negotiated.lease_ms));
+        }
         if (!acquired.acquired) {
             const grpc::Status status(
                 grpc::StatusCode::RESOURCE_EXHAUSTED,
@@ -524,7 +538,7 @@ grpc::Status ArmTeleopServiceImpl::Teleoperate(
             return status;
         }
 
-        bool backend_open_attempted = true;
+        bool backend_open_attempted = false;
         bool backend_stopped = false;
         const auto safeStop =
             [&](const arm_teleop::StopReason reason,
@@ -552,7 +566,20 @@ grpc::Status ArmTeleopServiceImpl::Teleoperate(
                 "arm teleoperation handler terminated unexpectedly");
         });
 
-        const auto backend_open = backend_->open(first_frame.open());
+        ArmTeleopBackendResult backend_open;
+        {
+            auto dispatch =
+                authority_->tryBeginDispatch(control_lease);
+            if (!dispatch.acquired()) {
+                const grpc::Status status(
+                    grpc::StatusCode::ABORTED,
+                    "arm teleoperation control authority was revoked before backend open");
+                writeBareRejection(stream, status);
+                return status;
+            }
+            backend_open_attempted = true;
+            backend_open = backend_->open(first_frame.open());
+        }
         if (!backend_open.success) {
             const auto stopped = safeStop(
                 arm_teleop::STOP_REASON_PROTOCOL_ERROR,
@@ -793,6 +820,17 @@ grpc::Status ArmTeleopServiceImpl::Teleoperate(
                                  status.error_message() + "; " +
                                      stopped.detail);
             }
+            if (!authority_->validate(control_lease)) {
+                const std::string detail =
+                    "arm teleoperation control authority was revoked";
+                return finish(
+                    arm_teleop::SESSION_PHASE_LEASE_LOST,
+                    arm_teleop::STOP_REASON_LEASE_REVOKED,
+                    detail,
+                    grpc::Status(
+                        grpc::StatusCode::ABORTED, detail),
+                    true);
+            }
 
             std::optional<PendingFrame> pending;
             bool ended = false;
@@ -1064,8 +1102,24 @@ grpc::Status ArmTeleopServiceImpl::Teleoperate(
             session.lease_deadline =
                 pending->arrived +
                 std::chrono::milliseconds(session.lease_ms);
-            const auto applied =
-                backend_->applySetpoint(setpoint, command_deadline);
+            ArmTeleopBackendResult applied;
+            {
+                auto dispatch =
+                    authority_->tryBeginDispatch(control_lease);
+                if (!dispatch.acquired()) {
+                    const std::string detail =
+                        "arm teleoperation control authority was revoked";
+                    return finish(
+                        arm_teleop::SESSION_PHASE_LEASE_LOST,
+                        arm_teleop::STOP_REASON_LEASE_REVOKED,
+                        detail,
+                        grpc::Status(
+                            grpc::StatusCode::ABORTED, detail),
+                        true);
+                }
+                applied = backend_->applySetpoint(
+                    setpoint, command_deadline);
+            }
             if (!applied.success) {
                 ++session.rejected_setpoints;
                 return finish(

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

@@ -1,5 +1,8 @@
 #include "common/base/logging/logger.h"
 #include "manager/media_source_hub/include/device_media_source_adapter.h"
+#include "service/grpc/include/camera_operational_activity_registry.h"
+#include "service/grpc/include/camera_ptz_activity_registry.h"
+#include "service/grpc/include/media_activity_coordinator.h"
 //
 // Created by xtkuang on 2025/6/1.
 //
@@ -18,7 +21,6 @@ using namespace cmvr::device;
 namespace {
 template <typename ResponseT>
 grpc::Status failResponse(ResponseT* response, const std::string& message) {
-    CMVR_LOG(ERROR) << "[gRPCCameraServiceImpl] " << message;
     response->mutable_header()->set_success(false);
     response->mutable_header()->set_error_message(message);
     setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@@ -72,9 +74,13 @@ bool toPtzCommand(cmvr::api::ControlPtzCommand_Command command, PtzCommand& out)
 // the one startStreaming() reference acquired by this call.
 class CameraStreamingLease final {
 public:
-    explicit CameraStreamingLease(std::shared_ptr<AbstractCamera> camera)
+    CameraStreamingLease(
+        std::shared_ptr<AbstractCamera> camera,
+        const MediaActivityCoordinator::Session& session)
         : camera_(std::move(camera)) {
-        active_ = camera_ && camera_->startStreaming();
+        (void)session.runIfCurrent([this] {
+            active_ = camera_ && camera_->startStreaming();
+        });
     }
 
     ~CameraStreamingLease() {
@@ -100,6 +106,25 @@ private:
     std::shared_ptr<AbstractCamera> camera_;
     bool active_{false};
 };
+
+grpc::Status mediaStoppedStatus()
+{
+    return grpc::Status(
+        grpc::StatusCode::CANCELLED,
+        "Media activity stopped by StopAll");
+}
+
+template <typename FeedbackT, typename StreamT>
+grpc::Status rejectStreamDuringStopAll(StreamT* stream)
+{
+    FeedbackT response;
+    response.mutable_header()->set_success(false);
+    response.mutable_header()->set_error_message(
+        "Media activities are temporarily paused by StopAll");
+    setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
+    stream->Write(response);
+    return grpc::Status::OK;
+}
 }
 
 gRPCCameraServiceImpl::gRPCCameraServiceImpl(
@@ -143,6 +168,11 @@ grpc::Status gRPCCameraServiceImpl::GetStatus(grpc::ServerContext* context,
 grpc::Status gRPCCameraServiceImpl::StartCamera(grpc::ServerContext* context,
     const api::StartCameraCommand_Request* request, api::StartCameraCommand_Feedback* response)
 {
+    auto media_session = globalMediaActivityCoordinator().beginSession();
+    if (!media_session) {
+        return failResponse(
+            response, "Media activities are temporarily paused by StopAll");
+    }
     try {
         string dev_id = request->header().device_id();
         CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StartCamera): id=" << dev_id;
@@ -150,7 +180,24 @@ grpc::Status gRPCCameraServiceImpl::StartCamera(grpc::ServerContext* context,
         if (!dev) {
             return failResponse(response, "Camera device not found: " + dev_id);
         }
-        if (!dev->start()) {
+        CameraOperationalActivityRegistry::DispatchResult dispatch =
+            CameraOperationalActivityRegistry::DispatchResult::DeviceFailure;
+        const bool start_allowed = media_session.runIfCurrent([&] {
+            dispatch = globalCameraOperationalActivityRegistry().start(
+                dev_id, dev);
+        });
+        if (!start_allowed) {
+            return failResponse(
+                response, "Camera start was canceled by StopAll");
+        }
+        if (dispatch ==
+            CameraOperationalActivityRegistry::DispatchResult::
+                RejectedByStopAll) {
+            return failResponse(
+                response, "Camera start was canceled by StopAll");
+        }
+        if (dispatch ==
+            CameraOperationalActivityRegistry::DispatchResult::DeviceFailure) {
             return failResponse(response, "Failed to start camera: " + dev_id);
         }
         response->mutable_header()->set_success(true);
@@ -175,9 +222,20 @@ grpc::Status gRPCCameraServiceImpl::StopCamera(grpc::ServerContext* context,
         if (!dev) {
             return failResponse(response, "Camera device not found: " + dev_id);
         }
-        if (!dev->stop()) {
+        const auto dispatch =
+            globalCameraOperationalActivityRegistry().stopLifecycle(
+                dev_id, dev);
+        if (dispatch ==
+            CameraOperationalActivityRegistry::DispatchResult::
+                RejectedByStopAll) {
+            return failResponse(
+                response, "Camera control is temporarily paused by StopAll");
+        }
+        if (dispatch ==
+            CameraOperationalActivityRegistry::DispatchResult::DeviceFailure) {
             return failResponse(response, "Failed to stop camera: " + dev_id);
         }
+        globalCameraPtzActivityRegistry().markCameraStopped(dev_id);
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
         return grpc::Status::OK;
@@ -193,6 +251,16 @@ grpc::Status gRPCCameraServiceImpl::StopCamera(grpc::ServerContext* context,
 grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
     const api::GetRGBImageCommand_Request* request, api::GetRGBImageCommand_Feedback* response)
 {
+    auto media_session = globalMediaActivityCoordinator().beginSession(
+        [context] {
+            if (context) {
+                context->TryCancel();
+            }
+        });
+    if (!media_session) {
+        return failResponse(
+            response, "Camera capture is temporarily paused by StopAll");
+    }
     try {
         string dev_id = request->header().device_id();
         CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImage): id=" << dev_id;
@@ -202,7 +270,11 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
             return failResponse(response, "Camera device not found: " + dev_id);
         }
         Rs2Intrinsics intrinsics = {0};
-        dev->getRGBImage(image,intrinsics);
+        if (!media_session.runIfCurrent(
+                [&] { dev->getRGBImage(image, intrinsics); })) {
+            return failResponse(
+                response, "Camera capture was canceled by StopAll");
+        }
         response->mutable_header()->set_success(true);
 
         response->mutable_intrinsics()->set_fx(intrinsics.fx);
@@ -246,6 +318,16 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
 grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context,
     const api::GetDepthImageCommand_Request* request, api::GetDepthImageCommand_Feedback* response)
 {
+    auto media_session = globalMediaActivityCoordinator().beginSession(
+        [context] {
+            if (context) {
+                context->TryCancel();
+            }
+        });
+    if (!media_session) {
+        return failResponse(
+            response, "Camera capture is temporarily paused by StopAll");
+    }
     try {
         string dev_id = request->header().device_id();
         CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImage): id=" << dev_id;
@@ -255,7 +337,11 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context,
             return failResponse(response, "Camera device not found: " + dev_id);
         }
         Rs2Intrinsics intrinsics = {0};
-        dev->getDepthImage(image,intrinsics);
+        if (!media_session.runIfCurrent(
+                [&] { dev->getDepthImage(image, intrinsics); })) {
+            return failResponse(
+                response, "Camera capture was canceled by StopAll");
+        }
         response->mutable_header()->set_success(true);
 
         response->mutable_intrinsics()->set_fx(intrinsics.fx);
@@ -302,6 +388,16 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context,
 grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context,
     const api::GetRGBDImagesCommand_Request* request, api::GetRGBDImagesCommand_Feedback* response)
 {
+    auto media_session = globalMediaActivityCoordinator().beginSession(
+        [context] {
+            if (context) {
+                context->TryCancel();
+            }
+        });
+    if (!media_session) {
+        return failResponse(
+            response, "Camera capture is temporarily paused by StopAll");
+    }
     try {
         string dev_id = request->header().device_id();
         CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImages): id=" << dev_id;
@@ -311,7 +407,14 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context,
             return failResponse(response, "Camera device not found: " + dev_id);
         }
         Rs2Intrinsics intrinsics = {0};
-        dev->getRGBDImages(color_image,depth_image, intrinsics);
+        if (!media_session.runIfCurrent(
+                [&] {
+                    dev->getRGBDImages(
+                        color_image, depth_image, intrinsics);
+                })) {
+            return failResponse(
+                response, "Camera capture was canceled by StopAll");
+        }
         response->mutable_header()->set_success(true);
 
         response->mutable_intrinsics()->set_fx(intrinsics.fx);
@@ -371,6 +474,11 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context,
 grpc::Status gRPCCameraServiceImpl::StartRecording(grpc::ServerContext* context,
     const api::StartCameraRecordingCommand_Request* request, api::StartCameraRecordingCommand_Feedback* response)
 {
+    auto media_session = globalMediaActivityCoordinator().beginSession();
+    if (!media_session) {
+        return failResponse(
+            response, "Media activities are temporarily paused by StopAll");
+    }
     try {
         string dev_id = request->header().device_id();
         CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StartRecording): id=" << dev_id;
@@ -378,7 +486,12 @@ grpc::Status gRPCCameraServiceImpl::StartRecording(grpc::ServerContext* context,
         if (!dev) {
             return failResponse(response, "Camera device not found: " + dev_id);
         }
-        dev->startRecording(request->video_path());
+        if (!media_session.runIfCurrent([&] {
+                dev->startRecording(request->video_path());
+            })) {
+            return failResponse(
+                response, "Camera recording start was canceled by StopAll");
+        }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
         return grpc::Status::OK;
@@ -438,7 +551,17 @@ grpc::Status gRPCCameraServiceImpl::ControlPtz(grpc::ServerContext* context,
             return failResponse(response, "Invalid PTZ action");
         }
         const bool stop = request->action() == api::ControlPtzCommand_Action_STOP;
-        if (!dev->controlPtz(command, stop, static_cast<int>(request->speed()))) {
+        const auto dispatch = globalCameraPtzActivityRegistry().control(
+            dev_id,
+            dev,
+            command,
+            stop,
+            static_cast<int>(request->speed()));
+        if (dispatch == CameraPtzActivityRegistry::DispatchResult::RejectedByStopAll) {
+            return failResponse(
+                response, "PTZ control is temporarily paused by StopAll");
+        }
+        if (dispatch == CameraPtzActivityRegistry::DispatchResult::DeviceFailure) {
             CameraState state{};
             dev->getState(state);
             const std::string error_message =
@@ -461,11 +584,19 @@ grpc::Status gRPCCameraServiceImpl::ControlPtz(grpc::ServerContext* context,
 
 grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* context
     , grpc::ServerReaderWriter<cmvr::api::GetDepthImageStreamCommand_Feedback, cmvr::api::GetDepthImageStreamCommand_Request>* stream){
+    auto media_session = globalMediaActivityCoordinator().beginSession(
+        [context] { context->TryCancel(); });
+    if (!media_session) {
+        return rejectStreamDuringStopAll<
+            api::GetDepthImageStreamCommand_Feedback>(stream);
+    }
     try {
         //读取首次传递的数据，获取设备id
         api::GetDepthImageStreamCommand_Request request;
         if (!stream->Read(&request)) {
-            return grpc::Status::OK;
+            return media_session.cancelled()
+                ? mediaStoppedStatus()
+                : grpc::Status::OK;
         }
         string dev_id = request.header().device_id();
         CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImageStream): start,id=" << dev_id;
@@ -478,7 +609,7 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
             stream->Write(response);
             return grpc::Status::OK;
         }
-        CameraStreamingLease stream_lease(dev);
+        CameraStreamingLease stream_lease(dev, media_session);
         if (!stream_lease) {
             api::GetDepthImageStreamCommand_Feedback response;
             response.mutable_header()->set_success(false);
@@ -492,7 +623,7 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
         size_t index = 0;
         while (true)
         {
-            if (context->IsCancelled())
+            if (media_session.cancelled() || context->IsCancelled())
             {
                 CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled,id=" << dev_id;
                 break;
@@ -501,6 +632,7 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
             api::GetDepthImageStreamCommand_Feedback response;
             cmvr::device::StreamFrameData frame_data;
             if (dev->waitEncodedFrame(frame_data, index, std::chrono::milliseconds(100)) &&
+                !media_session.cancelled() &&
                 !frame_data.depthFrame.empty()) {
                 response.mutable_header()->set_success(true);
                 setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
@@ -527,9 +659,14 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
             }
         }
         CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImageStream): end,id=" << dev_id;
-        return grpc::Status::OK;
+        return media_session.cancelled()
+            ? mediaStoppedStatus()
+            : grpc::Status::OK;
     }
     catch (const exception &e) {
+        if (media_session.cancelled()) {
+            return mediaStoppedStatus();
+        }
         api::GetDepthImageStreamCommand_Feedback response;
         response.mutable_header()->set_success(false);
         response.mutable_header()->set_error_message(e.what());
@@ -540,11 +677,19 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
 }
 grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* context
     , grpc::ServerReaderWriter<cmvr::api::GetRGBDImagesStreamCommand_Feedback, cmvr::api::GetRGBDImagesStreamCommand_Request>* stream){
+    auto media_session = globalMediaActivityCoordinator().beginSession(
+        [context] { context->TryCancel(); });
+    if (!media_session) {
+        return rejectStreamDuringStopAll<
+            api::GetRGBDImagesStreamCommand_Feedback>(stream);
+    }
     try {
         //读取首次传递的数据，获取设备id
         api::GetRGBDImagesStreamCommand_Request request;
         if (!stream->Read(&request)) {
-            return grpc::Status::OK;
+            return media_session.cancelled()
+                ? mediaStoppedStatus()
+                : grpc::Status::OK;
         }
         string dev_id = request.header().device_id();
         CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): start,id=" << dev_id;
@@ -557,7 +702,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con
             stream->Write(response);
             return grpc::Status::OK;
         }
-        CameraStreamingLease stream_lease(dev);
+        CameraStreamingLease stream_lease(dev, media_session);
         if (!stream_lease) {
             api::GetRGBDImagesStreamCommand_Feedback response;
             response.mutable_header()->set_success(false);
@@ -571,7 +716,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con
         size_t index = 0;
         while (true)
         {
-            if (context->IsCancelled())
+            if (media_session.cancelled() || context->IsCancelled())
             {
                 CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBDImagesStream) context is cancelled,id=" << dev_id;
                 break;
@@ -580,6 +725,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con
             api::GetRGBDImagesStreamCommand_Feedback response;
             cmvr::device::StreamFrameData frame_data;
             if (dev->waitEncodedFrame(frame_data, index, std::chrono::milliseconds(100)) &&
+                !media_session.cancelled() &&
                 !frame_data.rgbFrame.empty() &&
                 !frame_data.depthFrame.empty()) {
                 response.mutable_header()->set_success(true);
@@ -613,9 +759,14 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con
             }
         }
         CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): end,id=" << dev_id;
-        return grpc::Status::OK;
+        return media_session.cancelled()
+            ? mediaStoppedStatus()
+            : grpc::Status::OK;
     }
     catch (const exception &e) {
+        if (media_session.cancelled()) {
+            return mediaStoppedStatus();
+        }
         api::GetRGBDImagesStreamCommand_Feedback response;
         response.mutable_header()->set_success(false);
         response.mutable_header()->set_error_message(e.what());
@@ -625,11 +776,19 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con
     }
 }
 grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback, cmvr::api::GetRGBImageStreamCommand_Request>* stream){
+    auto media_session = globalMediaActivityCoordinator().beginSession(
+        [context] { context->TryCancel(); });
+    if (!media_session) {
+        return rejectStreamDuringStopAll<
+            api::GetRGBImageStreamCommand_Feedback>(stream);
+    }
     try {
         //读取首次传递的数据，获取设备id
         api::GetRGBImageStreamCommand_Request request;
         if (!stream->Read(&request)) {
-            return grpc::Status::OK;
+            return media_session.cancelled()
+                ? mediaStoppedStatus()
+                : grpc::Status::OK;
         }
         string dev_id = request.header().device_id();
         CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id
@@ -646,10 +805,17 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte
         }
         auto& media_hub = cmvr::media::globalMediaSourceHub();
         const std::string track_id = cmvr::media::cameraColorTrackId(dev_id);
-        if (!cmvr::media::ensureCameraMediaSource(media_hub, dev)) {
+        bool source_ready = false;
+        const bool source_setup_allowed = media_session.runIfCurrent([&] {
+            source_ready = cmvr::media::ensureCameraMediaSource(media_hub, dev);
+        });
+        if (!source_setup_allowed || !source_ready) {
             api::GetRGBImageStreamCommand_Feedback response;
             response.mutable_header()->set_success(false);
-            response.mutable_header()->set_error_message("Failed to register camera media source: " + dev_id);
+            response.mutable_header()->set_error_message(
+                media_session.cancelled()
+                    ? "Camera stream start was canceled by StopAll"
+                    : "Failed to register camera media source: " + dev_id);
             setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
             stream->Write(response);
             return grpc::Status::OK;
@@ -657,7 +823,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte
         auto subscription = media_hub.subscribe(
             track_id,
             cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED,
-            [context] { return context->IsCancelled(); });
+            [context, &media_session] {
+                return context->IsCancelled() || media_session.cancelled();
+            });
         if (!subscription) {
             api::GetRGBImageStreamCommand_Feedback response;
             response.mutable_header()->set_success(false);
@@ -713,13 +881,16 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte
         };
         while (true)
         {
-            if (context->IsCancelled())
+            if (media_session.cancelled() || context->IsCancelled())
             {
                 CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled,id=" << dev_id;
                 break;
             }
 
             const auto read = subscription.waitRead(std::chrono::milliseconds(100));
+            if (media_session.cancelled()) {
+                break;
+            }
             if (!read || !read->value || read->value->empty()) {
                 if (!subscription.valid()) {
                     break;
@@ -807,7 +978,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte
                 static_cast<uint64_t>(std::numeric_limits<int32_t>::max()))));
 
             const auto write_started = std::chrono::steady_clock::now();
-            if (!stream->Write(response)) {
+            if (media_session.cancelled() || !stream->Write(response)) {
                 CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id;
                 break;
             }
@@ -830,9 +1001,14 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte
             }
         }
         CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end,id=" << dev_id;
-        return grpc::Status::OK;
+        return media_session.cancelled()
+            ? mediaStoppedStatus()
+            : grpc::Status::OK;
     }
     catch (const exception &e) {
+        if (media_session.cancelled()) {
+            return mediaStoppedStatus();
+        }
         api::GetRGBImageStreamCommand_Feedback response;
         response.mutable_header()->set_success(false);
         response.mutable_header()->set_error_message(e.what());

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

@@ -5,13 +5,19 @@
 
 #include "../include/grpc_dexhand_service.h"
 
+#include <atomic>
 #include <cmath>
 #include <chrono>
+#include <cstdint>
 #include <memory>
 #include <thread>
+#include <utility>
 #include <vector>
 
 #include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h"
+#include "manager/control_authority/include/control_authority_manager.h"
+#include "service/grpc/include/media_activity_coordinator.h"
+#include "service/stop_all/include/stop_all_admission_gate.h"
 
 using namespace std;
 using namespace cmvr::service;
@@ -114,13 +120,130 @@ bool applyFreedomValues(const FreedomCollection& freedoms,
 
 template <typename ResponseT>
 grpc::Status failResponse(ResponseT* response, const std::string& message) {
-    CMVR_LOG(ERROR) << "[gRPCDexHandServiceImpl] " << message;
     response->mutable_header()->set_success(false);
     response->mutable_header()->set_error_message(message);
     setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
     return grpc::Status::OK;
 }
 
+class ScopedDexHandControlLease final {
+public:
+    ScopedDexHandControlLease(const std::string& device_id,
+                              const char* operation)
+        : manager_(cmvr::control::ControlAuthorityManager::instance()) {
+        static std::atomic<std::uint64_t> sequence{0U};
+        const std::string owner =
+            std::string("grpc-dexhand-unary:") + operation + ":" +
+            std::to_string(
+                sequence.fetch_add(1U, std::memory_order_relaxed) + 1U);
+        const auto ttl = std::chrono::duration_cast<
+            cmvr::control::ControlAuthorityManager::Duration>(
+                std::chrono::hours(24));
+
+        auto admission = globalStopAllAdmissionGate().lockAdmission();
+        if (!admission.accepting()) {
+            rejected_by_stop_all_ = true;
+            detail_ = "System StopAll admission is closed";
+            return;
+        }
+        admission_generation_ = admission.generation();
+
+        auto acquired = manager_.tryAcquire(device_id, owner, ttl);
+        acquired_ = acquired.acquired;
+        token_ = std::move(acquired.token);
+        detail_ = std::move(acquired.detail);
+    }
+
+    ~ScopedDexHandControlLease() {
+        manager_.release(token_);
+    }
+
+    bool acquired() const noexcept { return acquired_; }
+    bool rejectedByStopAll() const noexcept {
+        return rejected_by_stop_all_;
+    }
+    const std::string& detail() const noexcept { return detail_; }
+
+    bool admissionCurrent() const {
+        auto admission = globalStopAllAdmissionGate().lockAdmission();
+        return admission.accepting() &&
+            admission.generation() == admission_generation_;
+    }
+
+    cmvr::control::ControlDispatchGuard tryBeginDispatch() {
+        auto admission = globalStopAllAdmissionGate().lockAdmission();
+        if (!admission.accepting() ||
+            admission.generation() != admission_generation_) {
+            return {};
+        }
+        return manager_.tryBeginDispatch(token_);
+    }
+
+private:
+    cmvr::control::ControlAuthorityManager& manager_;
+    cmvr::control::ControlLeaseToken token_;
+    std::string detail_;
+    bool acquired_{false};
+    bool rejected_by_stop_all_{false};
+    std::uint64_t admission_generation_{0U};
+};
+
+template <typename ResponseT>
+grpc::Status failControlAdmission(
+    ResponseT* response,
+    const std::string& device_id,
+    const ScopedDexHandControlLease& lease) {
+    if (lease.rejectedByStopAll()) {
+        return failResponse(
+            response,
+            "DexHand control is temporarily paused by StopAll: " + device_id);
+    }
+    return failResponse(
+        response,
+        "DexHand control is leased by another active operation: " +
+            device_id +
+            (lease.detail().empty() ? "" : " (" + lease.detail() + ")"));
+}
+
+template <typename ResponseT>
+grpc::Status failControlDispatch(
+    ResponseT* response,
+    const std::string& device_id,
+    const ScopedDexHandControlLease& lease) {
+    if (!lease.admissionCurrent()) {
+        return failResponse(
+            response,
+            "DexHand control was preempted by StopAll: " + device_id);
+    }
+    return failResponse(
+        response,
+        "DexHand control lease was preempted before device dispatch: " +
+            device_id);
+}
+
+template <typename ResponseT, typename Operation>
+bool dispatchDexHandCommand(
+    ResponseT* response,
+    const std::string& device_id,
+    const std::shared_ptr<AbstractDexHand>& dev,
+    ScopedDexHandControlLease& lease,
+    Operation&& operation) {
+    auto dispatch = lease.tryBeginDispatch();
+    if (!dispatch.acquired()) {
+        (void)failControlDispatch(response, device_id, lease);
+        return false;
+    }
+    if (!dev->resumeOperationalActivity()) {
+        (void)failResponse(
+            response,
+            "DexHand operational activity could not be resumed: " +
+                device_id);
+        return false;
+    }
+    std::forward<Operation>(operation)();
+    return true;
+}
+
 std::vector<int> readCurrentAngles(const std::shared_ptr<AbstractDexHand>& dev) {
     DexHandState state{};
     dev->getState(state);
@@ -234,6 +357,10 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context
         if (!dev) {
             return failResponse(response, "DexHand device not found: " + dev_id);
         }
+        ScopedDexHandControlLease control_lease(dev_id, "SetDexHandPos");
+        if (!control_lease.acquired()) {
+            return failControlAdmission(response, dev_id, control_lease);
+        }
         if (respondUnsupportedForRh56(dev,
                                       "SetDexHandPos",
                                       "Use SetDexHandAngle for RH56 joint commands.",
@@ -246,7 +373,14 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context
         if (!applyFreedomValues(request->values(), DEXHAND_MAX_POSITION, finger_joint_targets, &error_message)) {
             return failResponse(response, error_message);
         }
-        dev->setPositions(finger_joint_targets);
+        if (!dispatchDexHandCommand(
+                response,
+                dev_id,
+                dev,
+                control_lease,
+                [&] { dev->setPositions(finger_joint_targets); })) {
+            return grpc::Status::OK;
+        }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
         CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandPos): success, id=" << dev_id
@@ -271,6 +405,10 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* contex
         if (!dev) {
             return failResponse(response, "DexHand device not found: " + dev_id);
         }
+        ScopedDexHandControlLease control_lease(dev_id, "SetDexHandAngle");
+        if (!control_lease.acquired()) {
+            return failControlAdmission(response, dev_id, control_lease);
+        }
 
         if (const auto rh56 = std::dynamic_pointer_cast<RH56DFTPDexhand>(dev)) {
             std::vector<int> finger_joint_targets = readCurrentAngles(dev);
@@ -278,14 +416,28 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* contex
             if (!applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets, &error_message)) {
                 return failResponse(response, error_message);
             }
-            rh56->setAngles(finger_joint_targets);
+            if (!dispatchDexHandCommand(
+                    response,
+                    dev_id,
+                    dev,
+                    control_lease,
+                    [&] { rh56->setAngles(finger_joint_targets); })) {
+                return grpc::Status::OK;
+            }
         } else {
             std::vector<int> finger_joint_targets(static_cast<size_t>(kDexHandDofCount), -1);
             std::string error_message;
             if (!applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets, &error_message)) {
                 return failResponse(response, error_message);
             }
-            dev->setAngles(finger_joint_targets);
+            if (!dispatchDexHandCommand(
+                    response,
+                    dev_id,
+                    dev,
+                    control_lease,
+                    [&] { dev->setAngles(finger_joint_targets); })) {
+                return grpc::Status::OK;
+            }
         }
 
         response->mutable_header()->set_success(true);
@@ -312,6 +464,10 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* contex
         if (!dev) {
             return failResponse(response, "DexHand device not found: " + dev_id);
         }
+        ScopedDexHandControlLease control_lease(dev_id, "SetDexHandForce");
+        if (!control_lease.acquired()) {
+            return failControlAdmission(response, dev_id, control_lease);
+        }
         if (respondUnsupportedForRh56(dev,
                                       "SetDexHandForce",
                                       "RH56DFTPDexhand currently exposes angle and tactile APIs only.",
@@ -324,7 +480,14 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* contex
         if (!applyFreedomValues(request->values(), DEXHAND_MAX_FORCE, finger_joint_targets, &error_message)) {
             return failResponse(response, error_message);
         }
-        dev->setForce(finger_joint_targets);
+        if (!dispatchDexHandCommand(
+                response,
+                dev_id,
+                dev,
+                control_lease,
+                [&] { dev->setForce(finger_joint_targets); })) {
+            return grpc::Status::OK;
+        }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
         CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandForce): success, id=" << dev_id
@@ -349,6 +512,10 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* contex
         if (!dev) {
             return failResponse(response, "DexHand device not found: " + dev_id);
         }
+        ScopedDexHandControlLease control_lease(dev_id, "SetDexHandSpeed");
+        if (!control_lease.acquired()) {
+            return failControlAdmission(response, dev_id, control_lease);
+        }
         if (respondUnsupportedForRh56(dev,
                                       "SetDexHandSpeed",
                                       "RH56DFTPDexhand currently exposes angle and tactile APIs only.",
@@ -361,7 +528,14 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* contex
         if (!applyFreedomValues(request->values(), DEXHAND_MAX_SPEED, finger_joint_targets, &error_message)) {
             return failResponse(response, error_message);
         }
-        dev->setVelocities(finger_joint_targets);
+        if (!dispatchDexHandCommand(
+                response,
+                dev_id,
+                dev,
+                control_lease,
+                [&] { dev->setVelocities(finger_joint_targets); })) {
+            return grpc::Status::OK;
+        }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
         CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandSpeed): success, id=" << dev_id
@@ -386,6 +560,11 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPresetAct(grpc::ServerContext* co
         if (!dev) {
             return failResponse(response, "DexHand device not found: " + dev_id);
         }
+        ScopedDexHandControlLease control_lease(
+            dev_id, "SetDexHandPresetAct");
+        if (!control_lease.acquired()) {
+            return failControlAdmission(response, dev_id, control_lease);
+        }
         if (respondUnsupportedForRh56(dev,
                                       "SetDexHandPresetAct",
                                       "RH56DFTPDexhand currently exposes angle and tactile APIs only.",
@@ -394,7 +573,14 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPresetAct(grpc::ServerContext* co
         }
 
         auto presetActId = request->presetactid();
-        dev->setPresetAct(presetActId);
+        if (!dispatchDexHandCommand(
+                response,
+                dev_id,
+                dev,
+                control_lease,
+                [&] { dev->setPresetAct(presetActId); })) {
+            return grpc::Status::OK;
+        }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
         CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandPresetAct): success, id=" << dev_id
@@ -413,6 +599,13 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context
     , const cmvr::api::GetSensorDataCommand_Request* request
     , cmvr::api::GetSensorDataCommand_Feedback* response) {
 
+    auto media_session = globalMediaActivityCoordinator().beginSession();
+    if (!media_session) {
+        return failResponse(
+            response,
+            "DexHand sensor activity is temporarily paused by StopAll");
+    }
+
     try {
         string dev_id = request->header().device_id();
         CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorData): id=" << dev_id;
@@ -420,8 +613,28 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context
         if (!dev) {
             return failResponse(response, "DexHand device not found: " + dev_id);
         }
-        maybeConfigureRh56FullTactilePolling(dev);
-        appendSensorData(dev->getSensorData(), response);
+
+        bool resumed = false;
+        std::vector<AbstractDexHand::TactileRegionData> sensor_data;
+        if (!media_session.runIfCurrent([&] {
+                resumed = dev->resumeOperationalActivity();
+                if (!resumed) {
+                    return;
+                }
+                maybeConfigureRh56FullTactilePolling(dev);
+                sensor_data = dev->getSensorData();
+            })) {
+            return failResponse(
+                response,
+                "DexHand sensor activity was preempted by StopAll: " +
+                    dev_id);
+        }
+        if (!resumed) {
+            return failResponse(
+                response,
+                "DexHand sensor activity could not be resumed: " + dev_id);
+        }
+        appendSensorData(sensor_data, response);
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
         CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorData): success, id=" << dev_id
@@ -439,6 +652,18 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context
 grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* context
     , grpc::ServerReaderWriter<cmvr::api::GetSensorDataStreamCommand_Feedback, cmvr::api::GetSensorDataStreamCommand_Request>* stream)
 {
+    auto media_session = globalMediaActivityCoordinator().beginSession(
+        [context] {
+            if (context) {
+                context->TryCancel();
+            }
+        });
+    if (!media_session) {
+        return grpc::Status(
+            grpc::StatusCode::UNAVAILABLE,
+            "DexHand sensor stream is temporarily paused by StopAll");
+    }
+
     try {
         api::GetSensorDataStreamCommand_Request request;
         if (!stream->Read(&request)) {
@@ -456,16 +681,46 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co
             stream->Write(response);
             return grpc::Status::OK;
         }
-        maybeConfigureRh56FullTactilePolling(dev);
+
+        bool resumed = false;
+        if (!media_session.runIfCurrent([&] {
+                resumed = dev->resumeOperationalActivity();
+                if (resumed) {
+                    maybeConfigureRh56FullTactilePolling(dev);
+                }
+            })) {
+            return grpc::Status(
+                grpc::StatusCode::CANCELLED,
+                "DexHand sensor stream was preempted by StopAll");
+        }
+        if (!resumed) {
+            api::GetSensorDataStreamCommand_Feedback response;
+            response.mutable_header()->set_success(false);
+            response.mutable_header()->set_error_message(
+                "DexHand sensor activity could not be resumed: " + dev_id);
+            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
+            stream->Write(response);
+            return grpc::Status::OK;
+        }
         CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): streaming success, id=" << dev_id;
 
-        while (!context->IsCancelled())
+        while (!media_session.cancelled() &&
+               !(context && context->IsCancelled()))
         {
             api::GetSensorDataStreamCommand_Feedback response;
-            appendSensorData(dev->getSensorData(), &response);
+            std::vector<AbstractDexHand::TactileRegionData> sensor_data;
+            if (!media_session.runIfCurrent(
+                    [&] { sensor_data = dev->getSensorData(); })) {
+                break;
+            }
+            appendSensorData(sensor_data, &response);
             response.mutable_header()->set_success(true);
             setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
 
+            if (media_session.cancelled() ||
+                (context && context->IsCancelled())) {
+                break;
+            }
             if (!stream->Write(response)) {
                 CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (stream->Write) failed,id=" << dev_id;
                 break;

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

@@ -0,0 +1,504 @@
+#include "service/grpc/include/grpc_error_logging_interceptor.h"
+
+#include <atomic>
+#include <chrono>
+#include <cstdint>
+#include <limits>
+#include <mutex>
+#include <optional>
+#include <sstream>
+#include <string_view>
+#include <utility>
+
+#include <google/protobuf/descriptor.h>
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/wire_format_lite.h>
+#include <grpcpp/server_context.h>
+#include <grpcpp/support/interceptor.h>
+#include <grpcpp/support/proto_buffer_reader.h>
+#include <grpcpp/support/status.h>
+
+#include "cmvr/api/common.pb.h"
+#include "common/base/logging/logger.h"
+
+namespace cmvr::service {
+
+namespace {
+
+using Hook = grpc::experimental::InterceptionHookPoints;
+using CodedInputStream = google::protobuf::io::CodedInputStream;
+using WireFormatLite = google::protobuf::internal::WireFormatLite;
+
+constexpr std::string_view kFeedbackType =
+    "cmvr.api.CommandHeader.Feedback";
+constexpr std::size_t kMaxLogDetailBytes = 1024;
+constexpr int kMaxFeedbackBytes = 64 * 1024;
+
+enum class ResponseShape {
+    NONE,
+    DIRECT_FEEDBACK,
+    ENVELOPE,
+};
+
+struct ApplicationFeedback {
+    bool present{false};
+    bool parsed{false};
+    bool success{false};
+    std::string error_message;
+};
+
+ApplicationFeedback feedbackFromProto(
+    const api::CommandHeader_Feedback& message)
+{
+    return ApplicationFeedback{
+        true, true, message.success(), message.error_message()};
+}
+
+std::string boundedDetail(const std::string& detail)
+{
+    if (detail.size() <= kMaxLogDetailBytes) {
+        return detail;
+    }
+
+    constexpr std::string_view suffix = "... [truncated]";
+    std::size_t length = kMaxLogDetailBytes - suffix.size();
+    while (length > 0 &&
+           (static_cast<unsigned char>(detail[length]) & 0xc0U) == 0x80U) {
+        --length;
+    }
+    return detail.substr(0, length) + std::string(suffix);
+}
+
+const char* failureKindName(const GrpcFailureKind kind)
+{
+    switch (kind) {
+    case GrpcFailureKind::APPLICATION:
+        return "application";
+    case GrpcFailureKind::GRPC_STATUS:
+        return "grpc_status";
+    case GrpcFailureKind::MALFORMED_RESPONSE:
+        return "malformed_response";
+    }
+    return "unknown";
+}
+
+const char* statusCodeName(const grpc::StatusCode code)
+{
+    switch (code) {
+    case grpc::StatusCode::OK: return "OK";
+    case grpc::StatusCode::CANCELLED: return "CANCELLED";
+    case grpc::StatusCode::UNKNOWN: return "UNKNOWN";
+    case grpc::StatusCode::INVALID_ARGUMENT: return "INVALID_ARGUMENT";
+    case grpc::StatusCode::DEADLINE_EXCEEDED: return "DEADLINE_EXCEEDED";
+    case grpc::StatusCode::NOT_FOUND: return "NOT_FOUND";
+    case grpc::StatusCode::ALREADY_EXISTS: return "ALREADY_EXISTS";
+    case grpc::StatusCode::PERMISSION_DENIED: return "PERMISSION_DENIED";
+    case grpc::StatusCode::RESOURCE_EXHAUSTED: return "RESOURCE_EXHAUSTED";
+    case grpc::StatusCode::FAILED_PRECONDITION: return "FAILED_PRECONDITION";
+    case grpc::StatusCode::ABORTED: return "ABORTED";
+    case grpc::StatusCode::OUT_OF_RANGE: return "OUT_OF_RANGE";
+    case grpc::StatusCode::UNIMPLEMENTED: return "UNIMPLEMENTED";
+    case grpc::StatusCode::INTERNAL: return "INTERNAL";
+    case grpc::StatusCode::UNAVAILABLE: return "UNAVAILABLE";
+    case grpc::StatusCode::DATA_LOSS: return "DATA_LOSS";
+    case grpc::StatusCode::UNAUTHENTICATED: return "UNAUTHENTICATED";
+    case grpc::StatusCode::DO_NOT_USE: break;
+    }
+    return "UNKNOWN_CODE";
+}
+
+void logFailure(const GrpcFailureRecord& record)
+{
+    std::ostringstream message;
+    message << "[gRPC] request failed, method=" << record.method
+            << ", peer=" << (record.peer.empty() ? "unknown" : record.peer)
+            << ", kind=" << failureKindName(record.kind)
+            << ", code=" << static_cast<int>(record.status_code)
+            << '(' << statusCodeName(record.status_code) << ')'
+            << ", detail=" << boundedDetail(record.detail);
+    switch (record.severity) {
+    case GrpcFailureSeverity::INFO:
+        CMVR_LOG(INFO) << message.str();
+        break;
+    case GrpcFailureSeverity::WARNING:
+        CMVR_LOG(WARNING) << message.str();
+        logging::Logger::instance().flush();
+        break;
+    case GrpcFailureSeverity::ERROR:
+        CMVR_LOG(ERROR) << message.str();
+        break;
+    }
+}
+
+GrpcFailureSeverity severityForStatus(const grpc::StatusCode code)
+{
+    switch (code) {
+    case grpc::StatusCode::CANCELLED:
+    case grpc::StatusCode::INVALID_ARGUMENT:
+    case grpc::StatusCode::DEADLINE_EXCEEDED:
+    case grpc::StatusCode::NOT_FOUND:
+    case grpc::StatusCode::ALREADY_EXISTS:
+    case grpc::StatusCode::PERMISSION_DENIED:
+    case grpc::StatusCode::RESOURCE_EXHAUSTED:
+    case grpc::StatusCode::FAILED_PRECONDITION:
+    case grpc::StatusCode::ABORTED:
+    case grpc::StatusCode::OUT_OF_RANGE:
+    case grpc::StatusCode::UNIMPLEMENTED:
+    case grpc::StatusCode::UNAVAILABLE:
+    case grpc::StatusCode::UNAUTHENTICATED:
+        return GrpcFailureSeverity::WARNING;
+    case grpc::StatusCode::OK:
+    case grpc::StatusCode::UNKNOWN:
+    case grpc::StatusCode::INTERNAL:
+    case grpc::StatusCode::DATA_LOSS:
+    case grpc::StatusCode::DO_NOT_USE:
+        return GrpcFailureSeverity::ERROR;
+    }
+    return GrpcFailureSeverity::ERROR;
+}
+
+bool splitMethodName(const std::string_view full_method,
+                     std::string_view& service_name,
+                     std::string_view& method_name)
+{
+    if (full_method.empty()) {
+        return false;
+    }
+    const std::size_t service_begin = full_method.front() == '/' ? 1 : 0;
+    const std::size_t separator = full_method.find('/', service_begin);
+    if (separator == std::string_view::npos ||
+        separator == service_begin || separator + 1 >= full_method.size()) {
+        return false;
+    }
+    service_name = full_method.substr(service_begin, separator - service_begin);
+    method_name = full_method.substr(separator + 1);
+    return true;
+}
+
+ResponseShape responseShapeForMethod(const std::string_view full_method)
+{
+    std::string_view service_name;
+    std::string_view method_name;
+    if (!splitMethodName(full_method, service_name, method_name)) {
+        return ResponseShape::NONE;
+    }
+
+    const auto* pool = google::protobuf::DescriptorPool::generated_pool();
+    const auto* service = pool->FindServiceByName(std::string(service_name));
+    if (service == nullptr) {
+        return ResponseShape::NONE;
+    }
+    const auto* method = service->FindMethodByName(std::string(method_name));
+    if (method == nullptr || method->output_type() == nullptr) {
+        return ResponseShape::NONE;
+    }
+
+    const auto* output = method->output_type();
+    if (output->full_name() == kFeedbackType) {
+        return ResponseShape::DIRECT_FEEDBACK;
+    }
+
+    const auto* header = output->FindFieldByNumber(1);
+    if (header == nullptr || header->name() != "header" ||
+        header->is_repeated() ||
+        header->cpp_type() != google::protobuf::FieldDescriptor::CPPTYPE_MESSAGE ||
+        header->message_type() == nullptr ||
+        header->message_type()->full_name() != kFeedbackType) {
+        return ResponseShape::NONE;
+    }
+    return ResponseShape::ENVELOPE;
+}
+
+bool mergeFeedback(CodedInputStream& input,
+                   api::CommandHeader_Feedback& feedback)
+{
+    return feedback.MergePartialFromCodedStream(&input) &&
+           input.ConsumedEntireMessage();
+}
+
+bool mergeBoundedFeedback(CodedInputStream& input,
+                          api::CommandHeader_Feedback& feedback)
+{
+    std::uint32_t length = 0;
+    if (!input.ReadVarint32(&length) ||
+        length > static_cast<std::uint32_t>(kMaxFeedbackBytes) ||
+        length > static_cast<std::uint32_t>(std::numeric_limits<int>::max())) {
+        return false;
+    }
+
+    std::string payload;
+    if (!input.ReadString(&payload, static_cast<int>(length))) {
+        return false;
+    }
+    return feedback.MergeFromString(payload);
+}
+
+ApplicationFeedback parseDirectFeedback(grpc::ByteBuffer& buffer)
+{
+    ApplicationFeedback feedback;
+    grpc::ProtoBufferReader reader(&buffer);
+    if (!reader.status().ok()) {
+        return feedback;
+    }
+    CodedInputStream input(&reader);
+    input.SetTotalBytesLimit(kMaxFeedbackBytes);
+    api::CommandHeader_Feedback message;
+    if (!mergeFeedback(input, message)) {
+        feedback.present = true;
+        return feedback;
+    }
+    return feedbackFromProto(message);
+}
+
+ApplicationFeedback parseEnvelopeFeedback(grpc::ByteBuffer& buffer)
+{
+    ApplicationFeedback feedback;
+    grpc::ProtoBufferReader reader(&buffer);
+    if (!reader.status().ok()) {
+        return feedback;
+    }
+    CodedInputStream input(&reader);
+    const std::uint32_t tag = input.ReadTag();
+    if (tag == 0 || WireFormatLite::GetTagFieldNumber(tag) != 1) {
+        feedback.parsed = true;
+        return feedback;
+    }
+
+    feedback.present = true;
+    if (WireFormatLite::GetTagWireType(tag) !=
+            WireFormatLite::WIRETYPE_LENGTH_DELIMITED) {
+        return feedback;
+    }
+
+    api::CommandHeader_Feedback message;
+    if (!mergeBoundedFeedback(input, message)) {
+        return feedback;
+    }
+    return feedbackFromProto(message);
+}
+
+class GrpcErrorLoggingInterceptor final
+    : public grpc::experimental::Interceptor {
+public:
+    GrpcErrorLoggingInterceptor(std::string method,
+                                grpc::ServerContextBase* context,
+                                const ResponseShape response_shape,
+                                const bool server_streaming,
+                                const bool client_streaming,
+                                GrpcFailureSink sink)
+        : method_(std::move(method)),
+          context_(context),
+          peer_(context == nullptr ? std::string{} : context->peer()),
+          response_shape_(response_shape),
+          server_streaming_(server_streaming),
+          client_streaming_(client_streaming),
+          sink_(std::move(sink))
+    {
+    }
+
+    void Intercept(grpc::experimental::InterceptorBatchMethods* methods) override
+    {
+        if (methods->QueryInterceptionHookPoint(Hook::PRE_SEND_CANCEL)) {
+            // gRPC forbids delaying this hook. Only publish the signal here;
+            // the final status hook performs any logging.
+            server_cancel_requested_.store(true, std::memory_order_release);
+            return;
+        }
+        if (methods->QueryInterceptionHookPoint(Hook::PRE_SEND_MESSAGE)) {
+            inspectResponse(methods->GetSerializedSendMessage());
+        }
+        if (methods->QueryInterceptionHookPoint(Hook::PRE_SEND_STATUS)) {
+            inspectStatus(methods->GetSendStatus());
+        }
+        methods->Proceed();
+    }
+
+private:
+    void emit(GrpcFailureRecord record)
+    {
+        record.method = method_;
+        record.peer = peer_;
+        record.detail = boundedDetail(record.detail);
+        if (sink_) {
+            std::lock_guard lock(sink_mutex_);
+            sink_(record);
+        } else {
+            logFailure(record);
+        }
+    }
+
+    void inspectResponse(grpc::ByteBuffer* buffer)
+    {
+        if (response_shape_ == ResponseShape::NONE ||
+            buffer == nullptr || !buffer->Valid()) {
+            return;
+        }
+
+        ApplicationFeedback feedback =
+            response_shape_ == ResponseShape::DIRECT_FEEDBACK
+                ? parseDirectFeedback(*buffer)
+                : parseEnvelopeFeedback(*buffer);
+        std::optional<GrpcFailureRecord> failure;
+        if (!feedback.present) {
+            failure = GrpcFailureRecord{
+                GrpcFailureKind::MALFORMED_RESPONSE,
+                {}, {}, grpc::StatusCode::INTERNAL,
+                GrpcFailureSeverity::ERROR,
+                "response is missing CommandHeader.Feedback"};
+        } else if (!feedback.parsed) {
+            failure = GrpcFailureRecord{
+                GrpcFailureKind::MALFORMED_RESPONSE,
+                {}, {}, grpc::StatusCode::INTERNAL,
+                GrpcFailureSeverity::ERROR,
+                "response contains an invalid CommandHeader.Feedback"};
+        } else if (!feedback.success) {
+            failure = GrpcFailureRecord{
+                GrpcFailureKind::APPLICATION,
+                {}, {}, grpc::StatusCode::OK,
+                GrpcFailureSeverity::ERROR,
+                feedback.error_message.empty()
+                    ? "operation failed without an error message"
+                    : feedback.error_message};
+        }
+
+        if (!failure.has_value()) {
+            return;
+        }
+        if (server_streaming_) {
+            emit(std::move(*failure));
+            return;
+        }
+        pending_failure_ = std::move(*failure);
+    }
+
+    void inspectStatus(const grpc::Status& status)
+    {
+        const grpc::StatusCode status_code = status.error_code();
+        std::string status_detail = status.error_message();
+        if (!status.ok() && status_detail.empty()) {
+            status_detail = "RPC completed with a non-OK status";
+        }
+
+        if (!status.ok()) {
+            // A non-OK unary/client-streaming status suppresses the response
+            // body, so only the status is visible to the platform.
+            pending_failure_.reset();
+            if (status_code == grpc::StatusCode::CANCELLED) {
+                emitCancellationOnce(status_code, std::move(status_detail));
+                return;
+            }
+            emit({GrpcFailureKind::GRPC_STATUS,
+                  {}, {}, status_code,
+                  severityForStatus(status_code),
+                  status_detail});
+            return;
+        }
+
+        const bool deadline_expired =
+            context_ != nullptr &&
+            context_->deadline() <= std::chrono::system_clock::now();
+        // IsCancelled() waits for the client close on the synchronous API.
+        // Unary requests have already sent that close, but querying it here
+        // could block an early client-streaming/bidi response indefinitely.
+        if (deadline_expired ||
+            (context_ != nullptr && !client_streaming_ &&
+             context_->IsCancelled())) {
+            pending_failure_.reset();
+            const grpc::StatusCode code = deadline_expired
+                ? grpc::StatusCode::DEADLINE_EXCEEDED
+                : grpc::StatusCode::CANCELLED;
+            std::string detail;
+            if (deadline_expired) {
+                detail = "RPC deadline expired before completion";
+            } else if (server_cancel_requested_.load(
+                           std::memory_order_acquire)) {
+                detail =
+                    "RPC cancellation was requested by the edge server before completion";
+            } else {
+                detail =
+                    "RPC was cancelled by the client or transport before completion";
+            }
+            emitCancellationOnce(code, std::move(detail));
+            return;
+        }
+
+        if (server_cancel_requested_.load(std::memory_order_acquire)) {
+            pending_failure_.reset();
+            emitCancellationOnce(
+                grpc::StatusCode::CANCELLED,
+                "RPC cancellation was requested by the edge server before completion");
+            return;
+        }
+
+        if (pending_failure_.has_value()) {
+            emit(std::move(*pending_failure_));
+            pending_failure_.reset();
+        }
+    }
+
+    void emitCancellationOnce(const grpc::StatusCode code, std::string detail)
+    {
+        if (cancellation_recorded_.exchange(true, std::memory_order_acq_rel)) {
+            return;
+        }
+        emit({GrpcFailureKind::GRPC_STATUS,
+              {}, {}, code, severityForStatus(code), std::move(detail)});
+    }
+
+    std::string method_;
+    grpc::ServerContextBase* context_{nullptr};
+    std::string peer_;
+    ResponseShape response_shape_{ResponseShape::NONE};
+    bool server_streaming_{false};
+    bool client_streaming_{false};
+    GrpcFailureSink sink_;
+    std::optional<GrpcFailureRecord> pending_failure_;
+    std::mutex sink_mutex_;
+    std::atomic<bool> server_cancel_requested_{false};
+    std::atomic<bool> cancellation_recorded_{false};
+};
+
+class GrpcErrorLoggingInterceptorFactory final
+    : public grpc::experimental::ServerInterceptorFactoryInterface {
+public:
+    explicit GrpcErrorLoggingInterceptorFactory(GrpcFailureSink sink)
+        : sink_(std::move(sink))
+    {
+    }
+
+    grpc::experimental::Interceptor* CreateServerInterceptor(
+        grpc::experimental::ServerRpcInfo* info) override
+    {
+        const std::string method =
+            info != nullptr && info->method() != nullptr
+                ? info->method()
+                : "<unknown>";
+        const bool server_streaming =
+            info != nullptr &&
+            (info->type() == grpc::experimental::ServerRpcInfo::Type::SERVER_STREAMING ||
+             info->type() == grpc::experimental::ServerRpcInfo::Type::BIDI_STREAMING);
+        const bool client_streaming =
+            info != nullptr &&
+            (info->type() == grpc::experimental::ServerRpcInfo::Type::CLIENT_STREAMING ||
+             info->type() == grpc::experimental::ServerRpcInfo::Type::BIDI_STREAMING);
+        return new GrpcErrorLoggingInterceptor(
+            method,
+            info == nullptr ? nullptr : info->server_context(),
+            responseShapeForMethod(method), server_streaming,
+            client_streaming, sink_);
+    }
+
+private:
+    GrpcFailureSink sink_;
+};
+
+} // namespace
+
+std::unique_ptr<grpc::experimental::ServerInterceptorFactoryInterface>
+makeGrpcErrorLoggingInterceptorFactory(GrpcFailureSink sink)
+{
+    return std::make_unique<GrpcErrorLoggingInterceptorFactory>(
+        std::move(sink));
+}
+
+} // namespace cmvr::service

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

@@ -5,9 +5,12 @@
 #include "manager/device_manager/include/device_manager.h"
 #include "common/base/grpc_utils.h"
 #include "biohead/biohead_esp32/include/biohead_esp32.h"
+#include "service/grpc/include/media_activity_coordinator.h"
+#include "service/stop_all/include/stop_all_admission_gate.h"
 #include <chrono>
 #include <algorithm>
 #include <iostream>
+#include <optional>
 
 using namespace std;
 using namespace cmvr::service;
@@ -17,7 +20,6 @@ using namespace cmvr::api;
 namespace {
 template <typename ResponseT>
 grpc::Status failResponse(ResponseT* response, const std::string& message) {
-    CMVR_LOG(ERROR) << "[gRPCMBioHeadServiceImpl] " << message;
     response->mutable_header()->set_success(false);
     response->mutable_header()->set_error_message(message);
     setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@@ -28,6 +30,25 @@ void logSuccess(const char* rpc_name, const std::string& device_id) {
     CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (" << rpc_name
                     << "): success, id=" << device_id;
 }
+
+std::optional<AbstractBiohead::OperationalToken> admitHeadCommand(
+    const std::shared_ptr<AbstractBiohead>& robot)
+{
+    auto admission = globalStopAllAdmissionGate().lockAdmission();
+    if (!admission.accepting() || !robot) {
+        return std::nullopt;
+    }
+    return robot->beginOperationalActivity();
+}
+
+template <typename ResponseT>
+grpc::Status failStoppedCommand(ResponseT* response)
+{
+    return failResponse(
+        response,
+        "Biohead command was rejected because StopAll is in progress or "
+        "the command was preempted");
+}
 }
 
 gRPCMBioHeadServiceImpl::gRPCMBioHeadServiceImpl()
@@ -47,7 +68,12 @@ grpc::Status gRPCMBioHeadServiceImpl::SetExpression(
             return failResponse(response, "Biohead device not found: " + dev_id);
         }
 
-        FacialExpressionState& expression_state = robot->expression_state_;
+        const auto activity = admitHeadCommand(robot);
+        if (!activity) {
+            return failStoppedCommand(response);
+        }
+
+        FacialExpressionState expression_state;
 
         expression_state.left_eyebrow_outside_y = request->expression().eyebrow().left_outside_y();
         expression_state.left_eyebrow_inside_y = request->expression().eyebrow().left_inside_y();
@@ -70,7 +96,10 @@ grpc::Status gRPCMBioHeadServiceImpl::SetExpression(
         expression_state.upper_lip_y = request->expression().mouth().upper_lip_y();
         expression_state.lower_lip_y = request->expression().mouth().lower_lip_y();
 
-        robot->setExpressionPose(expression_state);
+        if (!robot->setExpressionPoseIfCurrent(
+                *activity, expression_state)) {
+            return failStoppedCommand(response);
+        }
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@@ -94,12 +123,25 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression(
     std::string dev_id;
     std::shared_ptr<AbstractBiohead> robot;
     bool first_message = true;
+    AbstractBiohead::OperationalToken activity{0U};
+    auto media_session = globalMediaActivityCoordinator().beginSession(
+        [context] {
+            if (context) {
+                context->TryCancel();
+            }
+        });
+    if (!media_session) {
+        return grpc::Status(
+            grpc::StatusCode::UNAVAILABLE,
+            "Biohead stream rejected because StopAll is in progress");
+    }
 
     try {
         StreamFacialExpression_Request request_msg;
         constexpr float control_frequency = 10;
         const auto time_interval = std::chrono::milliseconds(static_cast<int>(1000 / control_frequency));
-        auto last_control_time = std::chrono::steady_clock::now();
+        auto last_control_time =
+            std::chrono::steady_clock::now() - time_interval;
 
         CMVR_LOG(INFO) << "StreamExpression started.";
 
@@ -107,7 +149,6 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression(
             if (first_message) {
                 dev_id = request_msg.header().device_id();
                 if (dev_id.empty()) {
-                    CMVR_LOG(ERROR) << "[gRPCMBioHeadServiceImpl] Device ID is empty in first message";
                     feedback_msg.mutable_header()->set_success(false);
                     feedback_msg.mutable_header()->set_error_message("Device ID is empty in first message");
                     setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp());
@@ -117,7 +158,6 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression(
                 robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
                 if (!robot) {
                     const std::string message = "Biohead device not found: " + dev_id;
-                    CMVR_LOG(ERROR) << "[gRPCMBioHeadServiceImpl] " << message;
                     feedback_msg.mutable_header()->set_success(false);
                     feedback_msg.mutable_header()->set_error_message(message);
                     setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp());
@@ -125,15 +165,26 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression(
                     return grpc::Status::OK;
                 }
 
-                // ✅ 重置紧急停止标志
-                robot->emergency_stop_requested = false;
+                const auto admitted = admitHeadCommand(robot);
+                if (!admitted) {
+                    feedback_msg.mutable_header()->set_success(false);
+                    feedback_msg.mutable_header()->set_error_message(
+                        "Biohead stream rejected because StopAll is in "
+                        "progress");
+                    setCurrentTimestamp(
+                        feedback_msg.mutable_header()->mutable_timestamp());
+                    stream->Write(feedback_msg);
+                    return grpc::Status::OK;
+                }
+                activity = *admitted;
 
                 first_message = false;
                 CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (StreamExpression): streaming success, id=" << dev_id;
             }
 
             // ✅ 如果紧急停止触发，直接退出
-            if (robot->emergency_stop_requested) {
+            if (media_session.cancelled() ||
+                (context && context->IsCancelled())) {
                 CMVR_LOG(WARNING) << "[Stream] Emergency stop requested. Terminating stream for device: " << dev_id;
                 break;
             }
@@ -181,7 +232,18 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression(
             expression_state.jaw_x = request_msg.expr().jaw().x();
             expression_state.jaw_y = request_msg.expr().jaw().y();
 
-            robot->streamFacialPose(expression_state, 0, 0);
+            bool dispatched = false;
+            const bool current_session = media_session.runIfCurrent([&] {
+                dispatched = robot->streamFacialPoseIfCurrent(
+                    activity, expression_state, 0, 0);
+            });
+            if (!current_session || !dispatched) {
+                CMVR_LOG(WARNING)
+                    << "[gRPCMBioHeadServiceImpl] StreamExpression was "
+                       "preempted, id="
+                    << dev_id;
+                break;
+            }
             last_control_time = current_time;
 
             feedback_msg.mutable_header()->set_success(true);
@@ -194,7 +256,6 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression(
         CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (StreamExpression): finished, id=" << dev_id;
         return grpc::Status::OK;
     } catch (const std::exception& e) {
-        CMVR_LOG(ERROR) << "StreamExpression error: " << e.what();
         feedback_msg.mutable_header()->set_success(false);
         feedback_msg.mutable_header()->set_error_message(e.what());
         setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp());
@@ -245,13 +306,12 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop(
             return failResponse(response, "Biohead device not found: " + dev_id);
         }
 
-        robot->eStop();  // 停止执行
-        robot->emergency_stop_requested = true;  // ✅ 设置中断标志
-
-
-
-
-
+        if (!robot->stopOperationalActivity()) {
+            return failResponse(
+                response,
+                "Biohead could not confirm that operational activity "
+                "stopped: " + dev_id);
+        }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
         logSuccess("EmergencyStop", dev_id);
@@ -279,7 +339,10 @@ grpc::Status gRPCMBioHeadServiceImpl::SpeakStart(grpc::ServerContext* context, c
                 return failResponse(response, "Biohead device not found: " + dev_id);
             }
 
-            robot->speakstart();  // kaish开始
+            const auto activity = admitHeadCommand(robot);
+            if (!activity || !robot->speakStartIfCurrent(*activity)) {
+                return failStoppedCommand(response);
+            }
 
 
             response->mutable_header()->set_success(true);
@@ -333,7 +396,10 @@ grpc::Status gRPCMBioHeadServiceImpl::Happy(grpc::ServerContext* context, const
             return failResponse(response, "Biohead device not found: " + dev_id);
         }
 
-        robot->expressionHappy();  // 停止执行
+        const auto activity = admitHeadCommand(robot);
+        if (!activity || !robot->expressionHappyIfCurrent(*activity)) {
+            return failStoppedCommand(response);
+        }
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@@ -357,7 +423,10 @@ grpc::Status gRPCMBioHeadServiceImpl::Surprise(grpc::ServerContext* context, con
             return failResponse(response, "Biohead device not found: " + dev_id);
         }
 
-        robot->expressionSurprised();  //
+        const auto activity = admitHeadCommand(robot);
+        if (!activity || !robot->expressionSurprisedIfCurrent(*activity)) {
+            return failStoppedCommand(response);
+        }
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@@ -382,7 +451,10 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionTired(grpc::ServerContext* conte
             return failResponse(response, "Biohead device not found: " + dev_id);
         }
 
-        robot->expressionTired();  // 停止执行
+        const auto activity = admitHeadCommand(robot);
+        if (!activity || !robot->expressionTiredIfCurrent(*activity)) {
+            return failStoppedCommand(response);
+        }
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@@ -408,7 +480,10 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionAngry(grpc::ServerContext* conte
             return failResponse(response, "Biohead device not found: " + dev_id);
         }
 
-        robot->expressionAngry();  // 停止执行
+        const auto activity = admitHeadCommand(robot);
+        if (!activity || !robot->expressionAngryIfCurrent(*activity)) {
+            return failStoppedCommand(response);
+        }
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@@ -434,7 +509,10 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionSadness(grpc::ServerContext* con
             return failResponse(response, "Biohead device not found: " + dev_id);
         }
 
-        robot->expressionSadness();  // 停止执行
+        const auto activity = admitHeadCommand(robot);
+        if (!activity || !robot->expressionSadnessIfCurrent(*activity)) {
+            return failStoppedCommand(response);
+        }
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@@ -459,7 +537,10 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionYawn(grpc::ServerContext* contex
             return failResponse(response, "Biohead device not found: " + dev_id);
         }
 
-        robot->expressionYawn();  // 停止执行
+        const auto activity = admitHeadCommand(robot);
+        if (!activity || !robot->expressionYawnIfCurrent(*activity)) {
+            return failStoppedCommand(response);
+        }
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());

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

@@ -13,6 +13,7 @@
 
 #include "common/base/logging/logger.h"
 #include "manager/task_manager/include/task_manager.h"
+#include "service/stop_all/include/stop_all_admission_gate.h"
 #include "task/touch_screen_task/include/touch_screen_task.h"
 
 
@@ -51,7 +52,26 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
             return grpc::Status(grpc::StatusCode::NOT_FOUND, error);
         }
 
-        if (!touch_task->touch(request->u(), request->v())) {
+        auto& admission_gate = globalStopAllAdmissionGate();
+        std::uint64_t admission_generation = 0U;
+        {
+            auto admission = admission_gate.lockAdmission();
+            if (!admission.accepting()) {
+                const std::string error =
+                    "TouchScreenTask is temporarily paused by StopAll";
+                fillTouchResponse(response, false, error);
+                return grpc::Status(grpc::StatusCode::UNAVAILABLE, error);
+            }
+            admission_generation = admission.generation();
+        }
+
+        if (!touch_task->touchIfCurrent(
+                request->u(), request->v(),
+                [&admission_gate, admission_generation] {
+                    auto admission = admission_gate.lockAdmission();
+                    return admission.accepting() &&
+                           admission.generation() == admission_generation;
+                })) {
             const std::string error =
                 buildTouchFailureMessage(*touch_task, "TouchScreenTask touch request rejected");
             fillTouchResponse(response, false, error);

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

@@ -1,5 +1,6 @@
 #include "common/base/logging/logger.h"
 #include "manager/media_source_hub/include/device_media_source_adapter.h"
+#include "service/grpc/include/media_activity_coordinator.h"
 #include <algorithm>
 #include <chrono>
 #include <cstdint>
@@ -17,13 +18,19 @@ using namespace cmvr::service;
 namespace {
 template <typename ResponseT>
 grpc::Status failResponse(ResponseT* response, const std::string& message) {
-    CMVR_LOG(ERROR) << "[gRPCMicroPhoneServiceImpl] " << message;
     response->mutable_header()->set_success(false);
     response->mutable_header()->set_error_message(message);
     setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
     return grpc::Status::OK;
 }
 
+grpc::Status mediaStoppedStatus()
+{
+    return grpc::Status(
+        grpc::StatusCode::CANCELLED,
+        "Media activity stopped by StopAll");
+}
+
 }
 
 gRPCMicroPhoneServiceImpl::gRPCMicroPhoneServiceImpl(): dmgr_(DeviceManager::getInstance()) {}
@@ -63,6 +70,11 @@ grpc::Status gRPCMicroPhoneServiceImpl::GetStatus(grpc::ServerContext* context,
 
 grpc::Status gRPCMicroPhoneServiceImpl::StartRecord(grpc::ServerContext* context,
     const api::StartMicRecordingCommand_Request* request, api::StartMicRecordingCommand_Feedback* response) {
+    auto media_session = globalMediaActivityCoordinator().beginSession();
+    if (!media_session) {
+        return failResponse(
+            response, "Media activities are temporarily paused by StopAll");
+    }
     try {
         string dev_id = request->header().device_id();
         CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StartRecord): id=" << dev_id;
@@ -70,10 +82,20 @@ grpc::Status gRPCMicroPhoneServiceImpl::StartRecord(grpc::ServerContext* context
         if (!dev) {
             return failResponse(response, "Microphone device not found: " + dev_id);
         }
-        if (!dev->start()) {
+        bool started = false;
+        const bool start_allowed = media_session.runIfCurrent([&] {
+            started = dev->start();
+            if (started) {
+                dev->startRecording(request->file_path());
+            }
+        });
+        if (!start_allowed) {
+            return failResponse(
+                response, "Microphone recording start was canceled by StopAll");
+        }
+        if (!started) {
             return failResponse(response, "Failed to start microphone: " + dev_id);
         }
-        dev->startRecording(request->file_path());
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
         CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (StartRecord): success, id=" << dev_id
@@ -136,6 +158,11 @@ grpc::Status gRPCMicroPhoneServiceImpl::PauseRecord(grpc::ServerContext* context
 
 grpc::Status gRPCMicroPhoneServiceImpl::ResumeRecord(grpc::ServerContext* context,
     const api::ResumeMicRecordingCommand_Request* request, api::ResumeMicRecordingCommand_Feedback* response) {
+    auto media_session = globalMediaActivityCoordinator().beginSession();
+    if (!media_session) {
+        return failResponse(
+            response, "Media activities are temporarily paused by StopAll");
+    }
     try {
         string dev_id = request->header().device_id();
         CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (ResumeRecord): id=" << dev_id;
@@ -143,7 +170,10 @@ grpc::Status gRPCMicroPhoneServiceImpl::ResumeRecord(grpc::ServerContext* contex
         if (!dev) {
             return failResponse(response, "Microphone device not found: " + dev_id);
         }
-        dev->resume();
+        if (!media_session.runIfCurrent([&] { dev->resume(); })) {
+            return failResponse(
+                response, "Microphone recording resume was canceled by StopAll");
+        }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
         CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (ResumeRecord): success, id=" << dev_id;
@@ -160,6 +190,17 @@ grpc::Status gRPCMicroPhoneServiceImpl::ResumeRecord(grpc::ServerContext* contex
 grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context,
     const api::StreamMicAudioCommand_Request* request,
     grpc::ServerWriter<api::StreamMicAudioCommand_Feedback>* writer) {
+    auto media_session = globalMediaActivityCoordinator().beginSession(
+        [context] { context->TryCancel(); });
+    if (!media_session) {
+        api::StreamMicAudioCommand_Feedback feedback;
+        feedback.mutable_header()->set_success(false);
+        feedback.mutable_header()->set_error_message(
+            "Media activities are temporarily paused by StopAll");
+        setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp());
+        writer->Write(feedback);
+        return grpc::Status::OK;
+    }
     try {
         const string dev_id = request->header().device_id();
         CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StreamAudio): id=" << dev_id;
@@ -175,10 +216,18 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context
 
         auto& media_hub = cmvr::media::globalMediaSourceHub();
         const std::string track_id = cmvr::media::microphoneTrackId(dev_id);
-        if (!cmvr::media::ensureMicrophoneMediaSource(media_hub, dev)) {
+        bool source_ready = false;
+        const bool source_setup_allowed = media_session.runIfCurrent([&] {
+            source_ready =
+                cmvr::media::ensureMicrophoneMediaSource(media_hub, dev);
+        });
+        if (!source_setup_allowed || !source_ready) {
             api::StreamMicAudioCommand_Feedback feedback;
             feedback.mutable_header()->set_success(false);
-            feedback.mutable_header()->set_error_message("Failed to register microphone media source: " + dev_id);
+            feedback.mutable_header()->set_error_message(
+                media_session.cancelled()
+                    ? "Microphone stream start was canceled by StopAll"
+                    : "Failed to register microphone media source: " + dev_id);
             setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp());
             writer->Write(feedback);
             return grpc::Status::OK;
@@ -187,7 +236,9 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context
         auto subscription = media_hub.subscribe(
             track_id,
             cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED,
-            [context] { return context->IsCancelled(); });
+            [context, &media_session] {
+                return context->IsCancelled() || media_session.cancelled();
+            });
         if (!subscription) {
             api::StreamMicAudioCommand_Feedback feedback;
             feedback.mutable_header()->set_success(false);
@@ -197,8 +248,11 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context
             return grpc::Status::OK;
         }
 
-        while (!context->IsCancelled()) {
+        while (!media_session.cancelled() && !context->IsCancelled()) {
             const auto read = subscription.waitRead(std::chrono::milliseconds(100));
+            if (media_session.cancelled()) {
+                break;
+            }
             if (!read || !read->value || read->value->empty()) {
                 if (!subscription.valid()) {
                     break;
@@ -236,7 +290,9 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context
                 feedback.mutable_header()->set_error_message(
                     "Unsupported microphone stream codec: " + dev_id);
                 feedback.clear_audio();
-                writer->Write(feedback);
+                if (!media_session.cancelled()) {
+                    writer->Write(feedback);
+                }
                 break;
             }
             audio->set_pts(frame.pts);
@@ -247,12 +303,17 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context
                 sample_count,
                 0,
                 std::numeric_limits<int32_t>::max())));
-            if (!writer->Write(feedback)) {
+            if (media_session.cancelled() || !writer->Write(feedback)) {
                 break;
             }
         }
-        return grpc::Status::OK;
+        return media_session.cancelled()
+            ? mediaStoppedStatus()
+            : grpc::Status::OK;
     } catch (const std::exception& error) {
+        if (media_session.cancelled()) {
+            return mediaStoppedStatus();
+        }
         api::StreamMicAudioCommand_Feedback feedback;
         feedback.mutable_header()->set_success(false);
         feedback.mutable_header()->set_error_message(error.what());

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

@@ -15,6 +15,7 @@
 #include "common/base/logging/logger.h"
 #include "devices/motor/manager/include/motor_manager.h"
 #include "manager/device_manager/include/device_manager.h"
+#include "service/stop_all/include/stop_all_admission_gate.h"
 
 namespace cmvr::service {
 
@@ -386,7 +387,7 @@ grpc::Status runCyclicLoop(
             }
             if (is_preempted(generation)) {
                 const std::string error =
-                    "cyclic stream preempted by emergency stop";
+                    "cyclic stream preempted by a stop request";
                 set_last_error(error);
                 const bool stopped = safeStop();
                 joinReader(true);
@@ -529,17 +530,21 @@ gRPCMotorServiceImpl::ControlLease::~ControlLease()
     if (!state_) {
         return;
     }
-    std::lock_guard lock(state_->mutex);
-    if (std::uncaught_exceptions() > uncaught_on_entry_) {
-        // Keep ownership reserved until the public RPC exception barrier has
-        // completed its best-effort stop. This closes the window where a new
-        // RPC could acquire the motor between stack unwinding and cleanup.
-        state_->exception_cleanup_pending = true;
-        ++state_->cancel_generation;
-        return;
+    {
+        std::lock_guard lock(state_->mutex);
+        if (std::uncaught_exceptions() > uncaught_on_entry_) {
+            // Keep ownership reserved until the public RPC exception barrier
+            // has completed its best-effort stop. This closes the window where
+            // a new RPC could acquire the motor between stack unwinding and
+            // cleanup.
+            state_->exception_cleanup_pending = true;
+            ++state_->cancel_generation;
+        } else {
+            state_->busy = false;
+            state_->active_control = api::MOTOR_CONTROL_NONE;
+        }
     }
-    state_->busy = false;
-    state_->active_control = api::MOTOR_CONTROL_NONE;
+    globalMotorActivityCoordinator().notifyStateChanged();
 }
 
 gRPCMotorServiceImpl::gRPCMotorServiceImpl()
@@ -570,15 +575,74 @@ gRPCMotorServiceImpl::stateFor(
     }
 
     auto state = std::make_shared<MotorControlState>();
+    const std::weak_ptr<MotorControlState> weak_state = state;
+    const std::weak_ptr<device::AbstractMotor> weak_motor = motor;
+    auto registration = globalMotorActivityCoordinator().registerControl(
+        [weak_state]() {
+            if (const auto state = weak_state.lock()) {
+                std::lock_guard state_lock(state->mutex);
+                ++state->cancel_generation;
+                state->last_error = "motor command preempted by System StopAll";
+            }
+        },
+        [weak_state, weak_motor]() {
+            const auto state = weak_state.lock();
+            const auto motor = weak_motor.lock();
+            if (!state || !motor) {
+                return true;
+            }
+
+            bool stopped = false;
+            try {
+                // This confirmed stop is ordered after any device write that
+                // passed its generation check before StopAll invalidated it.
+                std::lock_guard command_lock(state->command_mutex);
+                stopped = motor->quickStop();
+            } catch (...) {
+                stopped = false;
+            }
+            if (!stopped) {
+                std::lock_guard state_lock(state->mutex);
+                state->last_error =
+                    "System StopAll could not confirm motor quick-stop";
+            }
+            return stopped;
+        },
+        [weak_state]() {
+            const auto state = weak_state.lock();
+            if (!state) {
+                return true;
+            }
+            std::lock_guard state_lock(state->mutex);
+            return !state->busy && !state->exception_cleanup_pending;
+        },
+        "motor " + std::to_string(motor->id()) + " (" +
+            motor->jointName() + ")");
     states_.emplace(
-        motor.get(), MotorControlEntry{std::weak_ptr<device::AbstractMotor>(motor),
-                                       state});
+        motor.get(),
+        MotorControlEntry{
+            std::weak_ptr<device::AbstractMotor>(motor), state,
+            std::move(registration)});
     return state;
 }
 
+std::shared_ptr<gRPCMotorServiceImpl::MotorControlState>
+gRPCMotorServiceImpl::existingStateFor(
+    const std::shared_ptr<device::AbstractMotor>& motor) const
+{
+    std::lock_guard lock(states_mutex_);
+    const auto existing = states_.find(motor.get());
+    if (existing == states_.end()) {
+        return nullptr;
+    }
+    const auto owner = existing->second.owner.lock();
+    return owner && owner == motor ? existing->second.state : nullptr;
+}
+
 grpc::Status gRPCMotorServiceImpl::resolveMotor(
     const api::MotorTarget& target,
-    ResolvedMotor& resolved) const
+    ResolvedMotor& resolved,
+    const ResolveAccess access) const
 {
     const std::string& manager_id = target.header().device_id();
     if (manager_id.empty()) {
@@ -617,7 +681,19 @@ grpc::Status gRPCMotorServiceImpl::resolveMotor(
         return grpc::Status(grpc::StatusCode::NOT_FOUND,
                             "motor not found in MotorManager: " + manager_id);
     }
-    resolved.control = stateFor(resolved.motor);
+    const auto arm_owner = manager->armOwnerForJoint(
+        resolved.motor->jointName());
+    if (access == ResolveAccess::Control && !arm_owner.empty()) {
+        return grpc::Status(
+            grpc::StatusCode::FAILED_PRECONDITION,
+            "motor joint is controlled by RobotArm '" + arm_owner +
+                "'; use ArmService for control: " +
+                resolved.motor->jointName());
+    }
+    resolved.control =
+        access == ResolveAccess::Observe && !arm_owner.empty()
+            ? existingStateFor(resolved.motor)
+            : stateFor(resolved.motor);
     return grpc::Status::OK;
 }
 
@@ -628,6 +704,15 @@ gRPCMotorServiceImpl::acquireControl(
     grpc::Status& failure,
     const bool allow_emergency_stopped) const
 {
+    auto system_admission = globalStopAllAdmissionGate().lockAdmission();
+    auto motor_admission = globalMotorActivityCoordinator().lockAdmission();
+    if (!system_admission.accepting() || !motor_admission.accepting()) {
+        failure = grpc::Status(
+            grpc::StatusCode::ABORTED,
+            "motor control admission is paused by System StopAll");
+        return nullptr;
+    }
+
     std::lock_guard lock(resolved.control->mutex);
     if (resolved.control->busy) {
         failure = grpc::Status(grpc::StatusCode::RESOURCE_EXHAUSTED,
@@ -717,6 +802,7 @@ void gRPCMotorServiceImpl::bestEffortQuickStop(
             resolved.control->busy = false;
             resolved.control->active_control = api::MOTOR_CONTROL_NONE;
         }
+        globalMotorActivityCoordinator().notifyStateChanged();
     } catch (...) {
     }
 }
@@ -729,7 +815,7 @@ void gRPCMotorServiceImpl::fillMotorStatus(
     bool emergency_stopped = false;
     api::MotorControlType active_control = api::MOTOR_CONTROL_NONE;
     std::string last_error;
-    {
+    if (resolved.control) {
         std::lock_guard lock(resolved.control->mutex);
         busy = resolved.control->busy;
         emergency_stopped = resolved.control->emergency_stopped;
@@ -921,7 +1007,7 @@ grpc::Status gRPCMotorServiceImpl::setZeroImpl(
         }
         if (preempted) {
             const std::string error =
-                "zero calibration preempted by emergency stop";
+                "zero calibration preempted by a stop request";
             lease.reset();
             fillFeedback(response->mutable_header(), false, error);
             fillMotorStatus(resolved, response->mutable_status());
@@ -1038,7 +1124,7 @@ grpc::Status gRPCMotorServiceImpl::runProfilePosition(
         }
         if (preempted) {
             const std::string error =
-                "profile position preempted by emergency stop";
+                "profile position preempted by a stop request";
             lease.reset();
             fillFeedback(response->mutable_header(), false, error);
             fillMotorStatus(resolved, response->mutable_status());
@@ -1085,7 +1171,7 @@ grpc::Status gRPCMotorServiceImpl::runProfilePosition(
         }
         if (preempted_during_dispatch) {
             const std::string error =
-                "profile position rejected while being preempted by emergency stop";
+                "profile position rejected while being preempted by a stop request";
             setLastError(resolved.control, error);
             lease.reset();
             fillFeedback(response->mutable_header(), false, error);
@@ -1157,7 +1243,8 @@ grpc::Status gRPCMotorServiceImpl::waitForPosition(
             preempted = resolved.control->cancel_generation != generation;
         }
         if (preempted) {
-            const std::string error = "profile position preempted by emergency stop";
+            const std::string error =
+                "profile position preempted by a stop request";
             setLastError(resolved.control, error);
             fillFeedback(response->mutable_header(), false, error);
             response->set_elapsed_ms(elapsedMs(started));
@@ -1320,7 +1407,8 @@ grpc::Status gRPCMotorServiceImpl::waitForVelocity(
             preempted = resolved.control->cancel_generation != generation;
         }
         if (preempted) {
-            const std::string error = "profile velocity preempted by emergency stop";
+            const std::string error =
+                "profile velocity preempted by a stop request";
             setLastError(resolved.control, error);
             fillFeedback(response->mutable_header(), false, error);
             response->set_elapsed_ms(elapsedMs(started));
@@ -1483,7 +1571,7 @@ grpc::Status gRPCMotorServiceImpl::profileVelocityImpl(
         }
         if (preempted) {
             const std::string error =
-                "profile velocity preempted by emergency stop";
+                "profile velocity preempted by a stop request";
             lease.reset();
             fillFeedback(response->mutable_header(), false, error);
             fillMotorStatus(resolved, response->mutable_status());
@@ -1531,7 +1619,7 @@ grpc::Status gRPCMotorServiceImpl::profileVelocityImpl(
         }
         if (preempted_during_dispatch) {
             const std::string error =
-                "profile velocity rejected while being preempted by emergency stop";
+                "profile velocity rejected while being preempted by a stop request";
             setLastError(resolved.control, error);
             lease.reset();
             fillFeedback(response->mutable_header(), false, error);
@@ -1620,7 +1708,7 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicPositionImpl(
         if (preempted) {
             return grpc::Status(
                 grpc::StatusCode::ABORTED,
-                "cyclic position open preempted by emergency stop");
+                "cyclic position open preempted by a stop request");
         }
         resolved.motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
         std::lock_guard state_lock(resolved.control->mutex);
@@ -1645,7 +1733,7 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicPositionImpl(
                 if (resolved.control->cancel_generation != generation) {
                     return grpc::Status(
                         grpc::StatusCode::ABORTED,
-                        "cyclic position setpoint preempted by emergency stop");
+                        "cyclic position setpoint preempted by a stop request");
                 }
             }
             if (!isFinite(setpoint.target_position_rad()) ||
@@ -1743,7 +1831,7 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicVelocityImpl(
         if (preempted) {
             return grpc::Status(
                 grpc::StatusCode::ABORTED,
-                "cyclic velocity open preempted by emergency stop");
+                "cyclic velocity open preempted by a stop request");
         }
         resolved.motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
         std::lock_guard state_lock(resolved.control->mutex);
@@ -1768,7 +1856,7 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicVelocityImpl(
                 if (resolved.control->cancel_generation != generation) {
                     return grpc::Status(
                         grpc::StatusCode::ABORTED,
-                        "cyclic velocity setpoint preempted by emergency stop");
+                        "cyclic velocity setpoint preempted by a stop request");
                 }
             }
             if (!isFinite(setpoint.target_velocity_rad_s())) {
@@ -1884,7 +1972,8 @@ grpc::Status gRPCMotorServiceImpl::getStatusImpl(
     api::GetMotorStatusResponse* response)
 {
     ResolvedMotor resolved;
-    auto status = resolveMotor(request->target(), resolved);
+    auto status = resolveMotor(
+        request->target(), resolved, ResolveAccess::Observe);
     if (!status.ok()) {
         fillFeedback(response->mutable_header(), false, status.error_message());
         return status;
@@ -1949,7 +2038,7 @@ grpc::Status gRPCMotorServiceImpl::setEnabledImpl(
         }
         if (preempted) {
             const std::string error =
-                "enable/disable preempted by emergency stop";
+                "enable/disable preempted by a stop request";
             lease.reset();
             fillFeedback(response->mutable_header(), false, error);
             fillMotorStatus(resolved, response->mutable_status());
@@ -2012,7 +2101,7 @@ grpc::Status gRPCMotorServiceImpl::setEnabledImpl(
     }
     if (preempted_after_dispatch) {
         const std::string error =
-            "enable/disable preempted by emergency stop during dispatch";
+            "enable/disable preempted by a stop request during dispatch";
         setLastError(resolved.control, error);
         lease.reset();
         fillFeedback(response->mutable_header(), false, error);

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

@@ -1,4 +1,5 @@
 #include "common/base/logging/logger.h"
+#include "service/grpc/include/media_activity_coordinator.h"
 #include <memory>
 //
 // Created by xtkuang on 2025/6/10.
@@ -13,7 +14,6 @@ using namespace cmvr::service;
 namespace {
 template <typename ResponseT>
 grpc::Status failResponse(ResponseT* response, const std::string& message) {
-    CMVR_LOG(ERROR) << "[gRPCSpeakerServiceImpl] " << message;
     response->mutable_header()->set_success(false);
     response->mutable_header()->set_error_message(message);
     setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@@ -46,6 +46,52 @@ AudioStreamFrameData fromProtoAudioData(const cmvr::api::AudioData& audio) {
     frame.nb_samples = audio.nb_samples();
     return frame;
 }
+
+std::string speakerResourceKey(const std::string& device_id)
+{
+    return "speaker:" + device_id;
+}
+
+grpc::Status mediaStoppedStatus()
+{
+    return grpc::Status(
+        grpc::StatusCode::CANCELLED,
+        "Media activity stopped by StopAll");
+}
+
+class SpeakerStreamingLease final {
+public:
+    explicit SpeakerStreamingLease(std::shared_ptr<AbstractSpeaker> speaker)
+        : speaker_(std::move(speaker)) {}
+
+    ~SpeakerStreamingLease()
+    {
+        if (!armed_ || !speaker_) {
+            return;
+        }
+        try {
+            speaker_->stopStreaming();
+        } catch (const std::exception& error) {
+            CMVR_LOG(ERROR)
+                << "[gRPCSpeakerServiceImpl] failed to release speaker "
+                   "stream session: "
+                << error.what();
+        } catch (...) {
+            CMVR_LOG(ERROR)
+                << "[gRPCSpeakerServiceImpl] failed to release speaker "
+                   "stream session";
+        }
+    }
+
+    SpeakerStreamingLease(const SpeakerStreamingLease&) = delete;
+    SpeakerStreamingLease& operator=(const SpeakerStreamingLease&) = delete;
+
+    void arm() noexcept { armed_ = true; }
+
+private:
+    std::shared_ptr<AbstractSpeaker> speaker_;
+    bool armed_{false};
+};
 }
 
 gRPCSpeakerServiceImpl::gRPCSpeakerServiceImpl(): dmgr_(DeviceManager::getInstance()) {}
@@ -86,6 +132,11 @@ grpc::Status gRPCSpeakerServiceImpl::GetStatus(grpc::ServerContext* context,
 
 grpc::Status gRPCSpeakerServiceImpl::PlayAudio(grpc::ServerContext* context,
     const api::PlayAudioCommand_Request* request, api::PlayAudioCommand_Feedback* response) {
+    auto media_session = globalMediaActivityCoordinator().beginSession();
+    if (!media_session) {
+        return failResponse(
+            response, "Media activities are temporarily paused by StopAll");
+    }
     try {
         string dev_id = request->header().device_id();
         CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (PlayAudio): id=" << dev_id;
@@ -93,8 +144,16 @@ grpc::Status gRPCSpeakerServiceImpl::PlayAudio(grpc::ServerContext* context,
         if (!dev) {
             return failResponse(response, "Speaker device not found: " + dev_id);
         }
-        //dev->start();
-        dev->play(request->audio_path());
+        if (!media_session.claimExclusiveResource(speakerResourceKey(dev_id))) {
+            return failResponse(
+                response, "Speaker is already controlled by another media session: " + dev_id);
+        }
+        if (!media_session.runIfCurrent([&] {
+                dev->play(request->audio_path());
+            })) {
+            return failResponse(
+                response, "Speaker playback start was canceled by StopAll");
+        }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
         CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (PlayAudio): success, id=" << dev_id
@@ -112,12 +171,22 @@ grpc::Status gRPCSpeakerServiceImpl::PlayAudio(grpc::ServerContext* context,
 grpc::Status gRPCSpeakerServiceImpl::StreamAudio(grpc::ServerContext* context,
     grpc::ServerReader<api::StreamSpeakerAudioCommand_Request>* reader,
     api::StreamSpeakerAudioCommand_Feedback* response) {
+    auto media_session = globalMediaActivityCoordinator().beginSession(
+        [context] { context->TryCancel(); });
+    if (!media_session) {
+        return failResponse(
+            response, "Media activities are temporarily paused by StopAll");
+    }
     try {
         api::StreamSpeakerAudioCommand_Request request;
         std::shared_ptr<AbstractSpeaker> dev;
+        std::unique_ptr<SpeakerStreamingLease> stream_lease;
         std::string dev_id;
 
         while (reader->Read(&request)) {
+            if (media_session.cancelled()) {
+                return mediaStoppedStatus();
+            }
             if (!dev) {
                 dev_id = request.header().device_id();
                 CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (StreamAudio): id=" << dev_id;
@@ -125,25 +194,42 @@ grpc::Status gRPCSpeakerServiceImpl::StreamAudio(grpc::ServerContext* context,
                 if (!dev) {
                     return failResponse(response, "Speaker device not found: " + dev_id);
                 }
-                if (!dev->start()) {
-                    return failResponse(response, "Failed to start speaker: " + dev_id);
+                if (!media_session.claimExclusiveResource(
+                        speakerResourceKey(dev_id))) {
+                    return failResponse(
+                        response,
+                        "Speaker is already controlled by another media session: " + dev_id);
                 }
+                stream_lease = std::make_unique<SpeakerStreamingLease>(dev);
             }
 
-            if (!dev->pushAudioFrame(fromProtoAudioData(request.audio()))) {
-                dev->stopStreaming();
+            const auto frame = fromProtoAudioData(request.audio());
+            bool pushed = false;
+            const bool push_allowed = media_session.runIfCurrent([&] {
+                if (!frame.data.empty()) {
+                    stream_lease->arm();
+                }
+                pushed = dev->pushAudioFrame(frame);
+            });
+            if (!push_allowed || media_session.cancelled()) {
+                return mediaStoppedStatus();
+            }
+            if (!pushed) {
                 return failResponse(response, "Failed to push speaker audio frame: " + dev_id);
             }
         }
 
-        if (dev) {
-            dev->stopStreaming();
+        if (media_session.cancelled()) {
+            return mediaStoppedStatus();
         }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
+        if (media_session.cancelled()) {
+            return mediaStoppedStatus();
+        }
         response->mutable_header()->set_success(false);
         response->mutable_header()->set_error_message(e.what());
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@@ -160,7 +246,7 @@ grpc::Status gRPCSpeakerServiceImpl::StopPlayback(grpc::ServerContext* context,
         if (!dev) {
             return failResponse(response, "Speaker device not found: " + dev_id);
         }
-        if (!dev->stop()) {
+        if (!dev->stopPlayback()) {
             return failResponse(response, "Failed to stop speaker: " + dev_id);
         }
         response->mutable_header()->set_success(true);
@@ -201,6 +287,11 @@ grpc::Status gRPCSpeakerServiceImpl::PausePlayback(grpc::ServerContext* context,
 
 grpc::Status gRPCSpeakerServiceImpl::ResumePlayback(grpc::ServerContext* context,
     const api::ResumeSpeakerCommand_Request* request, api::ResumeSpeakerCommand_Feedback* response) {
+    auto media_session = globalMediaActivityCoordinator().beginSession();
+    if (!media_session) {
+        return failResponse(
+            response, "Media activities are temporarily paused by StopAll");
+    }
     try {
         string dev_id = request->header().device_id();
         CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (ResumePlayback): id=" << dev_id;
@@ -208,7 +299,14 @@ grpc::Status gRPCSpeakerServiceImpl::ResumePlayback(grpc::ServerContext* context
         if (!dev) {
             return failResponse(response, "Speaker device not found: " + dev_id);
         }
-        dev->resume();
+        if (!media_session.claimExclusiveResource(speakerResourceKey(dev_id))) {
+            return failResponse(
+                response, "Speaker is already controlled by another media session: " + dev_id);
+        }
+        if (!media_session.runIfCurrent([&] { dev->resume(); })) {
+            return failResponse(
+                response, "Speaker playback resume was canceled by StopAll");
+        }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
         CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (ResumePlayback): success, id=" << dev_id;

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

@@ -0,0 +1,437 @@
+#include "service/grpc/include/media_activity_coordinator.h"
+
+#include "common/base/logging/logger.h"
+#include "service/stop_all/include/stop_all_admission_gate.h"
+
+#include <algorithm>
+#include <atomic>
+#include <condition_variable>
+#include <exception>
+#include <mutex>
+#include <unordered_map>
+#include <unordered_set>
+#include <utility>
+#include <vector>
+
+namespace cmvr::service {
+
+struct MediaActivityCoordinator::SessionState final {
+    SessionState(
+        const std::uint64_t session_id_value,
+        const std::uint64_t generation_value,
+        CancelCallback callback)
+        : session_id(session_id_value),
+          generation(generation_value),
+          cancel_callback(std::move(callback)) {}
+
+    void markCancelled() noexcept
+    {
+        is_cancelled.store(true, std::memory_order_release);
+    }
+
+    DeferredStopResult requestCancel()
+    {
+        markCancelled();
+
+        CancelCallback callback;
+        {
+            std::unique_lock lock(callback_mutex);
+            if (callback_in_flight) {
+                callback_condition.wait(
+                    lock, [this] { return !callback_in_flight; });
+            }
+            if (callback_invoked) {
+                return cancel_result;
+            }
+            if (released || !cancel_callback) {
+                callback_invoked = true;
+                cancel_result = {true, {}};
+                return cancel_result;
+            }
+            callback_invoked = true;
+            callback_in_flight = true;
+            callback = cancel_callback;
+        }
+
+        try {
+            DeferredStopResult result{true, {}};
+            try {
+                callback();
+            } catch (const std::exception& error) {
+                result = {
+                    false,
+                    std::string("media cancellation callback threw: ") +
+                        error.what()};
+                CMVR_LOG(ERROR)
+                    << "[MediaActivityCoordinator] " << result.detail;
+            } catch (...) {
+                result = {
+                    false,
+                    "media cancellation callback threw an unknown exception"};
+                CMVR_LOG(ERROR)
+                    << "[MediaActivityCoordinator] " << result.detail;
+            }
+
+            {
+                std::lock_guard lock(callback_mutex);
+                cancel_result = result;
+                callback_in_flight = false;
+            }
+            callback_condition.notify_all();
+            return result;
+        } catch (...) {
+            {
+                std::lock_guard lock(callback_mutex);
+                callback_in_flight = false;
+            }
+            callback_condition.notify_all();
+            throw;
+        }
+    }
+
+    void prepareRelease() noexcept
+    {
+        std::unique_lock lock(callback_mutex);
+        released = true;
+        cancel_callback = {};
+        callback_condition.wait(lock, [this] { return !callback_in_flight; });
+    }
+
+    const std::uint64_t session_id;
+    const std::uint64_t generation;
+    std::atomic<bool> is_cancelled{false};
+    std::mutex callback_mutex;
+    std::condition_variable callback_condition;
+    CancelCallback cancel_callback;
+    bool callback_invoked{false};
+    bool callback_in_flight{false};
+    bool released{false};
+    DeferredStopResult cancel_result;
+    std::size_t dispatches_in_flight{0U};
+    std::unordered_set<std::string> exclusive_resources;
+};
+
+struct MediaActivityCoordinator::Impl final {
+    bool hasSessionsBefore(const std::uint64_t generation) const
+    {
+        for (const auto& [session_id, session] : sessions) {
+            (void)session_id;
+            if (session->generation < generation) {
+                return true;
+            }
+        }
+        return false;
+    }
+
+    mutable std::mutex mutex;
+    std::condition_variable condition;
+    std::unordered_map<std::uint64_t, std::shared_ptr<SessionState>> sessions;
+    std::unordered_map<std::string, std::uint64_t> exclusive_resources;
+    std::unordered_set<std::uint64_t> stop_all_tickets;
+    std::uint64_t generation{1U};
+    std::uint64_t next_session_id{0U};
+    std::uint64_t next_ticket_id{0U};
+    bool accepting{true};
+    bool stop_all_failed{false};
+};
+
+MediaActivityCoordinator::Session::Session(
+    std::shared_ptr<Impl> impl,
+    std::shared_ptr<SessionState> state)
+    : impl_(std::move(impl)), state_(std::move(state)) {}
+
+MediaActivityCoordinator::Session::~Session()
+{
+    reset();
+}
+
+MediaActivityCoordinator::Session::Session(Session&& other) noexcept
+    : impl_(std::move(other.impl_)), state_(std::move(other.state_)) {}
+
+MediaActivityCoordinator::Session&
+MediaActivityCoordinator::Session::operator=(Session&& other) noexcept
+{
+    if (this != &other) {
+        reset();
+        impl_ = std::move(other.impl_);
+        state_ = std::move(other.state_);
+    }
+    return *this;
+}
+
+MediaActivityCoordinator::Session::operator bool() const noexcept
+{
+    return impl_ && state_;
+}
+
+bool MediaActivityCoordinator::Session::cancelled() const noexcept
+{
+    return !state_ || state_->is_cancelled.load(std::memory_order_acquire);
+}
+
+bool MediaActivityCoordinator::Session::runIfCurrent(
+    const std::function<void()>& operation) const
+{
+    if (!impl_ || !state_ || !operation) {
+        return false;
+    }
+
+    {
+        std::lock_guard lock(impl_->mutex);
+        const auto active = impl_->sessions.find(state_->session_id);
+        if (!impl_->accepting ||
+            state_->generation != impl_->generation ||
+            state_->is_cancelled.load(std::memory_order_acquire) ||
+            active == impl_->sessions.end() ||
+            active->second != state_) {
+            return false;
+        }
+        ++state_->dispatches_in_flight;
+    }
+
+    const auto finish_dispatch = [impl = impl_, state = state_]() noexcept {
+        try {
+            {
+                std::lock_guard lock(impl->mutex);
+                if (state->dispatches_in_flight > 0U) {
+                    --state->dispatches_in_flight;
+                }
+            }
+            impl->condition.notify_all();
+        } catch (...) {
+        }
+    };
+
+    try {
+        operation();
+    } catch (...) {
+        finish_dispatch();
+        throw;
+    }
+    finish_dispatch();
+    return true;
+}
+
+bool MediaActivityCoordinator::Session::claimExclusiveResource(
+    const std::string& resource_key)
+{
+    if (!impl_ || !state_ || resource_key.empty()) {
+        return false;
+    }
+
+    std::lock_guard lock(impl_->mutex);
+    const auto active = impl_->sessions.find(state_->session_id);
+    if (!impl_->accepting ||
+        state_->generation != impl_->generation ||
+        state_->is_cancelled.load(std::memory_order_acquire) ||
+        active == impl_->sessions.end() ||
+        active->second != state_) {
+        return false;
+    }
+
+    const auto owner = impl_->exclusive_resources.find(resource_key);
+    if (owner != impl_->exclusive_resources.end()) {
+        return owner->second == state_->session_id;
+    }
+    impl_->exclusive_resources.emplace(resource_key, state_->session_id);
+    state_->exclusive_resources.emplace(resource_key);
+    return true;
+}
+
+void MediaActivityCoordinator::Session::reset() noexcept
+{
+    auto impl = std::move(impl_);
+    auto state = std::move(state_);
+    if (!impl || !state) {
+        return;
+    }
+
+    state->prepareRelease();
+    {
+        std::unique_lock lock(impl->mutex);
+        state->markCancelled();
+        impl->condition.wait(lock, [&state] {
+            return state->dispatches_in_flight == 0U;
+        });
+        const auto active = impl->sessions.find(state->session_id);
+        if (active != impl->sessions.end() && active->second == state) {
+            impl->sessions.erase(active);
+        }
+        for (const auto& resource_key : state->exclusive_resources) {
+            const auto owner = impl->exclusive_resources.find(resource_key);
+            if (owner != impl->exclusive_resources.end() &&
+                owner->second == state->session_id) {
+                impl->exclusive_resources.erase(owner);
+            }
+        }
+    }
+    impl->condition.notify_all();
+}
+
+MediaActivityCoordinator::MediaActivityCoordinator()
+    : impl_(std::make_shared<Impl>()) {}
+
+MediaActivityCoordinator::Session MediaActivityCoordinator::beginSession(
+    CancelCallback cancel)
+{
+    auto system_admission =
+        globalStopAllAdmissionGate().lockAdmission();
+    std::lock_guard lock(impl_->mutex);
+    if (!system_admission.accepting() || !impl_->accepting) {
+        return {};
+    }
+
+    const auto session_id = ++impl_->next_session_id;
+    auto state = std::make_shared<SessionState>(
+        session_id, impl_->generation, std::move(cancel));
+    impl_->sessions.emplace(session_id, state);
+    return Session(impl_, std::move(state));
+}
+
+MediaActivityCoordinator::StopAllTicket
+MediaActivityCoordinator::beginStopAll(const bool defer_cancellation)
+{
+    StopAllTicket ticket;
+    std::vector<std::shared_ptr<SessionState>> sessions;
+    {
+        std::lock_guard lock(impl_->mutex);
+        if (impl_->accepting || impl_->stop_all_tickets.empty()) {
+            impl_->accepting = false;
+            ++impl_->generation;
+            impl_->stop_all_failed = false;
+        }
+
+        ticket.generation = impl_->generation;
+        ticket.ticket_id = ++impl_->next_ticket_id;
+        impl_->stop_all_tickets.emplace(ticket.ticket_id);
+
+        sessions.reserve(impl_->sessions.size());
+        for (const auto& [session_id, session] : impl_->sessions) {
+            (void)session_id;
+            if (session->generation < ticket.generation) {
+                session->markCancelled();
+                sessions.push_back(session);
+            }
+        }
+    }
+
+    if (!defer_cancellation) {
+        for (const auto& session : sessions) {
+            (void)session->requestCancel();
+        }
+    }
+    return ticket;
+}
+
+bool MediaActivityCoordinator::collectCancellationOperations(
+    const StopAllTicket& ticket,
+    std::vector<DeferredStopOperation>& operations,
+    std::string* error) const
+{
+    operations.clear();
+    if (error) {
+        error->clear();
+    }
+    if (!ticket.valid()) {
+        if (error) {
+            *error = "invalid media StopAll ticket";
+        }
+        return false;
+    }
+
+    {
+        std::lock_guard lock(impl_->mutex);
+        if (impl_->accepting || ticket.generation != impl_->generation ||
+            impl_->stop_all_tickets.count(ticket.ticket_id) == 0U) {
+            if (error) {
+                *error = "media StopAll ticket is no longer current";
+            }
+            return false;
+        }
+        operations.reserve(impl_->sessions.size());
+        for (const auto& [session_id, session] : impl_->sessions) {
+            if (session->generation < ticket.generation) {
+                operations.push_back({
+                    "media-session:" + std::to_string(session_id),
+                    [session] { return session->requestCancel(); }});
+            }
+        }
+    }
+    std::sort(
+        operations.begin(), operations.end(),
+        [](const auto& lhs, const auto& rhs) {
+            return lhs.resource_key < rhs.resource_key;
+        });
+    return true;
+}
+
+bool MediaActivityCoordinator::requestCancellation(
+    const StopAllTicket& ticket)
+{
+    std::vector<DeferredStopOperation> operations;
+    if (!collectCancellationOperations(ticket, operations)) {
+        return false;
+    }
+
+    bool all_cancelled = true;
+    for (const auto& operation : operations) {
+        try {
+            if (!operation.operation().success) {
+                all_cancelled = false;
+            }
+        } catch (...) {
+            all_cancelled = false;
+        }
+    }
+    return all_cancelled;
+}
+
+bool MediaActivityCoordinator::waitForStopped(
+    const StopAllTicket& ticket,
+    const std::chrono::milliseconds timeout)
+{
+    if (!ticket.valid() || timeout < std::chrono::milliseconds::zero()) {
+        return false;
+    }
+
+    std::unique_lock lock(impl_->mutex);
+    if (ticket.generation != impl_->generation ||
+        impl_->stop_all_tickets.count(ticket.ticket_id) == 0U) {
+        return false;
+    }
+    return impl_->condition.wait_for(lock, timeout, [this, &ticket] {
+        return ticket.generation == impl_->generation &&
+            !impl_->hasSessionsBefore(ticket.generation);
+    });
+}
+
+bool MediaActivityCoordinator::finishStopAll(
+    const StopAllTicket& ticket,
+    const bool all_media_stopped)
+{
+    std::lock_guard lock(impl_->mutex);
+    if (!ticket.valid() || impl_->accepting ||
+        ticket.generation != impl_->generation ||
+        impl_->stop_all_tickets.erase(ticket.ticket_id) == 0U) {
+        return false;
+    }
+
+    const bool caller_succeeded =
+        all_media_stopped && !impl_->hasSessionsBefore(ticket.generation);
+    if (!caller_succeeded) {
+        impl_->stop_all_failed = true;
+    }
+    if (impl_->stop_all_tickets.empty() && !impl_->stop_all_failed &&
+        !impl_->hasSessionsBefore(ticket.generation)) {
+        impl_->accepting = true;
+    }
+    return caller_succeeded;
+}
+
+MediaActivityCoordinator& globalMediaActivityCoordinator()
+{
+    static MediaActivityCoordinator coordinator;
+    return coordinator;
+}
+
+} // namespace cmvr::service

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

@@ -0,0 +1,488 @@
+#include "service/grpc/include/motor_activity_coordinator.h"
+
+#include <algorithm>
+#include <condition_variable>
+#include <exception>
+#include <unordered_map>
+#include <unordered_set>
+#include <utility>
+#include <vector>
+
+#include "common/base/logging/logger.h"
+
+namespace cmvr::service {
+
+namespace {
+
+struct ControlCallbacks final {
+    MotorActivityCoordinator::CancelCallback cancel;
+    MotorActivityCoordinator::QuickStopCallback quick_stop;
+    MotorActivityCoordinator::IdleCallback idle;
+    std::string description;
+};
+
+struct MotorStopOperationState final {
+    explicit MotorStopOperationState(ControlCallbacks callbacks_value)
+        : callbacks(std::move(callbacks_value))
+    {
+    }
+
+    DeferredStopResult cancel()
+    {
+        {
+            std::unique_lock lock(mutex);
+            condition.wait(lock, [this] { return !cancel_running; });
+            if (cancel_completed) {
+                return cancel_result;
+            }
+            cancel_running = true;
+        }
+
+        try {
+            DeferredStopResult result{true, {}};
+            try {
+                callbacks.cancel();
+            } catch (const std::exception& error) {
+                result = {
+                    false,
+                    std::string("motor cancellation callback threw: ") +
+                        error.what()};
+            } catch (...) {
+                result = {
+                    false,
+                    "motor cancellation callback threw an unknown exception"};
+            }
+
+            {
+                std::lock_guard lock(mutex);
+                cancel_result = result;
+                cancel_completed = true;
+                cancel_running = false;
+            }
+            condition.notify_all();
+            return result;
+        } catch (...) {
+            {
+                std::lock_guard lock(mutex);
+                cancel_running = false;
+            }
+            condition.notify_all();
+            throw;
+        }
+    }
+
+    DeferredStopResult run()
+    {
+        {
+            std::unique_lock lock(mutex);
+            condition.wait(lock, [this] { return !stop_running; });
+            if (stop_completed) {
+                return stop_result;
+            }
+            stop_running = true;
+        }
+
+        try {
+            DeferredStopResult result = cancel();
+            bool quick_stopped = false;
+            std::string quick_stop_error;
+            try {
+                quick_stopped = callbacks.quick_stop();
+                if (!quick_stopped) {
+                    quick_stop_error = callbacks.description.empty()
+                        ? "a registered motor did not confirm quick-stop"
+                        : callbacks.description +
+                              ": quick-stop was not confirmed";
+                }
+            } catch (const std::exception& error) {
+                quick_stop_error =
+                    std::string("motor quick-stop callback threw: ") +
+                    error.what();
+            } catch (...) {
+                quick_stop_error =
+                    "motor quick-stop callback threw an unknown exception";
+            }
+            if (!quick_stopped) {
+                if (result.success) {
+                    result = {false, std::move(quick_stop_error)};
+                } else if (!quick_stop_error.empty()) {
+                    result.detail += "; " + quick_stop_error;
+                }
+            }
+
+            {
+                std::lock_guard lock(mutex);
+                stop_result = result;
+                stop_completed = true;
+                stop_running = false;
+            }
+            condition.notify_all();
+            return result;
+        } catch (...) {
+            {
+                std::lock_guard lock(mutex);
+                stop_running = false;
+            }
+            condition.notify_all();
+            throw;
+        }
+    }
+
+    ControlCallbacks callbacks;
+    std::mutex mutex;
+    std::condition_variable condition;
+    bool cancel_running{false};
+    bool cancel_completed{false};
+    bool stop_running{false};
+    bool stop_completed{false};
+    DeferredStopResult cancel_result;
+    DeferredStopResult stop_result;
+};
+
+} // namespace
+
+struct MotorActivityCoordinator::Impl final {
+    bool targetsIdle() const
+    {
+        for (const auto& [id, stop_state] : round_targets) {
+            (void)stop_state;
+            const auto entry = controls.find(id);
+            if (entry == controls.end()) {
+                continue;
+            }
+            try {
+                if (!entry->second.idle || !entry->second.idle()) {
+                    return false;
+                }
+            } catch (...) {
+                return false;
+            }
+        }
+        return true;
+    }
+
+    std::mutex mutex;
+    std::condition_variable condition;
+    std::unordered_map<std::uint64_t, ControlCallbacks> controls;
+    std::unordered_map<
+        std::uint64_t, std::shared_ptr<MotorStopOperationState>> round_targets;
+    std::unordered_set<std::uint64_t> stop_all_tickets;
+    std::uint64_t generation{1U};
+    std::uint64_t next_control_id{0U};
+    std::uint64_t next_ticket_id{0U};
+    bool accepting{true};
+    bool stop_all_failed{false};
+};
+
+MotorActivityCoordinator::Registration::Registration(
+    std::shared_ptr<Impl> impl,
+    const std::uint64_t id) noexcept
+    : impl_(std::move(impl)), id_(id)
+{
+}
+
+MotorActivityCoordinator::Registration::~Registration()
+{
+    reset();
+}
+
+MotorActivityCoordinator::Registration::Registration(
+    Registration&& other) noexcept
+    : impl_(std::move(other.impl_)), id_(std::exchange(other.id_, 0U))
+{
+}
+
+MotorActivityCoordinator::Registration&
+MotorActivityCoordinator::Registration::operator=(Registration&& other) noexcept
+{
+    if (this != &other) {
+        reset();
+        impl_ = std::move(other.impl_);
+        id_ = std::exchange(other.id_, 0U);
+    }
+    return *this;
+}
+
+MotorActivityCoordinator::Registration::operator bool() const noexcept
+{
+    return impl_ && id_ != 0U;
+}
+
+void MotorActivityCoordinator::Registration::reset() noexcept
+{
+    auto impl = std::move(impl_);
+    const auto id = std::exchange(id_, 0U);
+    if (!impl || id == 0U) {
+        return;
+    }
+    try {
+        {
+            std::lock_guard lock(impl->mutex);
+            impl->controls.erase(id);
+        }
+        impl->condition.notify_all();
+    } catch (...) {
+    }
+}
+
+MotorActivityCoordinator::AdmissionGuard::AdmissionGuard(
+    std::unique_lock<std::mutex>&& lock,
+    const bool accepting) noexcept
+    : lock_(std::move(lock)), accepting_(accepting)
+{
+}
+
+MotorActivityCoordinator::MotorActivityCoordinator()
+    : impl_(std::make_shared<Impl>())
+{
+}
+
+MotorActivityCoordinator::Registration
+MotorActivityCoordinator::registerControl(
+    CancelCallback cancel,
+    QuickStopCallback quick_stop,
+    IdleCallback idle,
+    std::string description)
+{
+    if (!cancel || !quick_stop || !idle) {
+        return {};
+    }
+
+    std::lock_guard lock(impl_->mutex);
+    const auto id = ++impl_->next_control_id;
+    impl_->controls.emplace(
+        id,
+        ControlCallbacks{
+            std::move(cancel), std::move(quick_stop), std::move(idle),
+            std::move(description)});
+    return Registration(impl_, id);
+}
+
+MotorActivityCoordinator::AdmissionGuard
+MotorActivityCoordinator::lockAdmission()
+{
+    std::unique_lock lock(impl_->mutex);
+    return AdmissionGuard(std::move(lock), impl_->accepting);
+}
+
+MotorActivityCoordinator::StopAllTicket
+MotorActivityCoordinator::beginStopAll(const bool defer_callbacks)
+{
+    StopAllTicket ticket;
+    std::vector<std::shared_ptr<MotorStopOperationState>> targets;
+    {
+        std::lock_guard lock(impl_->mutex);
+        if (impl_->accepting || impl_->stop_all_tickets.empty()) {
+            impl_->accepting = false;
+            impl_->stop_all_failed = false;
+            ++impl_->generation;
+            impl_->round_targets.clear();
+            targets.reserve(impl_->controls.size());
+            for (const auto& [id, callbacks] : impl_->controls) {
+                auto state =
+                    std::make_shared<MotorStopOperationState>(callbacks);
+                impl_->round_targets.emplace(id, state);
+                targets.push_back(std::move(state));
+            }
+        }
+
+        ticket.generation = impl_->generation;
+        ticket.ticket_id = ++impl_->next_ticket_id;
+        impl_->stop_all_tickets.emplace(ticket.ticket_id);
+        if (targets.empty()) {
+            targets.reserve(impl_->round_targets.size());
+            for (const auto& [id, state] : impl_->round_targets) {
+                (void)id;
+                targets.push_back(state);
+            }
+        }
+    }
+
+    if (!defer_callbacks) {
+        for (const auto& target : targets) {
+            const auto result = target->cancel();
+            if (!result.success) {
+                CMVR_LOG(ERROR)
+                    << "[MotorActivityCoordinator] " << result.detail;
+            }
+        }
+    }
+    impl_->condition.notify_all();
+    return ticket;
+}
+
+bool MotorActivityCoordinator::collectStopOperations(
+    const StopAllTicket& ticket,
+    std::vector<DeferredStopOperation>& operations,
+    std::string* error) const
+{
+    operations.clear();
+    if (error) {
+        error->clear();
+    }
+    if (!ticket.valid()) {
+        if (error) {
+            *error = "invalid motor StopAll ticket";
+        }
+        return false;
+    }
+
+    {
+        std::lock_guard lock(impl_->mutex);
+        if (impl_->accepting || ticket.generation != impl_->generation ||
+            impl_->stop_all_tickets.count(ticket.ticket_id) == 0U) {
+            if (error) {
+                *error = "motor StopAll ticket is no longer current";
+            }
+            return false;
+        }
+        operations.reserve(impl_->round_targets.size());
+        for (const auto& [id, state] : impl_->round_targets) {
+            operations.push_back({
+                "motor-control:" + std::to_string(id),
+                [state] { return state->run(); }});
+        }
+    }
+    std::sort(
+        operations.begin(), operations.end(),
+        [](const auto& lhs, const auto& rhs) {
+            return lhs.resource_key < rhs.resource_key;
+        });
+    return true;
+}
+
+bool MotorActivityCoordinator::requestStop(
+    const StopAllTicket& ticket,
+    std::string* error)
+{
+    std::vector<DeferredStopOperation> operations;
+    if (!collectStopOperations(ticket, operations, error)) {
+        return false;
+    }
+
+    bool all_stopped = true;
+    for (const auto& operation : operations) {
+        DeferredStopResult result;
+        try {
+            result = operation.operation();
+        } catch (const std::exception& exception) {
+            result = {
+                false,
+                std::string("motor stop operation threw: ") +
+                    exception.what()};
+        } catch (...) {
+            result = {
+                false, "motor stop operation threw an unknown exception"};
+        }
+        if (!result.success) {
+            all_stopped = false;
+            if (error && error->empty()) {
+                *error = result.detail.empty()
+                    ? "a registered motor did not confirm quick-stop"
+                    : result.detail;
+            }
+            CMVR_LOG(ERROR)
+                << "[MotorActivityCoordinator] "
+                << (result.detail.empty()
+                        ? "motor stop operation was not confirmed"
+                        : result.detail);
+        }
+    }
+    return all_stopped;
+}
+
+bool MotorActivityCoordinator::waitForStopped(
+    const StopAllTicket& ticket,
+    const std::chrono::milliseconds timeout,
+    std::string* error)
+{
+    if (!ticket.valid() || timeout < std::chrono::milliseconds::zero()) {
+        if (error && error->empty()) {
+            *error = "invalid motor StopAll ticket or timeout";
+        }
+        return false;
+    }
+
+    const auto deadline = std::chrono::steady_clock::now() + timeout;
+    std::unique_lock lock(impl_->mutex);
+    const bool idle = impl_->condition.wait_until(lock, deadline, [&] {
+        return ticket.generation != impl_->generation ||
+            impl_->stop_all_tickets.count(ticket.ticket_id) == 0U ||
+            impl_->targetsIdle();
+    });
+    const bool ticket_current =
+        !impl_->accepting && ticket.generation == impl_->generation &&
+        impl_->stop_all_tickets.count(ticket.ticket_id) != 0U;
+    const bool targets_idle = ticket_current && impl_->targetsIdle();
+    if ((!idle || !targets_idle) && error && error->empty()) {
+        *error = "timed out waiting for active motor RPCs to stop";
+    }
+    return idle && targets_idle;
+}
+
+bool MotorActivityCoordinator::stopAndWait(
+    const StopAllTicket& ticket,
+    const std::chrono::milliseconds timeout,
+    std::string* error)
+{
+    if (error) {
+        error->clear();
+    }
+    const bool stop_requested = requestStop(ticket, error);
+    const bool stopped = waitForStopped(ticket, timeout, error);
+    return stop_requested && stopped;
+}
+
+bool MotorActivityCoordinator::finishStopAll(
+    const StopAllTicket& ticket,
+    const bool all_motors_stopped)
+{
+    std::lock_guard lock(impl_->mutex);
+    if (!ticket.valid() || impl_->accepting ||
+        ticket.generation != impl_->generation ||
+        impl_->stop_all_tickets.erase(ticket.ticket_id) == 0U) {
+        return false;
+    }
+
+    const bool caller_succeeded =
+        all_motors_stopped && impl_->targetsIdle();
+    if (!caller_succeeded) {
+        impl_->stop_all_failed = true;
+    }
+    if (impl_->stop_all_tickets.empty() && !impl_->stop_all_failed &&
+        impl_->targetsIdle()) {
+        impl_->accepting = true;
+        impl_->round_targets.clear();
+    }
+    return caller_succeeded;
+}
+
+void MotorActivityCoordinator::notifyStateChanged() noexcept
+{
+    try {
+        impl_->condition.notify_all();
+    } catch (...) {
+    }
+}
+
+void MotorActivityCoordinator::clearForTesting() noexcept
+{
+    try {
+        std::lock_guard lock(impl_->mutex);
+        impl_->accepting = true;
+        impl_->stop_all_failed = false;
+        ++impl_->generation;
+        impl_->round_targets.clear();
+        impl_->stop_all_tickets.clear();
+    } catch (...) {
+    }
+    notifyStateChanged();
+}
+
+MotorActivityCoordinator& globalMotorActivityCoordinator()
+{
+    static MotorActivityCoordinator coordinator;
+    return coordinator;
+}
+
+} // namespace cmvr::service

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

@@ -0,0 +1,491 @@
+#include "service/grpc/include/camera_operational_activity_registry.h"
+
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "service/stop_all/include/stop_all_admission_gate.h"
+
+namespace cmvr::service {
+namespace {
+
+class TestCamera final : public device::AbstractCamera {
+public:
+    explicit TestCamera(std::string id)
+    {
+        id_ = std::move(id);
+        state_.is_initialized = true;
+    }
+
+    std::string typeName() const override { return "TestCamera"; }
+
+    void getState(device::CameraState& state) override
+    {
+        std::lock_guard lock(mutex_);
+        state = state_;
+    }
+
+    bool start() override
+    {
+        ++lifecycle_start_calls_;
+        return true;
+    }
+
+    bool stop() override
+    {
+        ++lifecycle_stop_calls_;
+        std::lock_guard lock(mutex_);
+        operational_active_ = false;
+        state_.is_opened = false;
+        return !fail_lifecycle_stop_;
+    }
+
+    bool startOperationalActivity() override
+    {
+        ++operational_start_calls_;
+        {
+            std::unique_lock lock(mutex_);
+            start_entered_ = true;
+            condition_.notify_all();
+            condition_.wait(lock, [this] { return !block_start_; });
+            if (fail_operational_start_) {
+                return false;
+            }
+            operational_active_ = true;
+            state_.is_opened = true;
+        }
+        return true;
+    }
+
+    bool stopOperationalActivity() override
+    {
+        ++operational_stop_calls_;
+        std::unique_lock lock(mutex_);
+        stop_entered_ = true;
+        condition_.notify_all();
+        condition_.wait(lock, [this] { return !block_stop_; });
+        if (fail_operational_stop_) {
+            return false;
+        }
+        operational_active_ = false;
+        state_.is_opened = false;
+        return true;
+    }
+
+    void setFailOperationalStart(const bool fail)
+    {
+        fail_operational_start_ = fail;
+    }
+
+    void setFailOperationalStop(const bool fail)
+    {
+        fail_operational_stop_ = fail;
+    }
+
+    void blockStop()
+    {
+        std::lock_guard lock(mutex_);
+        block_stop_ = true;
+        stop_entered_ = false;
+    }
+
+    void waitForStopEntered()
+    {
+        std::unique_lock lock(mutex_);
+        condition_.wait(lock, [this] { return stop_entered_; });
+    }
+
+    void releaseStop()
+    {
+        {
+            std::lock_guard lock(mutex_);
+            block_stop_ = false;
+        }
+        condition_.notify_all();
+    }
+
+    void blockStart()
+    {
+        std::lock_guard lock(mutex_);
+        block_start_ = true;
+        start_entered_ = false;
+    }
+
+    void waitForStartEntered()
+    {
+        std::unique_lock lock(mutex_);
+        condition_.wait(lock, [this] { return start_entered_; });
+    }
+
+    void releaseStart()
+    {
+        {
+            std::lock_guard lock(mutex_);
+            block_start_ = false;
+        }
+        condition_.notify_all();
+    }
+
+    bool operationalActive() const
+    {
+        std::lock_guard lock(mutex_);
+        return operational_active_;
+    }
+
+    int lifecycleStartCalls() const { return lifecycle_start_calls_; }
+    int lifecycleStopCalls() const { return lifecycle_stop_calls_; }
+    int operationalStartCalls() const { return operational_start_calls_; }
+    int operationalStopCalls() const { return operational_stop_calls_; }
+
+private:
+    mutable std::mutex mutex_;
+    std::condition_variable condition_;
+    bool operational_active_{false};
+    bool fail_operational_start_{false};
+    bool fail_operational_stop_{false};
+    bool fail_lifecycle_stop_{false};
+    bool block_start_{false};
+    bool start_entered_{false};
+    bool block_stop_{false};
+    bool stop_entered_{false};
+    std::atomic<int> lifecycle_start_calls_{0};
+    std::atomic<int> lifecycle_stop_calls_{0};
+    std::atomic<int> operational_start_calls_{0};
+    std::atomic<int> operational_stop_calls_{0};
+};
+
+class CameraOperationalActivityRegistryTest : public ::testing::Test {
+protected:
+    void SetUp() override
+    {
+        globalStopAllAdmissionGate().clearForTesting();
+    }
+
+    void TearDown() override
+    {
+        globalStopAllAdmissionGate().clearForTesting();
+    }
+};
+
+TEST_F(CameraOperationalActivityRegistryTest,
+       StopAllStopsTrackedActivityWithoutLifecycleStopAndCanRestart)
+{
+    CameraOperationalActivityRegistry registry;
+    auto camera = std::make_shared<TestCamera>("camera");
+
+    EXPECT_EQ(
+        registry.start(camera->id(), camera),
+        CameraOperationalActivityRegistry::DispatchResult::Success);
+    EXPECT_TRUE(camera->operationalActive());
+    EXPECT_EQ(registry.activeCameraCount(), 1U);
+
+    auto ticket = globalStopAllAdmissionGate().beginStopAll();
+    ASSERT_TRUE(ticket.valid());
+    EXPECT_TRUE(registry.stopAllActivities());
+    EXPECT_FALSE(camera->operationalActive());
+    EXPECT_EQ(camera->lifecycleStopCalls(), 0);
+    EXPECT_EQ(camera->operationalStopCalls(), 1);
+    EXPECT_EQ(registry.activeCameraCount(), 0U);
+    ASSERT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true));
+
+    EXPECT_EQ(
+        registry.start(camera->id(), camera),
+        CameraOperationalActivityRegistry::DispatchResult::Success);
+    EXPECT_TRUE(camera->operationalActive());
+    EXPECT_EQ(camera->operationalStartCalls(), 2);
+    EXPECT_EQ(camera->lifecycleStartCalls(), 0);
+}
+
+TEST_F(CameraOperationalActivityRegistryTest,
+       FailedOperationalStopRemainsTrackedAndKeepsAdmissionClosed)
+{
+    CameraOperationalActivityRegistry registry;
+    auto camera = std::make_shared<TestCamera>("camera");
+    ASSERT_EQ(
+        registry.start(camera->id(), camera),
+        CameraOperationalActivityRegistry::DispatchResult::Success);
+
+    camera->setFailOperationalStop(true);
+    auto ticket = globalStopAllAdmissionGate().beginStopAll();
+    std::vector<std::string> failures;
+    EXPECT_FALSE(registry.stopAllActivities(&failures));
+    EXPECT_EQ(registry.activeCameraCount(), 1U);
+    ASSERT_EQ(failures.size(), 1U);
+    EXPECT_EQ(
+        registry.start(camera->id(), camera),
+        CameraOperationalActivityRegistry::DispatchResult::RejectedByStopAll);
+    EXPECT_FALSE(globalStopAllAdmissionGate().finishStopAll(ticket, false));
+
+    camera->setFailOperationalStop(false);
+    ticket = globalStopAllAdmissionGate().beginStopAll();
+    EXPECT_TRUE(registry.stopAllActivities());
+    EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true));
+    EXPECT_EQ(registry.activeCameraCount(), 0U);
+}
+
+TEST_F(CameraOperationalActivityRegistryTest,
+       StopAllWaitsForRacingStartThenStopsTheStartedActivity)
+{
+    CameraOperationalActivityRegistry registry;
+    auto camera = std::make_shared<TestCamera>("camera");
+    camera->blockStart();
+
+    CameraOperationalActivityRegistry::DispatchResult start_result =
+        CameraOperationalActivityRegistry::DispatchResult::DeviceFailure;
+    std::thread start_thread([&] {
+        start_result = registry.start(camera->id(), camera);
+    });
+    camera->waitForStartEntered();
+
+    const auto ticket = globalStopAllAdmissionGate().beginStopAll();
+    std::atomic<bool> stop_returned{false};
+    bool stop_result = false;
+    std::thread stop_thread([&] {
+        stop_result = registry.stopAllActivities();
+        stop_returned = true;
+    });
+
+    std::this_thread::yield();
+    EXPECT_FALSE(stop_returned.load());
+    camera->releaseStart();
+    start_thread.join();
+    stop_thread.join();
+
+    EXPECT_EQ(
+        start_result,
+        CameraOperationalActivityRegistry::DispatchResult::Success);
+    EXPECT_TRUE(stop_result);
+    EXPECT_FALSE(camera->operationalActive());
+    EXPECT_EQ(camera->operationalStopCalls(), 1);
+    EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true));
+}
+
+TEST_F(CameraOperationalActivityRegistryTest,
+       TrackedIdsExposeAStartThatIsStillInsideDriverIo)
+{
+    CameraOperationalActivityRegistry registry;
+    auto camera = std::make_shared<TestCamera>("camera.pending");
+    camera->blockStart();
+
+    CameraOperationalActivityRegistry::DispatchResult start_result =
+        CameraOperationalActivityRegistry::DispatchResult::DeviceFailure;
+    std::thread start_thread([&] {
+        start_result = registry.start(camera->id(), camera);
+    });
+    camera->waitForStartEntered();
+
+    const auto snapshot_started = std::chrono::steady_clock::now();
+    EXPECT_EQ(
+        registry.trackedDeviceIds(),
+        (std::vector<std::string>{"camera.pending"}));
+    EXPECT_LT(
+        std::chrono::steady_clock::now() - snapshot_started,
+        std::chrono::milliseconds(100));
+
+    camera->releaseStart();
+    start_thread.join();
+    EXPECT_EQ(
+        start_result,
+        CameraOperationalActivityRegistry::DispatchResult::Success);
+}
+
+TEST_F(CameraOperationalActivityRegistryTest,
+       DefaultOperationalStopFailsClosed)
+{
+    class UnsupportedCamera final : public device::AbstractCamera {
+    public:
+        UnsupportedCamera() { id_ = "unsupported"; }
+        std::string typeName() const override { return "UnsupportedCamera"; }
+        void getState(device::CameraState& state) override { state = {}; }
+    };
+
+    CameraOperationalActivityRegistry registry;
+    auto camera = std::make_shared<UnsupportedCamera>();
+    ASSERT_EQ(
+        registry.start(camera->id(), camera),
+        CameraOperationalActivityRegistry::DispatchResult::Success);
+
+    const auto ticket = globalStopAllAdmissionGate().beginStopAll();
+    EXPECT_FALSE(registry.stopAllActivities());
+    EXPECT_EQ(registry.activeCameraCount(), 1U);
+    EXPECT_FALSE(globalStopAllAdmissionGate().finishStopAll(ticket, false));
+}
+
+TEST_F(CameraOperationalActivityRegistryTest,
+       StopForDeviceIsSelectiveAndActiveIdsReflectFailures)
+{
+    CameraOperationalActivityRegistry registry;
+    auto first = std::make_shared<TestCamera>("camera.first");
+    auto second = std::make_shared<TestCamera>("camera.second");
+    ASSERT_EQ(
+        registry.start(first->id(), first),
+        CameraOperationalActivityRegistry::DispatchResult::Success);
+    ASSERT_EQ(
+        registry.start(second->id(), second),
+        CameraOperationalActivityRegistry::DispatchResult::Success);
+    EXPECT_EQ(
+        registry.activeDeviceIds(),
+        (std::vector<std::string>{"camera.first", "camera.second"}));
+    EXPECT_EQ(
+        registry.trackedDeviceIds(),
+        (std::vector<std::string>{"camera.first", "camera.second"}));
+
+    const auto ticket = globalStopAllAdmissionGate().beginStopAll();
+    first->setFailOperationalStop(true);
+    std::vector<std::string> failures;
+    EXPECT_FALSE(registry.stopActivitiesForDevice(first->id(), &failures));
+    EXPECT_EQ(failures.size(), 1U);
+    EXPECT_TRUE(first->operationalActive());
+    EXPECT_TRUE(second->operationalActive());
+    EXPECT_EQ(second->operationalStopCalls(), 0);
+
+    first->setFailOperationalStop(false);
+    EXPECT_TRUE(registry.stopActivitiesForDevice(first->id()));
+    EXPECT_EQ(
+        registry.activeDeviceIds(),
+        (std::vector<std::string>{"camera.second"}));
+    EXPECT_TRUE(registry.stopActivitiesForDevice("missing"));
+    EXPECT_TRUE(registry.stopAllActivities());
+    EXPECT_TRUE(registry.activeDeviceIds().empty());
+    EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true));
+    EXPECT_EQ(first->lifecycleStopCalls(), 0);
+    EXPECT_EQ(second->lifecycleStopCalls(), 0);
+}
+
+TEST_F(CameraOperationalActivityRegistryTest,
+       InventoryFallbackStopsOncePerStopAllRoundAndCanStopAgainLater)
+{
+    CameraOperationalActivityRegistry registry;
+    auto camera = std::make_shared<TestCamera>("inventory-camera");
+    ASSERT_TRUE(camera->startOperationalActivity());
+
+    auto ticket = globalStopAllAdmissionGate().beginStopAll();
+    ASSERT_TRUE(ticket.valid());
+    EXPECT_TRUE(registry.stopActivitiesForDevice(camera->id(), camera));
+    EXPECT_TRUE(registry.stopActivitiesForDevice(camera->id(), camera));
+    EXPECT_FALSE(camera->operationalActive());
+    EXPECT_EQ(camera->operationalStopCalls(), 1);
+    EXPECT_EQ(camera->lifecycleStopCalls(), 0);
+    ASSERT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true));
+
+    ASSERT_TRUE(camera->startOperationalActivity());
+    ticket = globalStopAllAdmissionGate().beginStopAll();
+    ASSERT_TRUE(ticket.valid());
+    EXPECT_TRUE(registry.stopActivitiesForDevice(camera->id(), camera));
+    EXPECT_FALSE(camera->operationalActive());
+    EXPECT_EQ(camera->operationalStopCalls(), 2);
+    EXPECT_EQ(camera->lifecycleStopCalls(), 0);
+    EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true));
+}
+
+TEST_F(CameraOperationalActivityRegistryTest,
+       TrackedCameraTakesPrecedenceOverInventoryFallback)
+{
+    CameraOperationalActivityRegistry registry;
+    auto tracked = std::make_shared<TestCamera>("camera");
+    auto fallback = std::make_shared<TestCamera>("camera");
+    ASSERT_EQ(
+        registry.start(tracked->id(), tracked),
+        CameraOperationalActivityRegistry::DispatchResult::Success);
+    ASSERT_TRUE(fallback->startOperationalActivity());
+
+    const auto ticket = globalStopAllAdmissionGate().beginStopAll();
+    ASSERT_TRUE(ticket.valid());
+    EXPECT_TRUE(registry.stopActivitiesForDevice(
+        tracked->id(), fallback));
+    EXPECT_FALSE(tracked->operationalActive());
+    EXPECT_EQ(tracked->operationalStopCalls(), 1);
+    EXPECT_TRUE(fallback->operationalActive());
+    EXPECT_EQ(fallback->operationalStopCalls(), 0);
+    EXPECT_EQ(tracked->lifecycleStopCalls(), 0);
+    EXPECT_EQ(fallback->lifecycleStopCalls(), 0);
+    EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true));
+}
+
+TEST_F(CameraOperationalActivityRegistryTest,
+       ActiveDeviceIdCannotBeReboundToAnotherCameraInstance)
+{
+    CameraOperationalActivityRegistry registry;
+    auto original = std::make_shared<TestCamera>("camera");
+    auto replacement = std::make_shared<TestCamera>("camera");
+    ASSERT_EQ(
+        registry.start(original->id(), original),
+        CameraOperationalActivityRegistry::DispatchResult::Success);
+
+    CameraOperationalActivityRegistry::ActivityToken replacement_token;
+    EXPECT_EQ(
+        registry.start(
+            replacement->id(), replacement, &replacement_token),
+        CameraOperationalActivityRegistry::DispatchResult::DeviceFailure);
+    EXPECT_FALSE(replacement_token.valid());
+    EXPECT_TRUE(original->operationalActive());
+    EXPECT_FALSE(replacement->operationalActive());
+    EXPECT_EQ(replacement->operationalStartCalls(), 0);
+
+    const auto ticket = globalStopAllAdmissionGate().beginStopAll();
+    ASSERT_TRUE(ticket.valid());
+    EXPECT_TRUE(registry.stopAllActivities());
+    EXPECT_FALSE(original->operationalActive());
+    EXPECT_EQ(original->operationalStopCalls(), 1);
+    EXPECT_EQ(replacement->operationalStopCalls(), 0);
+    EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true));
+}
+
+TEST_F(CameraOperationalActivityRegistryTest,
+       DifferentDevicesStopConcurrentlyAndSameDeviceStopIsSerialized)
+{
+    CameraOperationalActivityRegistry registry;
+    auto first = std::make_shared<TestCamera>("camera.first");
+    auto second = std::make_shared<TestCamera>("camera.second");
+    ASSERT_EQ(
+        registry.start(first->id(), first),
+        CameraOperationalActivityRegistry::DispatchResult::Success);
+    ASSERT_EQ(
+        registry.start(second->id(), second),
+        CameraOperationalActivityRegistry::DispatchResult::Success);
+    first->blockStop();
+    second->blockStop();
+
+    const auto ticket = globalStopAllAdmissionGate().beginStopAll();
+    std::thread first_stop([&] {
+        EXPECT_TRUE(registry.stopActivitiesForDevice(first->id()));
+    });
+    first->waitForStopEntered();
+    const auto snapshot_started = std::chrono::steady_clock::now();
+    EXPECT_EQ(
+        registry.trackedDeviceIds(),
+        (std::vector<std::string>{"camera.first", "camera.second"}));
+    EXPECT_LT(
+        std::chrono::steady_clock::now() - snapshot_started,
+        std::chrono::milliseconds(100));
+    std::thread second_stop([&] {
+        EXPECT_TRUE(registry.stopActivitiesForDevice(second->id()));
+    });
+    second->waitForStopEntered();
+
+    std::atomic<bool> matching_stop_returned{false};
+    std::thread matching_stop([&] {
+        EXPECT_TRUE(registry.stopActivitiesForDevice(first->id()));
+        matching_stop_returned = true;
+    });
+    std::this_thread::yield();
+    EXPECT_FALSE(matching_stop_returned.load());
+
+    second->releaseStop();
+    second_stop.join();
+    first->releaseStop();
+    first_stop.join();
+    matching_stop.join();
+    EXPECT_TRUE(matching_stop_returned.load());
+    EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true));
+}
+
+} // namespace
+} // namespace cmvr::service

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

@@ -0,0 +1,264 @@
+#include "service/grpc/include/camera_ptz_activity_registry.h"
+
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <mutex>
+#include <thread>
+#include <utility>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "service/stop_all/include/stop_all_admission_gate.h"
+
+namespace cmvr::service {
+namespace {
+
+class TestCamera final : public device::AbstractCamera {
+public:
+    struct Call {
+        device::PtzCommand command;
+        bool stop;
+        int speed;
+    };
+
+    explicit TestCamera(std::string id)
+    {
+        id_ = std::move(id);
+    }
+
+    std::string typeName() const override { return "TestCamera"; }
+    void getState(device::CameraState& state) override { state = {}; }
+
+    bool controlPtz(
+        const device::PtzCommand command,
+        const bool stop,
+        const int speed) override
+    {
+        std::unique_lock lock(mutex_);
+        calls_.push_back({command, stop, speed});
+        if (stop) {
+            stop_entered_ = true;
+            condition_.notify_all();
+            condition_.wait(lock, [this] { return !block_stop_; });
+        }
+        return !(stop && fail_stop_);
+    }
+
+    bool stop() override
+    {
+        ++lifecycle_stop_calls_;
+        return true;
+    }
+
+    std::vector<Call> calls() const
+    {
+        std::lock_guard lock(mutex_);
+        return calls_;
+    }
+
+    void setFailStop(const bool fail) { fail_stop_ = fail; }
+    void blockStop()
+    {
+        std::lock_guard lock(mutex_);
+        block_stop_ = true;
+        stop_entered_ = false;
+    }
+    void waitForStopEntered()
+    {
+        std::unique_lock lock(mutex_);
+        condition_.wait(lock, [this] { return stop_entered_; });
+    }
+    void releaseStop()
+    {
+        {
+            std::lock_guard lock(mutex_);
+            block_stop_ = false;
+        }
+        condition_.notify_all();
+    }
+    int lifecycleStopCalls() const { return lifecycle_stop_calls_; }
+
+private:
+    mutable std::mutex mutex_;
+    std::condition_variable condition_;
+    std::vector<Call> calls_;
+    bool fail_stop_{false};
+    bool block_stop_{false};
+    bool stop_entered_{false};
+    int lifecycle_stop_calls_{0};
+};
+
+class CameraPtzActivityRegistryTest : public ::testing::Test {
+protected:
+    void SetUp() override
+    {
+        globalStopAllAdmissionGate().clearForTesting();
+    }
+
+    void TearDown() override
+    {
+        globalStopAllAdmissionGate().clearForTesting();
+    }
+};
+
+TEST_F(CameraPtzActivityRegistryTest,
+       StopAllMatchesEveryActiveStartAndNeverStopsLifecycle)
+{
+    CameraPtzActivityRegistry registry;
+    auto camera = std::make_shared<TestCamera>("camera");
+
+    EXPECT_EQ(
+        registry.control(
+            camera->id(), camera, device::PtzCommand::PanLeft, false, 4),
+        CameraPtzActivityRegistry::DispatchResult::Success);
+    EXPECT_EQ(
+        registry.control(
+            camera->id(), camera, device::PtzCommand::ZoomIn, false, 7),
+        CameraPtzActivityRegistry::DispatchResult::Success);
+
+    const auto ticket = globalStopAllAdmissionGate().beginStopAll();
+    ASSERT_TRUE(ticket.valid());
+    EXPECT_TRUE(registry.stopAllActivities());
+    EXPECT_EQ(registry.activeCommandCount(), 0U);
+    EXPECT_EQ(camera->lifecycleStopCalls(), 0);
+    ASSERT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true));
+
+    const auto calls = camera->calls();
+    ASSERT_EQ(calls.size(), 4U);
+    EXPECT_FALSE(calls[0].stop);
+    EXPECT_FALSE(calls[1].stop);
+    EXPECT_TRUE(calls[2].stop);
+    EXPECT_TRUE(calls[3].stop);
+}
+
+TEST_F(CameraPtzActivityRegistryTest,
+       FailedStopRemainsTrackedForRetryAndAdmissionRecovers)
+{
+    CameraPtzActivityRegistry registry;
+    auto camera = std::make_shared<TestCamera>("camera");
+    ASSERT_EQ(
+        registry.control(
+            camera->id(), camera, device::PtzCommand::TiltUp, false, 3),
+        CameraPtzActivityRegistry::DispatchResult::Success);
+
+    auto ticket = globalStopAllAdmissionGate().beginStopAll();
+    camera->setFailStop(true);
+    std::vector<std::string> failures;
+    EXPECT_FALSE(registry.stopAllActivities(&failures));
+    EXPECT_EQ(registry.activeCommandCount(), 1U);
+    ASSERT_FALSE(failures.empty());
+    EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, false) == false);
+
+    EXPECT_EQ(
+        registry.control(
+            camera->id(), camera, device::PtzCommand::ZoomOut, false, 5),
+        CameraPtzActivityRegistry::DispatchResult::RejectedByStopAll);
+
+    ticket = globalStopAllAdmissionGate().beginStopAll();
+    camera->setFailStop(false);
+    EXPECT_TRUE(registry.stopAllActivities());
+    EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true));
+    EXPECT_EQ(
+        registry.control(
+            camera->id(), camera, device::PtzCommand::ZoomOut, false, 5),
+        CameraPtzActivityRegistry::DispatchResult::Success);
+}
+
+TEST_F(CameraPtzActivityRegistryTest,
+       StopForDeviceIsSelectiveAndActiveIdsReflectFailures)
+{
+    CameraPtzActivityRegistry registry;
+    auto first = std::make_shared<TestCamera>("camera.first");
+    auto second = std::make_shared<TestCamera>("camera.second");
+    ASSERT_EQ(
+        registry.control(
+            first->id(), first, device::PtzCommand::PanLeft, false, 4),
+        CameraPtzActivityRegistry::DispatchResult::Success);
+    ASSERT_EQ(
+        registry.control(
+            second->id(), second, device::PtzCommand::ZoomIn, false, 7),
+        CameraPtzActivityRegistry::DispatchResult::Success);
+    EXPECT_EQ(
+        registry.activeDeviceIds(),
+        (std::vector<std::string>{"camera.first", "camera.second"}));
+    EXPECT_EQ(
+        registry.trackedDeviceIds(),
+        (std::vector<std::string>{"camera.first", "camera.second"}));
+
+    const auto ticket = globalStopAllAdmissionGate().beginStopAll();
+    first->setFailStop(true);
+    std::vector<std::string> failures;
+    EXPECT_FALSE(registry.stopActivitiesForDevice(first->id(), &failures));
+    EXPECT_EQ(registry.activeCommandCount(), 2U);
+    EXPECT_EQ(failures.size(), 1U);
+    EXPECT_EQ(second->calls().size(), 1U);
+
+    first->setFailStop(false);
+    EXPECT_TRUE(registry.stopActivitiesForDevice(first->id()));
+    EXPECT_EQ(registry.activeCommandCount(), 1U);
+    EXPECT_EQ(
+        registry.activeDeviceIds(),
+        (std::vector<std::string>{"camera.second"}));
+    EXPECT_TRUE(registry.stopActivitiesForDevice("missing"));
+    EXPECT_TRUE(registry.stopAllActivities());
+    EXPECT_TRUE(registry.activeDeviceIds().empty());
+    EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true));
+    EXPECT_EQ(first->lifecycleStopCalls(), 0);
+    EXPECT_EQ(second->lifecycleStopCalls(), 0);
+}
+
+TEST_F(CameraPtzActivityRegistryTest,
+       DifferentDevicesStopConcurrentlyButMatchingControlIsSerialized)
+{
+    CameraPtzActivityRegistry registry;
+    auto first = std::make_shared<TestCamera>("camera.first");
+    auto second = std::make_shared<TestCamera>("camera.second");
+    ASSERT_EQ(
+        registry.control(
+            first->id(), first, device::PtzCommand::PanLeft, false, 4),
+        CameraPtzActivityRegistry::DispatchResult::Success);
+    ASSERT_EQ(
+        registry.control(
+            second->id(), second, device::PtzCommand::ZoomIn, false, 7),
+        CameraPtzActivityRegistry::DispatchResult::Success);
+    first->blockStop();
+    second->blockStop();
+
+    const auto ticket = globalStopAllAdmissionGate().beginStopAll();
+    std::thread first_stop([&] {
+        EXPECT_TRUE(registry.stopActivitiesForDevice(first->id()));
+    });
+    first->waitForStopEntered();
+    const auto snapshot_started = std::chrono::steady_clock::now();
+    EXPECT_EQ(
+        registry.trackedDeviceIds(),
+        (std::vector<std::string>{"camera.first", "camera.second"}));
+    EXPECT_LT(
+        std::chrono::steady_clock::now() - snapshot_started,
+        std::chrono::milliseconds(100));
+    std::thread second_stop([&] {
+        EXPECT_TRUE(registry.stopActivitiesForDevice(second->id()));
+    });
+    second->waitForStopEntered();
+
+    std::atomic<bool> matching_stop_returned{false};
+    std::thread matching_stop([&] {
+        EXPECT_TRUE(registry.stopActivitiesForDevice(first->id()));
+        matching_stop_returned = true;
+    });
+    std::this_thread::yield();
+    EXPECT_FALSE(matching_stop_returned.load());
+
+    second->releaseStop();
+    second_stop.join();
+    first->releaseStop();
+    first_stop.join();
+    matching_stop.join();
+    EXPECT_TRUE(matching_stop_returned.load());
+    EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true));
+}
+
+} // namespace
+} // namespace cmvr::service

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

@@ -1,8 +1,10 @@
 #include "service/grpc/include/grpc_agv_service.h"
 
 #include <chrono>
+#include <future>
 #include <memory>
 #include <string>
+#include <thread>
 #include <vector>
 
 #include <grpcpp/grpcpp.h>
@@ -11,6 +13,7 @@
 #include "cmvr/config/device_manager_config/device_manager_config.pb.h"
 #include "manager/control_authority/include/control_authority_manager.h"
 #include "manager/device_manager/include/device_manager.h"
+#include "service/stop_all/include/stop_all_admission_gate.h"
 
 namespace cmvr::service {
 namespace {
@@ -42,12 +45,18 @@ public:
         const device::AgvMotionOptions& options,
         const device::AgvAdapterParams&) override
     {
-        pose_ = pose;
-        pose_options_ = options;
+        ++navigate_pose_calls_;
         pose_cancellation_bound_ =
             static_cast<bool>(options.cancellation_requested);
         pose_cancellation_requested_during_call_ =
             pose_cancellation_bound_ && options.cancellation_requested();
+        if (pose_cancellation_requested_during_call_) {
+            return device::AgvResult::failure(
+                device::AgvErrorCode::TaskCanceled,
+                "navigation canceled before fake device dispatch");
+        }
+        pose_ = pose;
+        pose_options_ = options;
         pose_options_.cancellation_requested = {};
         return pose_result_;
     }
@@ -84,6 +93,7 @@ public:
 
     device::AgvResult setVelocity(const device::AgvVelocity&) override
     {
+        ++set_velocity_calls_;
         return device::AgvResult::failure(
             device::AgvErrorCode::CommandFailed,
             kNativeErrorMessage);
@@ -124,7 +134,7 @@ public:
         return !probe.acquired;
     }
 
-    math::Pose2d pose_;
+    math::Pose2d pose_{};
     device::AgvMotionOptions pose_options_;
     device::AgvResult pose_result_{device::AgvResult::success()};
     std::string station_id_;
@@ -142,6 +152,8 @@ public:
     int cancel_navigation_calls_{0};
     int stop_velocity_calls_{0};
     int confirm_stopped_calls_{0};
+    int set_velocity_calls_{0};
+    int navigate_pose_calls_{0};
     bool emergency_stop_barrier_observed_{false};
     bool cancel_navigation_barrier_observed_{false};
     bool stop_velocity_barrier_observed_{false};
@@ -169,6 +181,7 @@ protected:
     void SetUp() override
     {
         control::ControlAuthorityManager::instance().clear();
+        globalStopAllAdmissionGate().clearForTesting();
         config::DeviceManagerConfig config;
         auto& manager = device::DeviceManager::getInstance(config);
         agv_ = std::make_shared<FakeAgv>();
@@ -182,6 +195,7 @@ protected:
         agv_.reset();
         device::DeviceManager::destroyInstance();
         control::ControlAuthorityManager::instance().clear();
+        globalStopAllAdmissionGate().clearForTesting();
     }
 
     std::shared_ptr<FakeAgv> agv_;
@@ -402,6 +416,104 @@ TEST_F(GrpcAgvServiceTest, ActionLeaseBlocksOrdinaryMutatingRpcs)
     authority.release(action_lease.token);
 }
 
+TEST_F(GrpcAgvServiceTest, NavigationCancellationIncludesControlLease)
+{
+    auto& authority = control::ControlAuthorityManager::instance();
+    const auto barrier = authority.preemptAcquire(
+        "test-agv", "stop-all-test", std::chrono::hours(1));
+    ASSERT_TRUE(barrier.acquired) << barrier.detail;
+
+    api::AgvNavigateToPoseCommand_Request request;
+    request.mutable_header()->set_device_id("test-agv");
+    api::AgvNavigateToPoseCommand_Feedback response;
+    grpc::ServerContext context;
+    const auto status = service_->navigateToPose(
+        &context, &request, &response);
+
+    EXPECT_EQ(
+        status.error_code(),
+        grpc::StatusCode::FAILED_PRECONDITION);
+    EXPECT_EQ(agv_->navigate_pose_calls_, 0);
+    authority.release(barrier.token);
+}
+
+TEST_F(GrpcAgvServiceTest, SetVelocityDispatchRunsUnderControlFence)
+{
+    api::AgvSetVelocityCommand_Request request;
+    request.mutable_header()->set_device_id("test-agv");
+    request.mutable_velocity()->set_vx(0.1);
+    api::AgvSetVelocityCommand_Feedback response;
+    grpc::ServerContext context;
+
+    const auto status = service_->setVelocity(
+        &context, &request, &response);
+
+    EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL);
+    EXPECT_EQ(agv_->set_velocity_calls_, 1);
+}
+
+TEST_F(GrpcAgvServiceTest,
+       StopAllGateRejectsMutatingCommandsButAllowsReadsAndStops)
+{
+    auto& admission = globalStopAllAdmissionGate();
+    const auto ticket = admission.beginStopAll();
+    ASSERT_TRUE(ticket.valid());
+
+    api::AgvNavigateToPoseCommand_Request navigation_request;
+    navigation_request.mutable_header()->set_device_id("test-agv");
+    api::AgvNavigateToPoseCommand_Feedback navigation_response;
+    grpc::ServerContext navigation_context;
+    const auto navigation_status = service_->navigateToPose(
+        &navigation_context, &navigation_request, &navigation_response);
+
+    api::AgvSetVelocityCommand_Request velocity_request;
+    velocity_request.mutable_header()->set_device_id("test-agv");
+    velocity_request.mutable_velocity()->set_vx(0.1);
+    api::AgvSetVelocityCommand_Feedback velocity_response;
+    grpc::ServerContext velocity_context;
+    const auto velocity_status = service_->setVelocity(
+        &velocity_context, &velocity_request, &velocity_response);
+
+    api::AgvRuntimeStateCommand_Request state_request;
+    state_request.mutable_header()->set_device_id("test-agv");
+    api::AgvRuntimeStateCommand_Feedback state_response;
+    grpc::ServerContext state_context;
+    const auto state_status = service_->getRuntimeState(
+        &state_context, &state_request, &state_response);
+
+    api::CommandHeader_Request stop_request;
+    stop_request.set_device_id("test-agv");
+    api::CommandHeader_Feedback stop_response;
+    grpc::ServerContext stop_context;
+    const auto stop_status = service_->cancelNavigation(
+        &stop_context, &stop_request, &stop_response);
+
+    EXPECT_EQ(
+        navigation_status.error_code(),
+        grpc::StatusCode::UNAVAILABLE);
+    EXPECT_FALSE(navigation_response.header().success());
+    EXPECT_EQ(agv_->navigate_pose_calls_, 0);
+    EXPECT_EQ(
+        velocity_status.error_code(),
+        grpc::StatusCode::UNAVAILABLE);
+    EXPECT_FALSE(velocity_response.header().success());
+    EXPECT_EQ(agv_->set_velocity_calls_, 0);
+
+    EXPECT_TRUE(state_status.ok()) << state_status.error_message();
+    EXPECT_TRUE(state_response.header().success());
+    EXPECT_TRUE(stop_status.ok()) << stop_status.error_message();
+    EXPECT_TRUE(stop_response.success())
+        << stop_response.error_message();
+    EXPECT_EQ(agv_->cancel_navigation_calls_, 2);
+
+    EXPECT_TRUE(admission.finishStopAll(ticket, true));
+    const auto resumed_status = service_->navigateToPose(
+        &navigation_context, &navigation_request, &navigation_response);
+    EXPECT_TRUE(resumed_status.ok()) << resumed_status.error_message();
+    EXPECT_TRUE(navigation_response.header().success());
+    EXPECT_EQ(agv_->navigate_pose_calls_, 1);
+}
+
 TEST_F(GrpcAgvServiceTest, QueriesBypassAndSafetyStopsPreemptActionLease)
 {
     auto& authority = control::ControlAuthorityManager::instance();
@@ -428,60 +540,99 @@ TEST_F(GrpcAgvServiceTest, QueriesBypassAndSafetyStopsPreemptActionLease)
 
     api::CommandHeader_Feedback emergency_response;
     grpc::ServerContext emergency_context;
-    const auto emergency_status = service_->emergencyStop(
-        &emergency_context,
-        &stop_request,
-        &emergency_response);
-    EXPECT_TRUE(emergency_status.ok()) << emergency_status.error_message();
+    auto emergency = std::async(
+        std::launch::async,
+        [this, &emergency_context, &stop_request, &emergency_response]() {
+            return service_->emergencyStop(
+                &emergency_context,
+                &stop_request,
+                &emergency_response);
+        });
+    const auto emergency_deadline =
+        std::chrono::steady_clock::now() + std::chrono::seconds(1);
+    while (authority.validate(action_lease.token) &&
+           std::chrono::steady_clock::now() < emergency_deadline) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
     EXPECT_FALSE(authority.validate(action_lease.token));
+    authority.release(action_lease.token);
+    const auto emergency_status = emergency.get();
+    EXPECT_TRUE(emergency_status.ok()) << emergency_status.error_message();
     EXPECT_TRUE(agv_->emergency_stop_barrier_observed_);
 
-    const auto lease_after_emergency = authority.tryAcquire(
+    const auto released_lease_after_emergency = authority.tryAcquire(
         "test-agv",
-        "action-sequence:after-emergency",
+        "action-sequence:after-emergency-release",
         std::chrono::hours(1));
-    ASSERT_TRUE(lease_after_emergency.acquired)
-        << lease_after_emergency.detail;
+    ASSERT_TRUE(released_lease_after_emergency.acquired)
+        << released_lease_after_emergency.detail;
 
     api::CommandHeader_Feedback cancel_response;
     grpc::ServerContext cancel_context;
-    const auto cancel_status = service_->cancelNavigation(
-        &cancel_context,
-        &stop_request,
-        &cancel_response);
+    auto cancel = std::async(
+        std::launch::async,
+        [this, &cancel_context, &stop_request, &cancel_response]() {
+            return service_->cancelNavigation(
+                &cancel_context,
+                &stop_request,
+                &cancel_response);
+        });
+    const auto cancel_deadline =
+        std::chrono::steady_clock::now() + std::chrono::seconds(1);
+    while (authority.validate(released_lease_after_emergency.token) &&
+           std::chrono::steady_clock::now() < cancel_deadline) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+    EXPECT_FALSE(authority.validate(
+        released_lease_after_emergency.token));
+    authority.release(released_lease_after_emergency.token);
+    const auto cancel_status = cancel.get();
     EXPECT_TRUE(cancel_status.ok()) << cancel_status.error_message();
-    EXPECT_FALSE(authority.validate(lease_after_emergency.token));
     EXPECT_TRUE(agv_->cancel_navigation_barrier_observed_);
 
-    const auto lease_after_cancel = authority.tryAcquire(
+    const auto released_lease_after_cancel = authority.tryAcquire(
         "test-agv",
-        "action-sequence:after-cancel",
+        "action-sequence:after-cancel-release",
         std::chrono::hours(1));
-    ASSERT_TRUE(lease_after_cancel.acquired)
-        << lease_after_cancel.detail;
+    ASSERT_TRUE(released_lease_after_cancel.acquired)
+        << released_lease_after_cancel.detail;
 
     api::CommandHeader_Feedback velocity_response;
     grpc::ServerContext velocity_context;
-    const auto velocity_status = service_->stopVelocityControl(
-        &velocity_context,
-        &stop_request,
-        &velocity_response);
+    auto velocity = std::async(
+        std::launch::async,
+        [this, &velocity_context, &stop_request, &velocity_response]() {
+            return service_->stopVelocityControl(
+                &velocity_context,
+                &stop_request,
+                &velocity_response);
+        });
+    const auto velocity_deadline =
+        std::chrono::steady_clock::now() + std::chrono::seconds(1);
+    while (authority.validate(released_lease_after_cancel.token) &&
+           std::chrono::steady_clock::now() < velocity_deadline) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+    EXPECT_FALSE(authority.validate(
+        released_lease_after_cancel.token));
+    authority.release(released_lease_after_cancel.token);
+    const auto velocity_status = velocity.get();
     EXPECT_TRUE(velocity_status.ok()) << velocity_status.error_message();
-    EXPECT_FALSE(authority.validate(lease_after_cancel.token));
     EXPECT_TRUE(agv_->stop_velocity_barrier_observed_);
 
-    EXPECT_EQ(agv_->emergency_stop_calls_, 1);
-    EXPECT_EQ(agv_->cancel_navigation_calls_, 1);
-    EXPECT_EQ(agv_->stop_velocity_calls_, 1);
-    EXPECT_EQ(agv_->confirm_stopped_calls_, 3);
-    EXPECT_TRUE(agv_->confirm_stopped_barrier_observed_);
-
     const auto lease_after_stops = authority.tryAcquire(
         "test-agv",
         "action-sequence:after-stops",
         std::chrono::hours(1));
     ASSERT_TRUE(lease_after_stops.acquired)
         << lease_after_stops.detail;
+
+    EXPECT_EQ(agv_->emergency_stop_calls_, 2);
+    EXPECT_EQ(agv_->cancel_navigation_calls_, 2);
+    EXPECT_EQ(agv_->stop_velocity_calls_, 2);
+    EXPECT_EQ(agv_->confirm_stopped_calls_, 3);
+    EXPECT_TRUE(agv_->confirm_stopped_barrier_observed_);
+
     authority.release(lease_after_stops.token);
 }
 
@@ -513,6 +664,39 @@ TEST_F(GrpcAgvServiceTest,
             "normal-control-after-unconfirmed-stop",
             std::chrono::hours(1));
     EXPECT_FALSE(lease.acquired);
+
+    auto& authority = control::ControlAuthorityManager::instance();
+    const auto recovery = authority.preemptAcquire(
+        "test-agv", "confirmed-stop-recovery", std::chrono::hours(1));
+    ASSERT_TRUE(recovery.acquired) << recovery.detail;
+    ASSERT_TRUE(authority.waitForPreemptedRelease(
+        recovery.token, std::chrono::milliseconds::zero()));
+    ASSERT_TRUE(authority.recoverRetiredSafetyHolders(recovery.token));
+    authority.release(recovery.token);
+    const auto recovered = authority.tryAcquire(
+        "test-agv", "normal-control-after-recovery", std::chrono::hours(1));
+    EXPECT_TRUE(recovered.acquired) << recovered.detail;
+    authority.release(recovered.token);
+}
+
+TEST_F(GrpcAgvServiceTest, StopMappingFailureReleasesOrdinaryControlLease)
+{
+    api::CommandHeader_Request request;
+    request.set_device_id("test-agv");
+    api::CommandHeader_Feedback response;
+    grpc::ServerContext context;
+
+    const auto status = service_->stopMapping(
+        &context, &request, &response);
+
+    EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL);
+    EXPECT_FALSE(response.success());
+    auto& authority = control::ControlAuthorityManager::instance();
+    const auto lease = authority.tryAcquire(
+        "test-agv", "normal-control-after-mapping-failure",
+        std::chrono::hours(1));
+    EXPECT_TRUE(lease.acquired) << lease.detail;
+    authority.release(lease.token);
 }
 
 TEST_F(GrpcAgvServiceTest, NavigateToStationForwardsPgvAdapterParams)

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

@@ -18,6 +18,7 @@
 #include "cmvr/config/device_manager_config/device_manager_config.pb.h"
 #include "manager/control_authority/include/control_authority_manager.h"
 #include "manager/device_manager/include/device_manager.h"
+#include "service/stop_all/include/stop_all_admission_gate.h"
 
 namespace cmvr::service {
 namespace {
@@ -91,9 +92,9 @@ public:
     bool isFault() const override { return false; }
 
     device::Result moveJ(const device::JointPositionCommand&,
-                         const device::MotionOptions&) override
+                         const device::MotionOptions& options) override
     {
-        return enterMotion("moveJ", move_j_calls_);
+        return enterMotion("moveJ", move_j_calls_, options);
     }
     device::Result speedJ(const device::JointVelocityCommand&,
                           double,
@@ -107,10 +108,10 @@ public:
     }
     device::Result moveL(
         const device::CartesianPose&,
-        const device::MotionOptions&,
+        const device::MotionOptions& options,
         device::FrameType = device::FrameType::Base) override
     {
-        return enterMotion("moveL", move_l_calls_);
+        return enterMotion("moveL", move_l_calls_, options);
     }
     device::Result speedL(
         const device::CartesianVelocity&,
@@ -243,6 +244,18 @@ public:
         return torque_off_calls_;
     }
 
+    bool lastMotionHadCancellation() const
+    {
+        std::lock_guard lock(motion_mutex_);
+        return last_motion_had_cancellation_;
+    }
+
+    bool lastMotionCancellationRequested() const
+    {
+        std::lock_guard lock(motion_mutex_);
+        return last_motion_cancellation_requested_;
+    }
+
     device::Result startServoMode(const device::ServoOptions&) override
     {
         return device::Result::success();
@@ -344,10 +357,18 @@ public:
     std::string last_request_json;
 
 private:
-    device::Result enterMotion(const char* operation, int& call_count)
+    device::Result enterMotion(
+        const char* operation,
+        int& call_count,
+        const device::MotionOptions& options)
     {
         std::unique_lock lock(motion_mutex_);
         ++call_count;
+        last_motion_had_cancellation_ =
+            static_cast<bool>(options.cancellation_requested);
+        last_motion_cancellation_requested_ =
+            last_motion_had_cancellation_ &&
+            options.cancellation_requested();
         if (!block_next_motion_) {
             return device::Result::success();
         }
@@ -380,6 +401,8 @@ private:
     int move_l_calls_{0};
     int stop_motion_calls_{0};
     int torque_off_calls_{0};
+    bool last_motion_had_cancellation_{false};
+    bool last_motion_cancellation_requested_{false};
 };
 
 class JsonCommandNonArmDevice final : public device::AbstractDevice {
@@ -407,6 +430,7 @@ protected:
     void SetUp() override
     {
         control::ControlAuthorityManager::instance().clear();
+        globalStopAllAdmissionGate().clearForTesting();
         device::DeviceManager::destroyInstance();
         config::DeviceManagerConfig config;
         auto& manager = device::DeviceManager::getInstance(config);
@@ -428,6 +452,7 @@ protected:
         left_arm_.reset();
         device::DeviceManager::destroyInstance();
         control::ControlAuthorityManager::instance().clear();
+        globalStopAllAdmissionGate().clearForTesting();
     }
 
     grpc::Status execute(const std::string& device_id,
@@ -610,9 +635,10 @@ TEST_F(GrpcArmServiceTest,
     grpc::Status torque_off_status;
     api::CommandHeader_Feedback stop_response;
     grpc::Status stop_status;
-    MoveOutcome resumed_move;
+    MoveOutcome before_retired_handler_release;
     bool stop_started = false;
     std::future<grpc::Status> blocked_stop;
+    std::future<grpc::Status> torque_off;
     if (move_started) {
         conflict = moveL("aubo_arm");
         aubo_arm_->blockNextStopMotion();
@@ -624,13 +650,15 @@ TEST_F(GrpcArmServiceTest,
         stop_started = aubo_arm_->waitForBlockingStop(
             std::chrono::seconds(2));
         if (stop_started) {
-            torque_off_status = torqueOff(
-                "aubo_arm", torque_off_response);
+            torque_off = std::async(
+                std::launch::async,
+                [this, &torque_off_response]() {
+                    return torqueOff("aubo_arm", torque_off_response);
+                });
             during_stop = moveL("aubo_arm");
         }
         aubo_arm_->releaseBlockingStop();
-        stop_status = blocked_stop.get();
-        resumed_move = moveL("aubo_arm");
+        before_retired_handler_release = moveL("aubo_arm");
     }
 
     // Keep the original RPC active until after the replacement MoveL has
@@ -638,6 +666,13 @@ TEST_F(GrpcArmServiceTest,
     // takes time to unwind and guards the lease hand-off itself.
     aubo_arm_->releaseBlockingMotion();
     const auto original_move = blocked_move.get();
+    if (blocked_stop.valid()) {
+        stop_status = blocked_stop.get();
+    }
+    if (torque_off.valid()) {
+        torque_off_status = torque_off.get();
+    }
+    const auto resumed_move = moveL("aubo_arm");
 
     ASSERT_TRUE(move_started);
     ASSERT_TRUE(stop_started);
@@ -655,6 +690,9 @@ TEST_F(GrpcArmServiceTest,
     EXPECT_TRUE(stop_status.ok()) << stop_status.error_message();
     EXPECT_TRUE(stop_response.success())
         << stop_response.error_message();
+    EXPECT_EQ(
+        before_retired_handler_release.status.error_code(),
+        grpc::StatusCode::FAILED_PRECONDITION);
     EXPECT_TRUE(resumed_move.status.ok())
         << resumed_move.status.error_message();
     EXPECT_TRUE(resumed_move.response_success)
@@ -665,8 +703,67 @@ TEST_F(GrpcArmServiceTest,
         << original_move.response_error;
     EXPECT_EQ(aubo_arm_->moveJCalls(), 1);
     EXPECT_EQ(aubo_arm_->moveLCalls(), 1);
-    EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1);
-    EXPECT_EQ(aubo_arm_->torqueOffCalls(), 1);
+    EXPECT_EQ(aubo_arm_->stopMotionCalls(), 2);
+    EXPECT_EQ(aubo_arm_->torqueOffCalls(), 2);
+}
+
+TEST_F(GrpcArmServiceTest, MoveBindsLeaseRevocationCancellation)
+{
+    const auto outcome = moveJ("aubo_arm");
+
+    ASSERT_TRUE(outcome.status.ok())
+        << outcome.status.error_message();
+    EXPECT_TRUE(outcome.response_success) << outcome.response_error;
+    EXPECT_TRUE(aubo_arm_->lastMotionHadCancellation());
+    EXPECT_FALSE(aubo_arm_->lastMotionCancellationRequested());
+}
+
+TEST_F(GrpcArmServiceTest,
+       StopAllGateRejectsMutatingCommandsButAllowsReadsAndStops)
+{
+    auto& admission = globalStopAllAdmissionGate();
+    const auto ticket = admission.beginStopAll();
+    ASSERT_TRUE(ticket.valid());
+
+    const auto rejected_move = moveJ("aubo_arm");
+
+    api::JsonDeviceCommand_Feedback json_response;
+    const auto json_status = execute(
+        "aubo_arm", R"({"command":"cabinet_io"})", json_response);
+
+    api::JointRequest state_request;
+    state_request.mutable_header()->set_device_id("aubo_arm");
+    api::JointResponse state_response;
+    grpc::ServerContext state_context;
+    const auto state_status = service_->getJointState(
+        &state_context, &state_request, &state_response);
+
+    api::CommandHeader_Feedback stop_response;
+    const auto stop_status = stopMotion("aubo_arm", stop_response);
+
+    EXPECT_EQ(
+        rejected_move.status.error_code(),
+        grpc::StatusCode::UNAVAILABLE);
+    EXPECT_FALSE(rejected_move.response_success);
+    EXPECT_EQ(aubo_arm_->moveJCalls(), 0);
+    EXPECT_EQ(json_status.error_code(), grpc::StatusCode::UNAVAILABLE);
+    EXPECT_FALSE(json_response.header().success());
+    EXPECT_EQ(aubo_arm_->execute_calls, 0);
+
+    EXPECT_TRUE(state_status.ok()) << state_status.error_message();
+    EXPECT_TRUE(state_response.header().success());
+    EXPECT_TRUE(stop_status.ok()) << stop_status.error_message();
+    EXPECT_TRUE(stop_response.success())
+        << stop_response.error_message();
+    EXPECT_EQ(aubo_arm_->stopMotionCalls(), 2);
+
+    EXPECT_TRUE(admission.finishStopAll(ticket, true));
+    const auto resumed_move = moveJ("aubo_arm");
+    EXPECT_TRUE(resumed_move.status.ok())
+        << resumed_move.status.error_message();
+    EXPECT_TRUE(resumed_move.response_success)
+        << resumed_move.response_error;
+    EXPECT_EQ(aubo_arm_->moveJCalls(), 1);
 }
 
 TEST_F(GrpcArmServiceTest,
@@ -683,15 +780,22 @@ TEST_F(GrpcArmServiceTest,
     MoveOutcome conflict;
     api::CommandHeader_Feedback stop_response;
     grpc::Status stop_status;
-    MoveOutcome resumed_move;
+    MoveOutcome before_retired_handler_release;
     if (move_started) {
         conflict = moveJ("aubo_arm");
-        stop_status = stopMotion("aubo_arm", stop_response);
-        resumed_move = moveJ("aubo_arm");
+        auto stop = std::async(
+            std::launch::async,
+            [this, &stop_response]() {
+                return stopMotion("aubo_arm", stop_response);
+            });
+        before_retired_handler_release = moveJ("aubo_arm");
+        aubo_arm_->releaseBlockingMotion();
+        stop_status = stop.get();
     }
 
     aubo_arm_->releaseBlockingMotion();
     const auto original_move = blocked_move.get();
+    const auto resumed_move = moveJ("aubo_arm");
 
     ASSERT_TRUE(move_started);
     EXPECT_EQ(conflict.status.error_code(),
@@ -700,6 +804,9 @@ TEST_F(GrpcArmServiceTest,
     EXPECT_TRUE(stop_status.ok()) << stop_status.error_message();
     EXPECT_TRUE(stop_response.success())
         << stop_response.error_message();
+    EXPECT_EQ(
+        before_retired_handler_release.status.error_code(),
+        grpc::StatusCode::FAILED_PRECONDITION);
     EXPECT_TRUE(resumed_move.status.ok())
         << resumed_move.status.error_message();
     EXPECT_TRUE(resumed_move.response_success)
@@ -710,7 +817,7 @@ TEST_F(GrpcArmServiceTest,
         << original_move.response_error;
     EXPECT_EQ(aubo_arm_->moveJCalls(), 1);
     EXPECT_EQ(aubo_arm_->moveLCalls(), 1);
-    EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1);
+    EXPECT_EQ(aubo_arm_->stopMotionCalls(), 2);
 }
 
 TEST_F(GrpcArmServiceTest, StopMotionFailureRetainsSafetyBarrier)
@@ -734,6 +841,19 @@ TEST_F(GrpcArmServiceTest, StopMotionFailureRetainsSafetyBarrier)
         grpc::StatusCode::FAILED_PRECONDITION);
     EXPECT_EQ(aubo_arm_->moveJCalls(), 0);
     EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1);
+
+    authority.release(action_lease.token);
+    const auto recovery = authority.preemptAcquire(
+        "aubo_arm", "confirmed-stop-recovery", std::chrono::hours(1));
+    ASSERT_TRUE(recovery.acquired) << recovery.detail;
+    ASSERT_TRUE(authority.waitForPreemptedRelease(
+        recovery.token, std::chrono::milliseconds::zero()));
+    ASSERT_TRUE(authority.recoverRetiredSafetyHolders(recovery.token));
+    authority.release(recovery.token);
+    const auto recovered = authority.tryAcquire(
+        "aubo_arm", "move-after-recovery", std::chrono::hours(1));
+    EXPECT_TRUE(recovered.acquired) << recovered.detail;
+    authority.release(recovered.token);
 }
 
 TEST_F(GrpcArmServiceTest, StopMotionExceptionRetainsSafetyBarrier)
@@ -757,6 +877,15 @@ TEST_F(GrpcArmServiceTest, StopMotionExceptionRetainsSafetyBarrier)
         grpc::StatusCode::FAILED_PRECONDITION);
     EXPECT_EQ(aubo_arm_->moveLCalls(), 0);
     EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1);
+
+    authority.release(action_lease.token);
+    const auto recovery = authority.preemptAcquire(
+        "aubo_arm", "confirmed-exception-recovery", std::chrono::hours(1));
+    ASSERT_TRUE(recovery.acquired) << recovery.detail;
+    ASSERT_TRUE(authority.waitForPreemptedRelease(
+        recovery.token, std::chrono::milliseconds::zero()));
+    ASSERT_TRUE(authority.recoverRetiredSafetyHolders(recovery.token));
+    authority.release(recovery.token);
 }
 
 } // namespace

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

@@ -4,6 +4,7 @@
 #include <chrono>
 #include <condition_variable>
 #include <cstdio>
+#include <future>
 #include <memory>
 #include <mutex>
 #include <set>
@@ -16,6 +17,8 @@
 #include <gtest/gtest.h>
 #include <unistd.h>
 
+#include "service/stop_all/include/stop_all_admission_gate.h"
+
 namespace cmvr::service {
 namespace {
 
@@ -667,5 +670,110 @@ TEST(ArmTeleopServiceTest, ExpiredSetpointIsNeverDispatched)
     EXPECT_EQ(applied.front(), 1U);
 }
 
+TEST(ArmTeleopServiceTest,
+     StopAllFencesDispatchRejectsAdmissionAndAllowsReuse)
+{
+    auto& authority = control::ControlAuthorityManager::instance();
+    auto& admission = globalStopAllAdmissionGate();
+    authority.clear();
+    admission.clearForTesting();
+
+    auto backend = std::make_shared<FakeArmTeleopBackend>();
+    TeleopServerHarness harness(backend);
+    grpc::ClientContext active_context;
+    active_context.set_deadline(
+        std::chrono::system_clock::now() + 3s);
+    auto active_stream = harness.stub().Teleoperate(&active_context);
+
+    ASSERT_TRUE(active_stream->Write(
+        makeOpenFrame(makeManifest(), 500, 2000)));
+    expectOpeningFrames(*active_stream);
+
+    backend->blockApply();
+    ASSERT_TRUE(active_stream->Write(makeSetpoint(1, 400000)));
+    ASSERT_TRUE(backend->waitForApply(1, 1s));
+
+    const auto stop_all_ticket = admission.beginStopAll();
+    ASSERT_TRUE(stop_all_ticket.valid());
+
+    grpc::ClientContext blocked_context;
+    blocked_context.set_deadline(
+        std::chrono::system_clock::now() + 2s);
+    auto blocked_stream = harness.stub().Teleoperate(&blocked_context);
+    ASSERT_TRUE(blocked_stream->Write(makeOpenFrame(makeManifest())));
+    ASSERT_TRUE(blocked_stream->WritesDone());
+    arm_teleop::ServerFrame response;
+    ASSERT_TRUE(blocked_stream->Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_REJECTED);
+    const auto blocked_status = blocked_stream->Finish();
+    EXPECT_EQ(
+        blocked_status.error_code(), grpc::StatusCode::UNAVAILABLE);
+    EXPECT_EQ(backend->openCalls(), 1);
+
+    const auto safety_lease = authority.preemptAcquire(
+        makeManifest().robot_id(), "stop-all-test",
+        std::chrono::hours(1));
+    ASSERT_TRUE(safety_lease.acquired) << safety_lease.detail;
+    const auto safety_token = safety_lease.token;
+    auto dispatch_fence = std::async(
+        std::launch::async,
+        [&authority, safety_token] {
+            return authority.waitForPreemptedRelease(safety_token, 1s);
+        });
+
+    EXPECT_EQ(
+        dispatch_fence.wait_for(50ms), std::future_status::timeout);
+    backend->releaseApply();
+    ASSERT_EQ(
+        dispatch_fence.wait_for(1s), std::future_status::ready);
+    EXPECT_TRUE(dispatch_fence.get());
+
+    ASSERT_TRUE(active_stream->Read(&response));
+    EXPECT_EQ(response.status().applied_sequence(), 1U);
+    ASSERT_TRUE(active_stream->Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_LEASE_LOST);
+    EXPECT_EQ(
+        response.status().stop_reason(),
+        arm_teleop::STOP_REASON_LEASE_REVOKED);
+    const auto active_status = active_stream->Finish();
+    EXPECT_TRUE(
+        active_status.error_code() == grpc::StatusCode::ABORTED ||
+        active_status.error_code() == grpc::StatusCode::CANCELLED)
+        << active_status.error_message();
+
+    const auto applied = backend->appliedSequences();
+    ASSERT_EQ(applied.size(), 1U);
+    EXPECT_EQ(applied.front(), 1U);
+    const auto stopped = backend->stopReasons();
+    ASSERT_FALSE(stopped.empty());
+    EXPECT_EQ(
+        stopped.back(), arm_teleop::STOP_REASON_LEASE_REVOKED);
+
+    authority.release(safety_token);
+    ASSERT_TRUE(admission.finishStopAll(stop_all_ticket, true));
+
+    grpc::ClientContext resumed_context;
+    resumed_context.set_deadline(
+        std::chrono::system_clock::now() + 2s);
+    auto resumed_stream = harness.stub().Teleoperate(&resumed_context);
+    ASSERT_TRUE(resumed_stream->Write(makeOpenFrame(makeManifest())));
+    expectOpeningFrames(*resumed_stream);
+    ASSERT_TRUE(resumed_stream->Write(makeStop()));
+    ASSERT_TRUE(resumed_stream->WritesDone());
+    ASSERT_TRUE(resumed_stream->Read(&response));
+    EXPECT_EQ(
+        response.status().phase(),
+        arm_teleop::SESSION_PHASE_STOPPED);
+    EXPECT_TRUE(resumed_stream->Finish().ok());
+    EXPECT_EQ(backend->openCalls(), 2);
+
+    authority.clear();
+    admission.clearForTesting();
+}
+
 } // namespace
 } // namespace cmvr::service

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

@@ -0,0 +1,351 @@
+#include "service/grpc/include/grpc_dexhand_service.h"
+
+#include <array>
+#include <atomic>
+#include <cstdio>
+#include <chrono>
+#include <condition_variable>
+#include <future>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <unistd.h>
+
+#include <grpcpp/grpcpp.h>
+#include <gtest/gtest.h>
+
+#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
+#include "manager/control_authority/include/control_authority_manager.h"
+#include "manager/device_manager/include/device_manager.h"
+#include "service/grpc/include/media_activity_coordinator.h"
+#include "service/stop_all/include/stop_all_admission_gate.h"
+
+namespace cmvr::service {
+namespace {
+
+using namespace std::chrono_literals;
+
+class FakeDexHand final : public device::AbstractDexHand {
+public:
+    FakeDexHand() {
+        id_ = "test-dexhand";
+        sensor_points_[0] = TactilePoint::fromFz(42);
+    }
+
+    std::string typeName() const override { return "FakeDexHand"; }
+    Status state() const override { return Status::STREAMING; }
+    std::string lastError() const override { return {}; }
+
+    bool stopOperationalActivity() override {
+        ++stop_operational_calls;
+        return true;
+    }
+
+    bool resumeOperationalActivity() override {
+        ++resume_operational_calls;
+        return true;
+    }
+
+    void setAngles(const std::vector<int>&) override {
+        ++set_angle_calls;
+        std::unique_lock lock(command_mutex_);
+        command_entered_ = true;
+        command_cv_.notify_all();
+        command_cv_.wait(lock, [this] { return !block_angle_command_; });
+    }
+
+    void setPositions(const std::vector<int>&) override {
+        ++set_position_calls;
+    }
+
+    void setVelocities(const std::vector<int>&) override {
+        ++set_speed_calls;
+    }
+
+    void setForce(const std::vector<int>&) override {
+        ++set_force_calls;
+    }
+
+    void setPresetAct(int) override {
+        ++set_preset_calls;
+    }
+
+    void setTactilePollingRegions(
+        const std::vector<TactileRegionKey>&) override {
+        ++configure_sensor_calls;
+    }
+
+    std::vector<TactileRegionData> getSensorData() override {
+        ++sensor_read_calls;
+        return {sensorRegion()};
+    }
+
+    TactileRegionData getSensorData(
+        FingerType finger, TactileRegion region) override {
+        return finger == FingerType::INDEX && region == TactileRegion::TIP
+            ? sensorRegion()
+            : TactileRegionData{};
+    }
+
+    ResultantForce getResultantForce(
+        FingerType finger, TactileRegion region) override {
+        return finger == FingerType::INDEX && region == TactileRegion::TIP
+            ? sensor_points_[0]
+            : ResultantForce{};
+    }
+
+    void blockAngleCommand() {
+        std::lock_guard lock(command_mutex_);
+        block_angle_command_ = true;
+        command_entered_ = false;
+    }
+
+    bool waitForAngleCommand(const std::chrono::milliseconds timeout) {
+        std::unique_lock lock(command_mutex_);
+        return command_cv_.wait_for(
+            lock, timeout, [this] { return command_entered_; });
+    }
+
+    void releaseAngleCommand() {
+        {
+            std::lock_guard lock(command_mutex_);
+            block_angle_command_ = false;
+        }
+        command_cv_.notify_all();
+    }
+
+    std::atomic<int> set_position_calls{0};
+    std::atomic<int> set_angle_calls{0};
+    std::atomic<int> set_force_calls{0};
+    std::atomic<int> set_speed_calls{0};
+    std::atomic<int> set_preset_calls{0};
+    std::atomic<int> configure_sensor_calls{0};
+    std::atomic<int> sensor_read_calls{0};
+    std::atomic<int> stop_operational_calls{0};
+    std::atomic<int> resume_operational_calls{0};
+
+private:
+    TactileRegionData sensorRegion() {
+        return TactileRegionData(
+            FingerType::INDEX,
+            TactileRegion::TIP,
+            TactileMatrixView{sensor_points_.data(), 1, 1},
+            "fake-tactile");
+    }
+
+    std::array<TactilePoint, 1> sensor_points_{};
+    std::mutex command_mutex_;
+    std::condition_variable command_cv_;
+    bool block_angle_command_{false};
+    bool command_entered_{false};
+};
+
+class GrpcDexHandServiceTest : public ::testing::Test {
+protected:
+    void SetUp() override {
+        device::DeviceManager::destroyInstance();
+        control::ControlAuthorityManager::instance().clear();
+        globalStopAllAdmissionGate().clearForTesting();
+        config::DeviceManagerConfig config;
+        auto& manager = device::DeviceManager::getInstance(config);
+        hand_ = std::make_shared<FakeDexHand>();
+        manager.registerDevice(hand_);
+        service_ = std::make_unique<gRPCDexHandServiceImpl>();
+    }
+
+    void TearDown() override {
+        hand_->releaseAngleCommand();
+        if (server_) {
+            server_->Shutdown();
+            server_->Wait();
+        }
+        stub_.reset();
+        service_.reset();
+        hand_.reset();
+        if (!socket_path_.empty()) {
+            std::remove(socket_path_.c_str());
+        }
+        device::DeviceManager::destroyInstance();
+        control::ControlAuthorityManager::instance().clear();
+        globalStopAllAdmissionGate().clearForTesting();
+    }
+
+    bool startGrpcServer() {
+        socket_path_ =
+            "/tmp/cmvr_dexhand_service_test_" +
+            std::to_string(static_cast<long long>(::getpid())) + ".sock";
+        std::remove(socket_path_.c_str());
+        const std::string address = "unix:" + socket_path_;
+        grpc::ServerBuilder builder;
+        builder.AddListeningPort(
+            address,
+            grpc::InsecureServerCredentials());
+        builder.RegisterService(service_.get());
+        server_ = builder.BuildAndStart();
+        if (!server_) {
+            return false;
+        }
+        stub_ = api::DexHandService::NewStub(grpc::CreateChannel(
+            address,
+            grpc::InsecureChannelCredentials()));
+        return stub_ != nullptr;
+    }
+
+    static api::GetSensorDataStreamCommand_Request sensorStreamRequest() {
+        api::GetSensorDataStreamCommand_Request request;
+        request.mutable_header()->set_device_id("test-dexhand");
+        return request;
+    }
+
+    std::shared_ptr<FakeDexHand> hand_;
+    std::unique_ptr<gRPCDexHandServiceImpl> service_;
+    std::unique_ptr<grpc::Server> server_;
+    std::unique_ptr<api::DexHandService::Stub> stub_;
+    std::string socket_path_;
+};
+
+TEST_F(GrpcDexHandServiceTest, StopAllGateRejectsEveryControlCommand) {
+    const auto ticket = globalStopAllAdmissionGate().beginStopAll();
+    ASSERT_TRUE(ticket.valid());
+
+    grpc::ServerContext position_context;
+    api::SetDexHandPositionsCommand_Request position_request;
+    api::SetDexHandPositionsCommand_Feedback position_response;
+    position_request.mutable_header()->set_device_id("test-dexhand");
+    EXPECT_TRUE(service_->SetDexHandPos(
+        &position_context, &position_request, &position_response).ok());
+    EXPECT_FALSE(position_response.header().success());
+
+    grpc::ServerContext angle_context;
+    api::SetDexHandAnglesCommand_Request angle_request;
+    api::SetDexHandAnglesCommand_Feedback angle_response;
+    angle_request.mutable_header()->set_device_id("test-dexhand");
+    EXPECT_TRUE(service_->SetDexHandAngle(
+        &angle_context, &angle_request, &angle_response).ok());
+    EXPECT_FALSE(angle_response.header().success());
+
+    grpc::ServerContext force_context;
+    api::SetDexHandForceCommand_Request force_request;
+    api::SetDexHandForceCommand_Feedback force_response;
+    force_request.mutable_header()->set_device_id("test-dexhand");
+    EXPECT_TRUE(service_->SetDexHandForce(
+        &force_context, &force_request, &force_response).ok());
+    EXPECT_FALSE(force_response.header().success());
+
+    grpc::ServerContext speed_context;
+    api::SetDexHandSpeedCommand_Request speed_request;
+    api::SetDexHandSpeedCommand_Feedback speed_response;
+    speed_request.mutable_header()->set_device_id("test-dexhand");
+    EXPECT_TRUE(service_->SetDexHandSpeed(
+        &speed_context, &speed_request, &speed_response).ok());
+    EXPECT_FALSE(speed_response.header().success());
+
+    grpc::ServerContext preset_context;
+    api::SetDexHandPresetActCommand_Request preset_request;
+    api::SetDexHandPresetActCommand_Feedback preset_response;
+    preset_request.mutable_header()->set_device_id("test-dexhand");
+    EXPECT_TRUE(service_->SetDexHandPresetAct(
+        &preset_context, &preset_request, &preset_response).ok());
+    EXPECT_FALSE(preset_response.header().success());
+
+    EXPECT_EQ(hand_->set_position_calls.load(), 0);
+    EXPECT_EQ(hand_->set_angle_calls.load(), 0);
+    EXPECT_EQ(hand_->set_force_calls.load(), 0);
+    EXPECT_EQ(hand_->set_speed_calls.load(), 0);
+    EXPECT_EQ(hand_->set_preset_calls.load(), 0);
+    EXPECT_EQ(hand_->resume_operational_calls.load(), 0);
+    EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true));
+}
+
+TEST_F(GrpcDexHandServiceTest,
+       SafetyPreemptionCannotPassAnExecutingDeviceDispatch) {
+    hand_->blockAngleCommand();
+    grpc::ServerContext context;
+    api::SetDexHandAnglesCommand_Request request;
+    api::SetDexHandAnglesCommand_Feedback response;
+    request.mutable_header()->set_device_id("test-dexhand");
+    request.add_values()->set_value(0.5F);
+
+    auto rpc = std::async(std::launch::async, [&] {
+        return service_->SetDexHandAngle(&context, &request, &response);
+    });
+    ASSERT_TRUE(hand_->waitForAngleCommand(1s));
+
+    auto& authority = control::ControlAuthorityManager::instance();
+    auto safety_result = authority.preemptAcquire(
+        "test-dexhand",
+        "test-stop-all",
+        std::chrono::duration_cast<
+            control::ControlAuthorityManager::Duration>(1h));
+    ASSERT_TRUE(safety_result.acquired);
+    const auto safety_token = safety_result.token;
+    auto dispatch_fence = std::async(std::launch::async,
+        [&authority, safety_token] {
+            return authority.waitForPreemptedRelease(safety_token, 1s);
+        });
+    EXPECT_EQ(
+        dispatch_fence.wait_for(50ms), std::future_status::timeout);
+
+    hand_->releaseAngleCommand();
+    ASSERT_EQ(rpc.wait_for(1s), std::future_status::ready);
+    EXPECT_TRUE(rpc.get().ok());
+    EXPECT_TRUE(response.header().success());
+    ASSERT_EQ(dispatch_fence.wait_for(1s), std::future_status::ready);
+    EXPECT_TRUE(dispatch_fence.get());
+    authority.release(safety_token);
+}
+
+TEST_F(GrpcDexHandServiceTest,
+       StopAllCancelsOldSensorStreamAndARecoveredStreamResumesActivity) {
+    ASSERT_TRUE(startGrpcServer());
+    grpc::ClientContext context;
+    context.set_deadline(std::chrono::system_clock::now() + 3s);
+    auto stream = stub_->GetSensorDataStream(&context);
+    ASSERT_TRUE(stream->Write(sensorStreamRequest()));
+
+    api::GetSensorDataStreamCommand_Feedback feedback;
+    ASSERT_TRUE(stream->Read(&feedback));
+    ASSERT_TRUE(feedback.header().success());
+    ASSERT_GE(hand_->sensor_read_calls.load(), 1);
+
+    const auto admission_ticket = globalStopAllAdmissionGate().beginStopAll();
+    const auto media_ticket = globalMediaActivityCoordinator().beginStopAll();
+    ASSERT_TRUE(admission_ticket.valid());
+    ASSERT_TRUE(media_ticket.valid());
+
+    stream->WritesDone();
+    while (stream->Read(&feedback)) {
+    }
+    const auto status = stream->Finish();
+    EXPECT_TRUE(
+        status.ok() || status.error_code() == grpc::StatusCode::CANCELLED);
+    EXPECT_TRUE(globalMediaActivityCoordinator().waitForStopped(
+        media_ticket, 1s));
+    const int stopped_stream_reads = hand_->sensor_read_calls.load();
+    std::this_thread::sleep_for(50ms);
+    EXPECT_EQ(hand_->sensor_read_calls.load(), stopped_stream_reads);
+    EXPECT_TRUE(globalMediaActivityCoordinator().finishStopAll(
+        media_ticket, true));
+    EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(
+        admission_ticket, true));
+
+    grpc::ClientContext resumed_context;
+    resumed_context.set_deadline(std::chrono::system_clock::now() + 3s);
+    auto resumed = stub_->GetSensorDataStream(&resumed_context);
+    ASSERT_TRUE(resumed->Write(sensorStreamRequest()));
+    ASSERT_TRUE(resumed->Read(&feedback));
+    EXPECT_TRUE(feedback.header().success());
+    EXPECT_GT(hand_->sensor_read_calls.load(), stopped_stream_reads);
+    EXPECT_GE(hand_->resume_operational_calls.load(), 2);
+    resumed_context.TryCancel();
+    resumed->WritesDone();
+    while (resumed->Read(&feedback)) {
+    }
+    (void)resumed->Finish();
+}
+
+} // namespace
+} // namespace cmvr::service

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

@@ -0,0 +1,864 @@
+#include "service/grpc/include/grpc_error_logging_interceptor.h"
+
+#include <atomic>
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <condition_variable>
+#include <cstdio>
+#include <filesystem>
+#include <fstream>
+#include <iterator>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <utility>
+#include <vector>
+
+#include <grpcpp/grpcpp.h>
+#include <grpcpp/generic/generic_stub.h>
+#include <grpcpp/impl/client_unary_call.h>
+#include <grpcpp/impl/rpc_method.h>
+#include <gtest/gtest.h>
+#include <unistd.h>
+
+#include "cmvr/api/agv_service.grpc.pb.h"
+#include "cmvr/api/camera_service.grpc.pb.h"
+#include "cmvr/api/test_service.grpc.pb.h"
+#include "common/base/logging/logger.h"
+
+namespace cmvr::service {
+namespace {
+
+std::atomic<std::uint64_t> g_socket_sequence{0};
+
+void setRpcDeadline(grpc::ClientContext& context)
+{
+    context.set_deadline(
+        std::chrono::system_clock::now() + std::chrono::seconds(3));
+}
+
+grpc::ByteBuffer byteBuffer(const std::string& payload)
+{
+    const grpc::Slice slice(payload);
+    return grpc::ByteBuffer(&slice, 1);
+}
+
+std::string byteBufferString(const grpc::ByteBuffer& buffer)
+{
+    std::vector<grpc::Slice> slices;
+    EXPECT_TRUE(buffer.Dump(&slices).ok());
+    std::string payload;
+    for (const auto& slice : slices) {
+        payload.append(
+            reinterpret_cast<const char*>(slice.begin()), slice.size());
+    }
+    return payload;
+}
+
+grpc::Status rawUnaryCall(const std::shared_ptr<grpc::Channel>& channel,
+                          const std::string& method,
+                          const std::string& request_payload,
+                          grpc::ByteBuffer& response)
+{
+    grpc::ClientContext context;
+    setRpcDeadline(context);
+    const auto request = byteBuffer(request_payload);
+    const grpc::internal::RpcMethod rpc_method(
+        method.c_str(), grpc::internal::RpcMethod::NORMAL_RPC);
+    return grpc::internal::BlockingUnaryCall(
+        channel.get(), rpc_method, &context, request, &response);
+}
+
+class ErrorLoggingTestService final : public api::TestService::Service {
+public:
+    grpc::Status Call(grpc::ServerContext* context,
+                      const api::TestReqeust* request,
+                      api::TestResponse* response) override
+    {
+        if (request->data() == "grpc-error") {
+            return grpc::Status(
+                grpc::StatusCode::INTERNAL, "transport sentinel");
+        }
+        if (request->data() == "cancelled") {
+            return grpc::Status(
+                grpc::StatusCode::CANCELLED, "client closed stream");
+        }
+        if (request->data() == "server-cancel") {
+            context->TryCancel();
+            return grpc::Status::OK;
+        }
+        if (request->data() == "client-cancel") {
+            {
+                std::lock_guard lock(cancel_mutex_);
+                cancel_started_ = true;
+            }
+            cancel_started_condition_.notify_all();
+            const auto timeout =
+                std::chrono::steady_clock::now() + std::chrono::seconds(3);
+            while (!context->IsCancelled() &&
+                   std::chrono::steady_clock::now() < timeout) {
+                std::this_thread::sleep_for(std::chrono::milliseconds(1));
+            }
+            return grpc::Status::OK;
+        }
+        response->set_data("ok");
+        return grpc::Status::OK;
+    }
+
+    bool waitForCancelableCall()
+    {
+        std::unique_lock lock(cancel_mutex_);
+        return cancel_started_condition_.wait_for(
+            lock, std::chrono::seconds(3),
+            [this] { return cancel_started_; });
+    }
+
+private:
+    std::mutex cancel_mutex_;
+    std::condition_variable cancel_started_condition_;
+    bool cancel_started_{false};
+};
+
+class ErrorLoggingAgvService final : public api::AgvService::Service {
+public:
+    grpc::Status emergencyStop(
+        grpc::ServerContext*,
+        const api::CommandHeader_Request* request,
+        api::CommandHeader_Feedback* response) override
+    {
+        if (request->device_id() == "missing") {
+            response->set_success(false);
+            response->set_error_message("missing sentinel");
+            return grpc::Status(
+                grpc::StatusCode::NOT_FOUND, "missing sentinel");
+        }
+        response->set_success(false);
+        response->set_error_message("direct sentinel");
+        return grpc::Status::OK;
+    }
+
+    grpc::Status streamMap(
+        grpc::ServerContext*,
+        const api::AgvMapStreamCommand_Request* request,
+        grpc::ServerWriter<api::AgvMapStreamCommand_Feedback>* writer) override
+    {
+        const auto write_failure = [writer](const std::string& detail) {
+            api::AgvMapStreamCommand_Feedback response;
+            response.mutable_header()->set_success(false);
+            response.mutable_header()->set_error_message(detail);
+            return writer->Write(response);
+        };
+
+        if (request->resume_token() == "multiple") {
+            write_failure("first stream sentinel");
+            write_failure("second stream sentinel");
+            return grpc::Status::OK;
+        }
+        if (request->resume_token() == "hold") {
+            write_failure("immediate stream sentinel");
+            std::unique_lock lock(stream_mutex_);
+            stream_released_.wait_for(
+                lock, std::chrono::seconds(3),
+                [this] { return release_stream_; });
+            return grpc::Status::OK;
+        }
+        if (request->resume_token() == "different") {
+            write_failure("application stream sentinel");
+            return grpc::Status(
+                grpc::StatusCode::INTERNAL, "status stream sentinel");
+        }
+        if (request->resume_token() == "long-different") {
+            const std::string common_prefix(1100, 'x');
+            write_failure(common_prefix + " application");
+            return grpc::Status(
+                grpc::StatusCode::INTERNAL,
+                common_prefix + " status");
+        }
+
+        write_failure("stream sentinel");
+        return grpc::Status(
+            grpc::StatusCode::INTERNAL, "stream sentinel");
+    }
+
+    void releaseHeldStream()
+    {
+        {
+            std::lock_guard lock(stream_mutex_);
+            release_stream_ = true;
+        }
+        stream_released_.notify_all();
+    }
+
+private:
+    std::mutex stream_mutex_;
+    std::condition_variable stream_released_;
+    bool release_stream_{false};
+};
+
+class ErrorLoggingCameraService final : public api::CameraService::Service {
+public:
+    grpc::Status StartCamera(
+        grpc::ServerContext*,
+        const api::StartCameraCommand_Request*,
+        api::StartCameraCommand_Feedback* response) override
+    {
+        response->mutable_header()->set_success(false);
+        response->mutable_header()->set_error_message("nested sentinel");
+        return grpc::Status::OK;
+    }
+
+    grpc::Status GetRGBImage(
+        grpc::ServerContext*,
+        const api::GetRGBImageCommand_Request*,
+        api::GetRGBImageCommand_Feedback* response) override
+    {
+        response->mutable_header()->set_success(true);
+        response->mutable_color_frame()->set_data(
+            std::string(2 * 1024 * 1024, 'x'));
+        return grpc::Status::OK;
+    }
+};
+
+class RawCameraService final
+    : public api::CameraService::WithRawCallbackMethod_StartCamera<
+          api::CameraService::Service> {
+public:
+    explicit RawCameraService(std::string response)
+        : response_(std::move(response))
+    {
+    }
+
+    grpc::ServerUnaryReactor* StartCamera(
+        grpc::CallbackServerContext* context,
+        const grpc::ByteBuffer*,
+        grpc::ByteBuffer* response) override
+    {
+        *response = byteBuffer(response_);
+        auto* reactor = context->DefaultReactor();
+        reactor->Finish(grpc::Status::OK);
+        return reactor;
+    }
+
+private:
+    std::string response_;
+};
+
+struct RawCallResult {
+    grpc::Status status;
+    std::vector<GrpcFailureRecord> records;
+    std::string response;
+};
+
+class ScopedServer final {
+public:
+    ScopedServer(std::unique_ptr<grpc::Server> server,
+                 std::string socket_path = {})
+        : server_(std::move(server)), socket_path_(std::move(socket_path))
+    {
+    }
+
+    ~ScopedServer()
+    {
+        if (server_) {
+            server_->Shutdown();
+            server_->Wait();
+        }
+        if (!socket_path_.empty()) {
+            std::remove(socket_path_.c_str());
+        }
+    }
+
+    grpc::Server* get() const { return server_.get(); }
+
+private:
+    std::unique_ptr<grpc::Server> server_;
+    std::string socket_path_;
+};
+
+class ScopedSocket final {
+public:
+    explicit ScopedSocket(std::string path) : path_(std::move(path))
+    {
+        std::remove(path_.c_str());
+    }
+
+    ~ScopedSocket() { std::remove(path_.c_str()); }
+
+private:
+    std::string path_;
+};
+
+RawCallResult callRawStartCamera(const std::string& response_payload)
+{
+    std::mutex records_mutex;
+    std::vector<GrpcFailureRecord> records;
+    RawCameraService service(response_payload);
+    grpc::ServerBuilder builder;
+    const std::string address =
+        "unix:/tmp/cmvr_grpc_error_logging_raw_" +
+        std::to_string(static_cast<long long>(::getpid())) + "_" +
+        std::to_string(g_socket_sequence.fetch_add(1U)) + ".sock";
+    const std::string socket_path = address.substr(5);
+    ScopedSocket socket(socket_path);
+    builder.AddListeningPort(address, grpc::InsecureServerCredentials());
+    builder.RegisterService(&service);
+    std::vector<std::unique_ptr<
+        grpc::experimental::ServerInterceptorFactoryInterface>> factories;
+    factories.emplace_back(makeGrpcErrorLoggingInterceptorFactory(
+        [&records, &records_mutex](const GrpcFailureRecord& record) {
+            std::lock_guard lock(records_mutex);
+            records.push_back(record);
+        }));
+    builder.experimental().SetInterceptorCreators(std::move(factories));
+    ScopedServer server(builder.BuildAndStart());
+    EXPECT_NE(server.get(), nullptr);
+    if (server.get() == nullptr) {
+        return {};
+    }
+
+    const auto channel = grpc::CreateChannel(
+        address, grpc::InsecureChannelCredentials());
+    api::StartCameraCommand_Request request;
+    grpc::ByteBuffer response;
+    const auto status = rawUnaryCall(
+        channel, "/cmvr.api.CameraService/StartCamera",
+        request.SerializeAsString(), response);
+    std::lock_guard lock(records_mutex);
+    return {status, records, byteBufferString(response)};
+}
+
+class GrpcErrorLoggingInterceptorTest : public testing::Test {
+protected:
+    void SetUp() override
+    {
+        grpc::ServerBuilder builder;
+        socket_path_ =
+            "/tmp/cmvr_grpc_error_logging_interceptor_test_" +
+            std::to_string(static_cast<long long>(::getpid())) + "_" +
+            std::to_string(g_socket_sequence.fetch_add(1U)) + ".sock";
+        std::remove(socket_path_.c_str());
+        const std::string address = "unix:" + socket_path_;
+        builder.AddListeningPort(
+            address, grpc::InsecureServerCredentials());
+        builder.RegisterService(&test_service_);
+        builder.RegisterService(&agv_service_);
+        builder.RegisterService(&camera_service_);
+
+        std::vector<std::unique_ptr<
+            grpc::experimental::ServerInterceptorFactoryInterface>> factories;
+        factories.emplace_back(makeGrpcErrorLoggingInterceptorFactory(
+            [this](const GrpcFailureRecord& record) {
+                {
+                    std::lock_guard lock(records_mutex_);
+                    records_.push_back(record);
+                }
+                records_changed_.notify_all();
+            }));
+        builder.experimental().SetInterceptorCreators(std::move(factories));
+        server_ = builder.BuildAndStart();
+        ASSERT_NE(server_, nullptr);
+
+        const auto channel = grpc::CreateChannel(
+            address,
+            grpc::InsecureChannelCredentials());
+        test_stub_ = api::TestService::NewStub(channel);
+        agv_stub_ = api::AgvService::NewStub(channel);
+        camera_stub_ = api::CameraService::NewStub(channel);
+    }
+
+    void TearDown() override
+    {
+        if (server_) {
+            server_->Shutdown();
+            server_->Wait();
+        }
+        test_stub_.reset();
+        agv_stub_.reset();
+        camera_stub_.reset();
+        if (!socket_path_.empty()) {
+            std::remove(socket_path_.c_str());
+        }
+    }
+
+    std::vector<GrpcFailureRecord> records()
+    {
+        std::lock_guard lock(records_mutex_);
+        return records_;
+    }
+
+    std::vector<GrpcFailureRecord> waitForRecords(
+        const std::size_t count,
+        const std::chrono::milliseconds timeout = std::chrono::seconds(3))
+    {
+        std::unique_lock lock(records_mutex_);
+        records_changed_.wait_for(
+            lock, timeout, [this, count] { return records_.size() >= count; });
+        return records_;
+    }
+
+    ErrorLoggingTestService test_service_;
+    ErrorLoggingAgvService agv_service_;
+    ErrorLoggingCameraService camera_service_;
+    std::unique_ptr<grpc::Server> server_;
+    std::unique_ptr<api::TestService::Stub> test_stub_;
+    std::unique_ptr<api::AgvService::Stub> agv_stub_;
+    std::unique_ptr<api::CameraService::Stub> camera_stub_;
+    std::string socket_path_;
+    std::mutex records_mutex_;
+    std::condition_variable records_changed_;
+    std::vector<GrpcFailureRecord> records_;
+};
+
+TEST_F(GrpcErrorLoggingInterceptorTest, RecordsNonOkGrpcStatus)
+{
+    grpc::ClientContext context;
+    setRpcDeadline(context);
+    api::TestReqeust request;
+    api::TestResponse response;
+    request.set_data("grpc-error");
+
+    const grpc::Status status = test_stub_->Call(&context, request, &response);
+    ASSERT_EQ(status.error_code(), grpc::StatusCode::INTERNAL);
+
+    const auto captured = waitForRecords(1U);
+    ASSERT_EQ(captured.size(), 1U);
+    EXPECT_EQ(captured.front().kind, GrpcFailureKind::GRPC_STATUS);
+    EXPECT_EQ(captured.front().method, "/cmvr.api.TestService/Call");
+    EXPECT_EQ(captured.front().status_code, grpc::StatusCode::INTERNAL);
+    EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::ERROR);
+    EXPECT_EQ(captured.front().detail, "transport sentinel");
+}
+
+TEST_F(GrpcErrorLoggingInterceptorTest, RecordsDirectFeedbackFailure)
+{
+    grpc::ClientContext context;
+    setRpcDeadline(context);
+    api::CommandHeader_Request request;
+    api::CommandHeader_Feedback response;
+
+    ASSERT_TRUE(agv_stub_->emergencyStop(&context, request, &response).ok());
+    ASSERT_FALSE(response.success());
+
+    const auto captured = waitForRecords(1U);
+    ASSERT_EQ(captured.size(), 1U);
+    EXPECT_EQ(captured.front().kind, GrpcFailureKind::APPLICATION);
+    EXPECT_EQ(captured.front().method, "/cmvr.api.AgvService/emergencyStop");
+    EXPECT_EQ(captured.front().status_code, grpc::StatusCode::OK);
+    EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::ERROR);
+    EXPECT_EQ(captured.front().detail, "direct sentinel");
+}
+
+TEST_F(GrpcErrorLoggingInterceptorTest, RecordsCancellationAsWarning)
+{
+    grpc::ClientContext context;
+    setRpcDeadline(context);
+    api::TestReqeust request;
+    api::TestResponse response;
+    request.set_data("cancelled");
+
+    const grpc::Status status = test_stub_->Call(&context, request, &response);
+    ASSERT_EQ(status.error_code(), grpc::StatusCode::CANCELLED);
+
+    const auto captured = waitForRecords(1U);
+    ASSERT_EQ(captured.size(), 1U);
+    EXPECT_EQ(captured.front().kind, GrpcFailureKind::GRPC_STATUS);
+    EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::WARNING);
+    EXPECT_EQ(captured.front().detail, "client closed stream");
+}
+
+TEST(GrpcErrorLoggingInterceptorFileTest, WarningStatusIsFlushedToFile)
+{
+    std::array<char, 80> directory_pattern{};
+    const std::string pattern = "/tmp/cmvr-grpc-log-test-XXXXXX";
+    std::copy(pattern.begin(), pattern.end(), directory_pattern.begin());
+    const char* created = ::mkdtemp(directory_pattern.data());
+    ASSERT_NE(created, nullptr);
+    const std::filesystem::path directory(created);
+
+    struct Cleanup {
+        ~Cleanup()
+        {
+            logging::shutdownLogging();
+            if (!socket_path.empty()) {
+                std::remove(socket_path.c_str());
+            }
+            std::error_code error;
+            std::filesystem::remove_all(directory, error);
+        }
+
+        std::filesystem::path directory;
+        std::string socket_path;
+    } cleanup{directory, {}};
+
+    config::LoggerConfig config;
+    config.set_minimum_level(config::LOG_LEVEL_WARNING);
+    config.set_directory(directory.string());
+    config.set_flush_interval_seconds(3600);
+    auto* warning_route = config.add_routes();
+    warning_route->set_level(config::LOG_LEVEL_WARNING);
+    warning_route->set_file(true);
+    ASSERT_TRUE(logging::initLogging(config, "grpc_log_test", directory));
+
+    ErrorLoggingTestService service;
+    grpc::ServerBuilder builder;
+    const std::string socket_path =
+        "/tmp/cmvr_grpc_error_logging_file_test_" +
+        std::to_string(static_cast<long long>(::getpid())) + "_" +
+        std::to_string(g_socket_sequence.fetch_add(1U)) + ".sock";
+    cleanup.socket_path = socket_path;
+    ScopedSocket socket(socket_path);
+    const std::string address = "unix:" + socket_path;
+    builder.AddListeningPort(address, grpc::InsecureServerCredentials());
+    builder.RegisterService(&service);
+    std::vector<std::unique_ptr<
+        grpc::experimental::ServerInterceptorFactoryInterface>> factories;
+    factories.emplace_back(makeGrpcErrorLoggingInterceptorFactory());
+    builder.experimental().SetInterceptorCreators(std::move(factories));
+    ScopedServer server(builder.BuildAndStart());
+    ASSERT_NE(server.get(), nullptr);
+
+    const auto channel = grpc::CreateChannel(
+        address, grpc::InsecureChannelCredentials());
+    auto stub = api::TestService::NewStub(channel);
+    grpc::ClientContext context;
+    setRpcDeadline(context);
+    api::TestReqeust request;
+    api::TestResponse response;
+    request.set_data("cancelled");
+    const grpc::Status status = stub->Call(&context, request, &response);
+    ASSERT_EQ(status.error_code(), grpc::StatusCode::CANCELLED);
+
+    std::ifstream input(directory / "grpc_log_test.log");
+    const std::string contents{
+        std::istreambuf_iterator<char>(input),
+        std::istreambuf_iterator<char>()};
+    EXPECT_NE(contents.find("/cmvr.api.TestService/Call"), std::string::npos);
+    EXPECT_NE(contents.find("code=1(CANCELLED)"), std::string::npos);
+    EXPECT_NE(contents.find("client closed stream"), std::string::npos);
+
+}
+
+TEST_F(GrpcErrorLoggingInterceptorTest, RecordsServerCancellationReturnedAsOk)
+{
+    grpc::ClientContext context;
+    setRpcDeadline(context);
+    api::TestReqeust request;
+    api::TestResponse response;
+    request.set_data("server-cancel");
+
+    const grpc::Status status = test_stub_->Call(&context, request, &response);
+    ASSERT_EQ(status.error_code(), grpc::StatusCode::CANCELLED);
+
+    // TryCancel is best effort: the final status may be intercepted before
+    // the cancellation hook. Either way, the interceptor must not recurse or
+    // emit duplicate records.
+    const auto captured = waitForRecords(1U, std::chrono::milliseconds(50));
+    ASSERT_LE(captured.size(), 1U);
+    if (!captured.empty()) {
+        EXPECT_EQ(captured.front().kind, GrpcFailureKind::GRPC_STATUS);
+        EXPECT_EQ(captured.front().status_code, grpc::StatusCode::CANCELLED);
+        EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::WARNING);
+        EXPECT_EQ(
+            captured.front().detail,
+            "RPC cancellation was requested by the edge server before completion");
+    }
+}
+
+TEST_F(GrpcErrorLoggingInterceptorTest, RecordsClientCancellation)
+{
+    grpc::ClientContext context;
+    setRpcDeadline(context);
+    api::TestReqeust request;
+    api::TestResponse response;
+    request.set_data("client-cancel");
+
+    grpc::Status status;
+    std::thread call([&] {
+        status = test_stub_->Call(&context, request, &response);
+    });
+    const bool call_started = test_service_.waitForCancelableCall();
+    context.TryCancel();
+    call.join();
+
+    ASSERT_TRUE(call_started);
+    ASSERT_EQ(status.error_code(), grpc::StatusCode::CANCELLED);
+    const auto captured = waitForRecords(1U);
+    ASSERT_EQ(captured.size(), 1U);
+    EXPECT_EQ(captured.front().kind, GrpcFailureKind::GRPC_STATUS);
+    EXPECT_EQ(captured.front().status_code, grpc::StatusCode::CANCELLED);
+    EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::WARNING);
+    EXPECT_EQ(
+        captured.front().detail,
+        "RPC was cancelled by the client or transport before completion");
+}
+
+TEST_F(GrpcErrorLoggingInterceptorTest,
+       RecordsStatusWhenNonOkSuppressesUnaryResponse)
+{
+    grpc::ClientContext context;
+    setRpcDeadline(context);
+    api::CommandHeader_Request request;
+    api::CommandHeader_Feedback response;
+    request.set_device_id("missing");
+
+    const grpc::Status status =
+        agv_stub_->emergencyStop(&context, request, &response);
+    ASSERT_EQ(status.error_code(), grpc::StatusCode::NOT_FOUND);
+
+    const auto captured = waitForRecords(1U);
+    ASSERT_EQ(captured.size(), 1U);
+    EXPECT_EQ(captured.front().kind, GrpcFailureKind::GRPC_STATUS);
+    EXPECT_EQ(captured.front().status_code, grpc::StatusCode::NOT_FOUND);
+    EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::WARNING);
+    EXPECT_EQ(captured.front().detail, "missing sentinel");
+}
+
+TEST_F(GrpcErrorLoggingInterceptorTest, RecordsNestedFeedbackFailure)
+{
+    grpc::ClientContext context;
+    setRpcDeadline(context);
+    api::StartCameraCommand_Request request;
+    api::StartCameraCommand_Feedback response;
+
+    ASSERT_TRUE(camera_stub_->StartCamera(&context, request, &response).ok());
+    ASSERT_FALSE(response.header().success());
+
+    const auto captured = waitForRecords(1U);
+    ASSERT_EQ(captured.size(), 1U);
+    EXPECT_EQ(captured.front().kind, GrpcFailureKind::APPLICATION);
+    EXPECT_EQ(captured.front().method, "/cmvr.api.CameraService/StartCamera");
+    EXPECT_EQ(captured.front().detail, "nested sentinel");
+}
+
+TEST_F(GrpcErrorLoggingInterceptorTest,
+       RecordsStreamingResponseAndFinalNonOkStatus)
+{
+    grpc::ClientContext context;
+    setRpcDeadline(context);
+    api::AgvMapStreamCommand_Request request;
+    auto reader = agv_stub_->streamMap(&context, request);
+    api::AgvMapStreamCommand_Feedback response;
+
+    ASSERT_TRUE(reader->Read(&response));
+    ASSERT_FALSE(response.header().success());
+    const grpc::Status status = reader->Finish();
+    ASSERT_EQ(status.error_code(), grpc::StatusCode::INTERNAL);
+
+    const auto captured = waitForRecords(2U);
+    ASSERT_EQ(captured.size(), 2U);
+    EXPECT_EQ(captured[0].kind, GrpcFailureKind::APPLICATION);
+    EXPECT_EQ(captured[0].method, "/cmvr.api.AgvService/streamMap");
+    EXPECT_EQ(captured[0].status_code, grpc::StatusCode::OK);
+    EXPECT_EQ(captured[0].severity, GrpcFailureSeverity::ERROR);
+    EXPECT_EQ(captured[0].detail, "stream sentinel");
+    EXPECT_EQ(captured[1].kind, GrpcFailureKind::GRPC_STATUS);
+    EXPECT_EQ(captured[1].status_code, grpc::StatusCode::INTERNAL);
+    EXPECT_EQ(captured[1].severity, GrpcFailureSeverity::ERROR);
+    EXPECT_EQ(captured[1].detail, "stream sentinel");
+}
+
+TEST_F(GrpcErrorLoggingInterceptorTest,
+       RecordsStreamingFailureBeforeTheStreamFinishes)
+{
+    grpc::ClientContext context;
+    setRpcDeadline(context);
+    api::AgvMapStreamCommand_Request request;
+    request.set_resume_token("hold");
+    auto reader = agv_stub_->streamMap(&context, request);
+    api::AgvMapStreamCommand_Feedback response;
+
+    const bool read = reader->Read(&response);
+    const auto captured_before_finish = waitForRecords(1U);
+    agv_service_.releaseHeldStream();
+    const grpc::Status status = reader->Finish();
+
+    ASSERT_TRUE(read);
+    ASSERT_TRUE(status.ok());
+    ASSERT_EQ(captured_before_finish.size(), 1U);
+    EXPECT_EQ(
+        captured_before_finish.front().detail,
+        "immediate stream sentinel");
+}
+
+TEST_F(GrpcErrorLoggingInterceptorTest,
+       RecordsEveryStreamingFailureResponse)
+{
+    grpc::ClientContext context;
+    setRpcDeadline(context);
+    api::AgvMapStreamCommand_Request request;
+    request.set_resume_token("multiple");
+    auto reader = agv_stub_->streamMap(&context, request);
+    api::AgvMapStreamCommand_Feedback response;
+
+    ASSERT_TRUE(reader->Read(&response));
+    ASSERT_TRUE(reader->Read(&response));
+    ASSERT_TRUE(reader->Finish().ok());
+
+    const auto captured = waitForRecords(2U);
+    ASSERT_EQ(captured.size(), 2U);
+    EXPECT_EQ(captured[0].detail, "first stream sentinel");
+    EXPECT_EQ(captured[1].detail, "second stream sentinel");
+}
+
+TEST_F(GrpcErrorLoggingInterceptorTest,
+       PreservesDifferentResponseAndStatusErrors)
+{
+    grpc::ClientContext context;
+    setRpcDeadline(context);
+    api::AgvMapStreamCommand_Request request;
+    request.set_resume_token("different");
+    auto reader = agv_stub_->streamMap(&context, request);
+    api::AgvMapStreamCommand_Feedback response;
+
+    ASSERT_TRUE(reader->Read(&response));
+    const grpc::Status status = reader->Finish();
+    ASSERT_EQ(status.error_code(), grpc::StatusCode::INTERNAL);
+
+    const auto captured = waitForRecords(2U);
+    ASSERT_EQ(captured.size(), 2U);
+    EXPECT_EQ(captured[0].kind, GrpcFailureKind::APPLICATION);
+    EXPECT_EQ(captured[0].detail, "application stream sentinel");
+    EXPECT_EQ(captured[1].kind, GrpcFailureKind::GRPC_STATUS);
+    EXPECT_EQ(captured[1].status_code, grpc::StatusCode::INTERNAL);
+    EXPECT_EQ(captured[1].detail, "status stream sentinel");
+}
+
+TEST_F(GrpcErrorLoggingInterceptorTest,
+       DoesNotDeduplicateLongErrorsByTruncatedPrefix)
+{
+    grpc::ClientContext context;
+    setRpcDeadline(context);
+    api::AgvMapStreamCommand_Request request;
+    request.set_resume_token("long-different");
+    auto reader = agv_stub_->streamMap(&context, request);
+    api::AgvMapStreamCommand_Feedback response;
+
+    ASSERT_TRUE(reader->Read(&response));
+    const grpc::Status status = reader->Finish();
+    ASSERT_EQ(status.error_code(), grpc::StatusCode::INTERNAL);
+
+    const auto captured = waitForRecords(2U);
+    ASSERT_EQ(captured.size(), 2U);
+    EXPECT_EQ(captured[0].kind, GrpcFailureKind::APPLICATION);
+    EXPECT_EQ(captured[1].kind, GrpcFailureKind::GRPC_STATUS);
+}
+
+TEST(GrpcErrorLoggingInterceptorWireTest, RequiresHeaderAsFirstField)
+{
+    api::CommandHeader_Feedback header;
+    header.set_success(false);
+    header.set_error_message("out-of-order sentinel");
+    std::string payload;
+    payload.push_back(static_cast<char>(0x12));
+    payload.push_back(static_cast<char>(0x01));
+    payload.push_back('x');
+    payload.push_back(static_cast<char>(0x0a));
+    payload.push_back(static_cast<char>(header.ByteSizeLong()));
+    payload.append(header.SerializeAsString());
+
+    const auto result = callRawStartCamera(payload);
+
+    ASSERT_TRUE(result.status.ok());
+    ASSERT_EQ(result.response, payload);
+    ASSERT_EQ(result.records.size(), 1U);
+    EXPECT_EQ(
+        result.records.front().kind,
+        GrpcFailureKind::MALFORMED_RESPONSE);
+}
+
+TEST(GrpcErrorLoggingInterceptorWireTest, DoesNotScanPayloadAfterHeader)
+{
+    api::CommandHeader_Feedback header;
+    header.set_success(false);
+    header.set_error_message("leading header sentinel");
+    std::string payload;
+    payload.push_back(static_cast<char>(0x0a));
+    payload.push_back(static_cast<char>(header.ByteSizeLong()));
+    payload.append(header.SerializeAsString());
+    payload.push_back(static_cast<char>(0x12));
+    payload.push_back(static_cast<char>(0xff));
+    payload.append(2U * 1024U * 1024U, 'x');
+
+    const auto result = callRawStartCamera(payload);
+
+    ASSERT_TRUE(result.status.ok());
+    ASSERT_EQ(result.records.size(), 1U);
+    EXPECT_EQ(result.records.front().kind, GrpcFailureKind::APPLICATION);
+    EXPECT_EQ(result.records.front().detail, "leading header sentinel");
+}
+
+TEST(GrpcErrorLoggingInterceptorWireTest, TreatsExplicitEmptyHeaderAsFailure)
+{
+    const std::string payload{
+        static_cast<char>(0x0a), static_cast<char>(0x00)};
+
+    const auto result = callRawStartCamera(payload);
+
+    ASSERT_TRUE(result.status.ok());
+    ASSERT_EQ(result.records.size(), 1U);
+    EXPECT_EQ(result.records.front().kind, GrpcFailureKind::APPLICATION);
+    EXPECT_EQ(
+        result.records.front().detail,
+        "operation failed without an error message");
+}
+
+TEST(GrpcErrorLoggingInterceptorWireTest, RejectsInvalidHeaderWireType)
+{
+    const std::string payload{
+        static_cast<char>(0x08), static_cast<char>(0x01)};
+
+    const auto result = callRawStartCamera(payload);
+
+    ASSERT_TRUE(result.status.ok());
+    ASSERT_EQ(result.records.size(), 1U);
+    EXPECT_EQ(
+        result.records.front().kind,
+        GrpcFailureKind::MALFORMED_RESPONSE);
+}
+
+TEST(GrpcErrorLoggingInterceptorWireTest, RejectsInvalidUtf8ErrorMessage)
+{
+    const std::string payload{
+        static_cast<char>(0x0a), static_cast<char>(0x04),
+        static_cast<char>(0x12), static_cast<char>(0x02),
+        static_cast<char>(0xc3), static_cast<char>(0x28)};
+
+    const auto result = callRawStartCamera(payload);
+
+    ASSERT_TRUE(result.status.ok());
+    ASSERT_EQ(result.records.size(), 1U);
+    EXPECT_EQ(
+        result.records.front().kind,
+        GrpcFailureKind::MALFORMED_RESPONSE);
+}
+
+TEST_F(GrpcErrorLoggingInterceptorTest, DoesNotRecordSuccessfulLargeResponse)
+{
+    grpc::ClientContext context;
+    setRpcDeadline(context);
+    api::GetRGBImageCommand_Request request;
+    api::GetRGBImageCommand_Feedback response;
+
+    ASSERT_TRUE(camera_stub_->GetRGBImage(&context, request, &response).ok());
+    ASSERT_TRUE(response.header().success());
+    ASSERT_EQ(response.color_frame().data().size(), 2U * 1024U * 1024U);
+    EXPECT_TRUE(records().empty());
+}
+
+TEST_F(GrpcErrorLoggingInterceptorTest, IgnoresSuccessfulResponseWithoutHeader)
+{
+    grpc::ClientContext context;
+    setRpcDeadline(context);
+    api::TestReqeust request;
+    api::TestResponse response;
+    request.set_data("ok");
+
+    ASSERT_TRUE(test_stub_->Call(&context, request, &response).ok());
+    EXPECT_EQ(response.data(), "ok");
+    EXPECT_TRUE(records().empty());
+}
+
+} // namespace
+} // namespace cmvr::service

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

@@ -0,0 +1,391 @@
+#include "service/grpc/include/grpc_head_service.h"
+
+#include <atomic>
+#include <chrono>
+#include <memory>
+#include <string>
+#include <thread>
+
+#include <grpcpp/grpcpp.h>
+#include <gtest/gtest.h>
+
+#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
+#include "manager/device_manager/include/device_manager.h"
+#include "service/grpc/include/media_activity_coordinator.h"
+#include "service/stop_all/include/stop_all_admission_gate.h"
+
+namespace cmvr::service {
+namespace {
+
+using namespace std::chrono_literals;
+
+class FakeBiohead final : public device::AbstractBiohead {
+public:
+    FakeBiohead() { id_ = "test-head"; }
+
+    std::string typeName() const override { return "FakeBiohead"; }
+    bool stop() override
+    {
+        ++lifecycle_stop_calls;
+        return true;
+    }
+
+    bool setExpressionPoseIfCurrent(
+        const OperationalToken token,
+        device::FacialExpressionState&,
+        double,
+        double) override
+    {
+        return recordIfCurrent(token, set_expression_calls);
+    }
+
+    bool streamFacialPoseIfCurrent(
+        const OperationalToken token,
+        device::FacialExpressionState&,
+        double,
+        double) override
+    {
+        return recordIfCurrent(token, stream_expression_calls);
+    }
+
+    bool speakStartIfCurrent(const OperationalToken token) override
+    {
+        return recordIfCurrent(token, speak_start_calls);
+    }
+
+    bool expressionHappyIfCurrent(const OperationalToken token) override
+    {
+        return recordIfCurrent(token, happy_calls);
+    }
+
+    bool expressionSurprisedIfCurrent(const OperationalToken token) override
+    {
+        return recordIfCurrent(token, surprise_calls);
+    }
+
+    bool expressionTiredIfCurrent(const OperationalToken token) override
+    {
+        return recordIfCurrent(token, tired_calls);
+    }
+
+    bool expressionAngryIfCurrent(const OperationalToken token) override
+    {
+        return recordIfCurrent(token, angry_calls);
+    }
+
+    bool expressionSadnessIfCurrent(const OperationalToken token) override
+    {
+        return recordIfCurrent(token, sadness_calls);
+    }
+
+    bool expressionYawnIfCurrent(const OperationalToken token) override
+    {
+        return recordIfCurrent(token, yawn_calls);
+    }
+
+    void speakstop() override { ++speak_stop_calls; }
+
+    bool stopOperationalActivity() override
+    {
+        invalidateOperationalActivities_();
+        ++operational_stop_calls;
+        ++speak_stop_calls;
+        return stop_confirmed.load(std::memory_order_acquire);
+    }
+
+    bool recordIfCurrent(
+        const OperationalToken token,
+        std::atomic<int>& calls)
+    {
+        return runIfOperationalActivityCurrent_(token, [&] { ++calls; });
+    }
+
+    std::atomic<int> set_expression_calls{0};
+    std::atomic<int> stream_expression_calls{0};
+    std::atomic<int> speak_start_calls{0};
+    std::atomic<int> speak_stop_calls{0};
+    std::atomic<int> happy_calls{0};
+    std::atomic<int> surprise_calls{0};
+    std::atomic<int> tired_calls{0};
+    std::atomic<int> angry_calls{0};
+    std::atomic<int> sadness_calls{0};
+    std::atomic<int> yawn_calls{0};
+    std::atomic<int> operational_stop_calls{0};
+    std::atomic<int> lifecycle_stop_calls{0};
+    std::atomic<bool> stop_confirmed{true};
+};
+
+class GrpcHeadServiceTest : public ::testing::Test {
+protected:
+    void SetUp() override
+    {
+        device::DeviceManager::destroyInstance();
+        globalStopAllAdmissionGate().clearForTesting();
+        config::DeviceManagerConfig config;
+        auto& manager = device::DeviceManager::getInstance(config);
+        head_ = std::make_shared<FakeBiohead>();
+        manager.registerDevice(head_);
+        service_ = std::make_unique<gRPCMBioHeadServiceImpl>();
+    }
+
+    void TearDown() override
+    {
+        if (server_) {
+            server_->Shutdown();
+            server_->Wait();
+        }
+        stub_.reset();
+        service_.reset();
+        head_.reset();
+        device::DeviceManager::destroyInstance();
+        globalStopAllAdmissionGate().clearForTesting();
+    }
+
+    bool startGrpcServer()
+    {
+        int selected_port = 0;
+        grpc::ServerBuilder builder;
+        builder.AddListeningPort(
+            "127.0.0.1:0",
+            grpc::InsecureServerCredentials(),
+            &selected_port);
+        builder.RegisterService(service_.get());
+        server_ = builder.BuildAndStart();
+        if (!server_ || selected_port <= 0) {
+            return false;
+        }
+        const std::string address =
+            "127.0.0.1:" + std::to_string(selected_port);
+        stub_ = api::BioHeadService::NewStub(grpc::CreateChannel(
+            address, grpc::InsecureChannelCredentials()));
+        return stub_ != nullptr;
+    }
+
+    static api::StreamFacialExpression_Request streamRequest()
+    {
+        api::StreamFacialExpression_Request request;
+        request.mutable_header()->set_device_id("test-head");
+        request.mutable_expr()->mutable_jaw()->set_x(0.5F);
+        return request;
+    }
+
+    std::shared_ptr<FakeBiohead> head_;
+    std::unique_ptr<gRPCMBioHeadServiceImpl> service_;
+    std::unique_ptr<grpc::Server> server_;
+    std::unique_ptr<api::BioHeadService::Stub> stub_;
+};
+
+TEST_F(GrpcHeadServiceTest,
+       OperationalStopInvalidatesOldTokenWithoutStoppingLifecycle)
+{
+    const auto old_token = head_->beginOperationalActivity();
+    ASSERT_TRUE(head_->recordIfCurrent(old_token, head_->happy_calls));
+
+    EXPECT_TRUE(head_->stopOperationalActivity());
+    EXPECT_FALSE(head_->recordIfCurrent(old_token, head_->happy_calls));
+    EXPECT_EQ(head_->happy_calls.load(), 1);
+    EXPECT_EQ(head_->operational_stop_calls.load(), 1);
+    EXPECT_EQ(head_->lifecycle_stop_calls.load(), 0);
+
+    const auto current_token = head_->beginOperationalActivity();
+    EXPECT_NE(current_token, old_token);
+    EXPECT_TRUE(head_->recordIfCurrent(current_token, head_->happy_calls));
+    EXPECT_EQ(head_->happy_calls.load(), 2);
+}
+
+TEST_F(GrpcHeadServiceTest,
+       StopAllGateRejectsMotionButAllowsOperationalStopRpcs)
+{
+    const auto ticket = globalStopAllAdmissionGate().beginStopAll();
+    ASSERT_TRUE(ticket.valid());
+
+    api::Happy_Request happy_request;
+    happy_request.mutable_header()->set_device_id("test-head");
+    api::Happy_Feedback happy_response;
+    grpc::ServerContext happy_context;
+    EXPECT_TRUE(service_->Happy(
+        &happy_context, &happy_request, &happy_response).ok());
+    EXPECT_FALSE(happy_response.header().success());
+    EXPECT_EQ(head_->happy_calls.load(), 0);
+
+    api::Surprise_Request surprise_request;
+    surprise_request.mutable_header()->set_device_id("test-head");
+    api::Surprise_Feedback surprise_response;
+    grpc::ServerContext surprise_context;
+    EXPECT_TRUE(service_->Surprise(
+        &surprise_context, &surprise_request, &surprise_response).ok());
+    EXPECT_FALSE(surprise_response.header().success());
+    EXPECT_EQ(head_->surprise_calls.load(), 0);
+
+    api::ExpressionTired_Request tired_request;
+    tired_request.mutable_header()->set_device_id("test-head");
+    api::ExpressionTired_Feedback tired_response;
+    grpc::ServerContext tired_context;
+    EXPECT_TRUE(service_->ExpressionTired(
+        &tired_context, &tired_request, &tired_response).ok());
+    EXPECT_FALSE(tired_response.header().success());
+    EXPECT_EQ(head_->tired_calls.load(), 0);
+
+    api::ExpressionAngry_Request angry_request;
+    angry_request.mutable_header()->set_device_id("test-head");
+    api::ExpressionAngry_Feedback angry_response;
+    grpc::ServerContext angry_context;
+    EXPECT_TRUE(service_->ExpressionAngry(
+        &angry_context, &angry_request, &angry_response).ok());
+    EXPECT_FALSE(angry_response.header().success());
+    EXPECT_EQ(head_->angry_calls.load(), 0);
+
+    api::ExpressionSadness_Request sadness_request;
+    sadness_request.mutable_header()->set_device_id("test-head");
+    api::ExpressionSadness_Feedback sadness_response;
+    grpc::ServerContext sadness_context;
+    EXPECT_TRUE(service_->ExpressionSadness(
+        &sadness_context, &sadness_request, &sadness_response).ok());
+    EXPECT_FALSE(sadness_response.header().success());
+    EXPECT_EQ(head_->sadness_calls.load(), 0);
+
+    api::ExpressionYawn_Request yawn_request;
+    yawn_request.mutable_header()->set_device_id("test-head");
+    api::ExpressionYawn_Feedback yawn_response;
+    grpc::ServerContext yawn_context;
+    EXPECT_TRUE(service_->ExpressionYawn(
+        &yawn_context, &yawn_request, &yawn_response).ok());
+    EXPECT_FALSE(yawn_response.header().success());
+    EXPECT_EQ(head_->yawn_calls.load(), 0);
+
+    api::SetFacialExpression_Request expression_request;
+    expression_request.mutable_header()->set_device_id("test-head");
+    api::SetFacialExpression_Feedback expression_response;
+    grpc::ServerContext expression_context;
+    EXPECT_TRUE(service_->SetExpression(
+        &expression_context,
+        &expression_request,
+        &expression_response).ok());
+    EXPECT_FALSE(expression_response.header().success());
+    EXPECT_EQ(head_->set_expression_calls.load(), 0);
+
+    api::SpeakStart_Request speak_request;
+    speak_request.mutable_header()->set_device_id("test-head");
+    api::SpeakStart_Feedback speak_response;
+    grpc::ServerContext speak_context;
+    EXPECT_TRUE(service_->SpeakStart(
+        &speak_context, &speak_request, &speak_response).ok());
+    EXPECT_FALSE(speak_response.header().success());
+    EXPECT_EQ(head_->speak_start_calls.load(), 0);
+
+    api::SpeakStop_Request speak_stop_request;
+    speak_stop_request.mutable_header()->set_device_id("test-head");
+    api::SpeakStop_Feedback speak_stop_response;
+    grpc::ServerContext speak_stop_context;
+    EXPECT_TRUE(service_->SpeakStop(
+        &speak_stop_context,
+        &speak_stop_request,
+        &speak_stop_response).ok());
+    EXPECT_TRUE(speak_stop_response.header().success());
+
+    api::EmergencyStop_Request stop_request;
+    stop_request.mutable_header()->set_device_id("test-head");
+    api::EmergencyStop_Feedback stop_response;
+    grpc::ServerContext stop_context;
+    EXPECT_TRUE(service_->EmergencyStop(
+        &stop_context, &stop_request, &stop_response).ok());
+    EXPECT_TRUE(stop_response.header().success());
+    EXPECT_EQ(head_->operational_stop_calls.load(), 1);
+    EXPECT_EQ(head_->lifecycle_stop_calls.load(), 0);
+
+    EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true));
+}
+
+TEST_F(GrpcHeadServiceTest,
+       StreamIsCancelledByStopAllAndNewStreamWorksAfterRecovery)
+{
+    ASSERT_TRUE(startGrpcServer());
+    grpc::ClientContext context;
+    context.set_deadline(std::chrono::system_clock::now() + 2s);
+    auto stream = stub_->StreamExpression(&context);
+
+    ASSERT_TRUE(stream->Write(streamRequest()));
+    api::StreamFacialExpression_Feedback feedback;
+    ASSERT_TRUE(stream->Read(&feedback));
+    ASSERT_TRUE(feedback.header().success());
+    ASSERT_EQ(head_->stream_expression_calls.load(), 1);
+
+    const auto admission_ticket = globalStopAllAdmissionGate().beginStopAll();
+    const auto media_ticket = globalMediaActivityCoordinator().beginStopAll();
+    ASSERT_TRUE(admission_ticket.valid());
+    ASSERT_TRUE(media_ticket.valid());
+
+    (void)stream->Write(streamRequest());
+    stream->WritesDone();
+    while (stream->Read(&feedback)) {
+    }
+    const auto status = stream->Finish();
+    EXPECT_TRUE(
+        status.ok() || status.error_code() == grpc::StatusCode::CANCELLED);
+    EXPECT_TRUE(globalMediaActivityCoordinator().waitForStopped(
+        media_ticket, 1s));
+    EXPECT_EQ(head_->stream_expression_calls.load(), 1);
+    EXPECT_TRUE(globalMediaActivityCoordinator().finishStopAll(
+        media_ticket, true));
+    EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(
+        admission_ticket, true));
+
+    grpc::ClientContext resumed_context;
+    resumed_context.set_deadline(std::chrono::system_clock::now() + 2s);
+    auto resumed = stub_->StreamExpression(&resumed_context);
+    ASSERT_TRUE(resumed->Write(streamRequest()));
+    ASSERT_TRUE(resumed->Read(&feedback));
+    EXPECT_TRUE(feedback.header().success());
+    EXPECT_EQ(head_->stream_expression_calls.load(), 2);
+    resumed_context.TryCancel();
+    resumed->WritesDone();
+    while (resumed->Read(&feedback)) {
+    }
+    (void)resumed->Finish();
+}
+
+TEST_F(GrpcHeadServiceTest,
+       StopAllCancelsStreamBeforeItsFirstFrameCanClearStopState)
+{
+    ASSERT_TRUE(startGrpcServer());
+    grpc::ClientContext context;
+    context.set_deadline(std::chrono::system_clock::now() + 2s);
+    auto stream = stub_->StreamExpression(&context);
+
+    std::this_thread::sleep_for(20ms);
+    const auto admission_ticket = globalStopAllAdmissionGate().beginStopAll();
+    const auto media_ticket = globalMediaActivityCoordinator().beginStopAll();
+    ASSERT_TRUE(admission_ticket.valid());
+    ASSERT_TRUE(media_ticket.valid());
+
+    (void)stream->Write(streamRequest());
+    stream->WritesDone();
+    api::StreamFacialExpression_Feedback feedback;
+    while (stream->Read(&feedback)) {
+    }
+    (void)stream->Finish();
+    EXPECT_TRUE(globalMediaActivityCoordinator().waitForStopped(
+        media_ticket, 1s));
+    EXPECT_EQ(head_->stream_expression_calls.load(), 0);
+    EXPECT_TRUE(globalMediaActivityCoordinator().finishStopAll(
+        media_ticket, true));
+    EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(
+        admission_ticket, true));
+}
+
+TEST_F(GrpcHeadServiceTest, UnconfirmedOperationalStopIsReported)
+{
+    head_->stop_confirmed = false;
+    api::EmergencyStop_Request request;
+    request.mutable_header()->set_device_id("test-head");
+    api::EmergencyStop_Feedback response;
+    grpc::ServerContext context;
+
+    EXPECT_TRUE(service_->EmergencyStop(&context, &request, &response).ok());
+    EXPECT_FALSE(response.header().success());
+    EXPECT_EQ(head_->operational_stop_calls.load(), 1);
+    EXPECT_EQ(head_->lifecycle_stop_calls.load(), 0);
+}
+
+} // namespace
+} // namespace cmvr::service

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

@@ -18,6 +18,8 @@
 #include "devices/motor/manager/include/motor_manager.h"
 #include "devices/motor/motor_protocol_interface.h"
 #include "manager/device_manager/include/device_manager.h"
+#include "service/grpc/include/motor_activity_coordinator.h"
+#include "service/stop_all/include/stop_all_admission_gate.h"
 
 namespace cmvr::service {
 
@@ -222,11 +224,13 @@ private:
 
 class FakeMotor final : public device::AbstractMotor {
 public:
-    explicit FakeMotor(const std::uint8_t node_id)
+    explicit FakeMotor(
+        const std::uint8_t node_id,
+        std::string joint_name = "test_joint")
         : AbstractMotor(node_id)
     {
         info_.id = node_id;
-        info_.joint_name = "test_joint";
+        info_.joint_name = std::move(joint_name);
     }
 
     std::string typeName() const override { return "FakeMotor"; }
@@ -236,6 +240,8 @@ class MotorServiceTest : public ::testing::Test {
 protected:
     void SetUp() override
     {
+        globalStopAllAdmissionGate().clearForTesting();
+        globalMotorActivityCoordinator().clearForTesting();
         config::DeviceManagerConfig device_config;
         auto& device_manager = device::DeviceManager::getInstance(device_config);
 
@@ -267,6 +273,8 @@ protected:
         motor_.reset();
         protocol_.reset();
         device::DeviceManager::destroyInstance();
+        globalMotorActivityCoordinator().clearForTesting();
+        globalStopAllAdmissionGate().clearForTesting();
     }
 
     static api::MotorTarget makeTarget()
@@ -408,6 +416,135 @@ TEST_F(MotorServiceTest, ProfilePositionReturnsOnlyAfterTargetIsReached)
     EXPECT_EQ(response.status().active_control(), api::MOTOR_CONTROL_NONE);
 }
 
+TEST_F(MotorServiceTest, DirectControlRejectsMotorClaimedByRobotArm)
+{
+    std::uint64_t claim_id = 0;
+    std::string claim_error;
+    ASSERT_TRUE(manager_->claimArmJoints(
+        "test_arm", {"test_joint"}, claim_id, &claim_error))
+        << claim_error;
+
+    api::ProfilePositionRequest request;
+    *request.mutable_target() = makeTarget();
+    request.set_target_position_rad(1.25);
+    request.set_max_velocity_rad_s(1.0);
+    request.set_acceleration_rad_s2(2.0);
+    grpc::ServerContext context;
+    api::MotorCommandResponse response;
+
+    const auto status = service_->profilePosition(
+        &context, &request, &response);
+
+    EXPECT_EQ(status.error_code(), grpc::StatusCode::FAILED_PRECONDITION);
+    EXPECT_FALSE(response.header().success());
+    EXPECT_NE(status.error_message().find("test_arm"), std::string::npos);
+    EXPECT_NE(status.error_message().find("ArmService"), std::string::npos);
+    EXPECT_FALSE(protocol_->command_started_.load());
+    EXPECT_EQ(protocol_->quick_stop_count_.load(), 0);
+}
+
+TEST_F(MotorServiceTest, ClaimedArmMotorRemainsObservableWithoutStopRegistration)
+{
+    std::uint64_t claim_id = 0;
+    ASSERT_TRUE(manager_->claimArmJoints(
+        "test_arm", {"test_joint"}, claim_id));
+
+    api::GetMotorStatusRequest request;
+    *request.mutable_target() = makeTarget();
+    grpc::ServerContext context;
+    api::GetMotorStatusResponse response;
+    const auto status = service_->getStatus(&context, &request, &response);
+
+    ASSERT_TRUE(status.ok()) << status.error_message();
+    EXPECT_TRUE(response.header().success());
+    EXPECT_EQ(response.status().joint_name(), "test_joint");
+
+    auto& coordinator = globalMotorActivityCoordinator();
+    const auto ticket = coordinator.beginStopAll();
+    ASSERT_TRUE(ticket.valid());
+    std::string stop_error;
+    EXPECT_TRUE(coordinator.requestStop(ticket, &stop_error)) << stop_error;
+    EXPECT_EQ(protocol_->quick_stop_count_.load(), 0);
+    EXPECT_TRUE(coordinator.waitForStopped(
+        ticket, std::chrono::milliseconds(50), &stop_error)) << stop_error;
+    EXPECT_TRUE(coordinator.finishStopAll(ticket, true));
+}
+
+TEST_F(MotorServiceTest, ReleasingArmClaimRestoresDirectMotorControl)
+{
+    std::uint64_t claim_id = 0;
+    ASSERT_TRUE(manager_->claimArmJoints(
+        "test_arm", {"test_joint"}, claim_id));
+    manager_->releaseArmJoints(claim_id);
+
+    api::ProfilePositionRequest request;
+    *request.mutable_target() = makeTarget();
+    request.set_target_position_rad(0.75);
+    request.set_max_velocity_rad_s(1.0);
+    request.set_acceleration_rad_s2(2.0);
+    request.mutable_wait()->set_settle_sample_count(1);
+    request.mutable_wait()->set_poll_period_ms(1);
+    grpc::ServerContext context;
+    api::MotorCommandResponse response;
+
+    const auto status = service_->profilePosition(
+        &context, &request, &response);
+
+    ASSERT_TRUE(status.ok()) << status.error_message();
+    EXPECT_TRUE(response.header().success());
+    EXPECT_DOUBLE_EQ(response.status().position_rad(), 0.75);
+}
+
+TEST_F(MotorServiceTest, ArmClaimCannotBeDisplacedOrReleasedByAnotherToken)
+{
+    std::uint64_t first_claim = 0;
+    ASSERT_TRUE(manager_->claimArmJoints(
+        "first_arm", {"test_joint"}, first_claim));
+
+    std::uint64_t conflicting_claim = 0;
+    std::string claim_error;
+    EXPECT_FALSE(manager_->claimArmJoints(
+        "second_arm", {"test_joint"}, conflicting_claim, &claim_error));
+    EXPECT_EQ(conflicting_claim, 0U);
+    EXPECT_NE(claim_error.find("first_arm"), std::string::npos);
+
+    manager_->releaseArmJoints(first_claim + 1U);
+    EXPECT_EQ(manager_->armOwnerForJoint("test_joint"), "first_arm");
+
+    manager_->releaseArmJoints(first_claim);
+    EXPECT_TRUE(manager_->armOwnerForJoint("test_joint").empty());
+}
+
+TEST_F(MotorServiceTest, ArmClaimDoesNotRejectUnclaimedMotor)
+{
+    auto other_protocol = std::make_shared<FakeMotorProtocol>();
+    auto other_motor = std::make_shared<FakeMotor>(2, "other_joint");
+    other_motor->setProtocol(other_protocol);
+    ASSERT_TRUE(manager_->addMotor(other_motor));
+
+    std::uint64_t claim_id = 0;
+    ASSERT_TRUE(manager_->claimArmJoints(
+        "test_arm", {"test_joint"}, claim_id));
+
+    api::ProfilePositionRequest request;
+    *request.mutable_target() = makeTarget();
+    request.mutable_target()->set_motor_id(2);
+    request.set_target_position_rad(0.5);
+    request.set_max_velocity_rad_s(1.0);
+    request.set_acceleration_rad_s2(2.0);
+    request.mutable_wait()->set_settle_sample_count(1);
+    request.mutable_wait()->set_poll_period_ms(1);
+    grpc::ServerContext context;
+    api::MotorCommandResponse response;
+
+    const auto status = service_->profilePosition(
+        &context, &request, &response);
+
+    ASSERT_TRUE(status.ok()) << status.error_message();
+    EXPECT_TRUE(response.header().success());
+    EXPECT_DOUBLE_EQ(response.status().position_rad(), 0.5);
+}
+
 TEST_F(MotorServiceTest, ProfileVelocityReturnsAfterTargetSettles)
 {
     api::ProfileVelocityRequest request;
@@ -1582,5 +1719,124 @@ TEST_F(MotorServiceTest, CyclicPositionStreamWatchdogStopsSilentClient)
     EXPECT_GE(protocol_->quick_stop_count_.load(), 1);
 }
 
+TEST_F(MotorServiceTest, SystemStopAllPreemptsProfileAndResumesControl)
+{
+    protocol_->hold_position_ = true;
+
+    api::ProfilePositionRequest motion_request;
+    *motion_request.mutable_target() = makeTarget();
+    motion_request.set_target_position_rad(2.0);
+    motion_request.set_max_velocity_rad_s(1.0);
+    motion_request.set_acceleration_rad_s2(1.0);
+    motion_request.mutable_wait()->set_timeout_ms(5000);
+    motion_request.mutable_wait()->set_poll_period_ms(1);
+
+    grpc::ServerContext motion_context;
+    api::MotorCommandResponse motion_response;
+    grpc::Status motion_status;
+    std::thread motion([&]() {
+        motion_status = service_->profilePosition(
+            &motion_context, &motion_request, &motion_response);
+    });
+
+    const auto command_deadline =
+        std::chrono::steady_clock::now() + std::chrono::seconds(1);
+    while (!protocol_->command_started_.load() &&
+           std::chrono::steady_clock::now() < command_deadline) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+    if (!protocol_->command_started_.load()) {
+        motion_context.TryCancel();
+        motion.join();
+        FAIL() << "profile command did not start";
+        return;
+    }
+
+    auto& coordinator = globalMotorActivityCoordinator();
+    const auto ticket = coordinator.beginStopAll();
+    ASSERT_TRUE(ticket.valid());
+    std::string stop_error;
+    EXPECT_TRUE(coordinator.stopAndWait(
+        ticket, std::chrono::seconds(1), &stop_error))
+        << stop_error;
+    EXPECT_TRUE(coordinator.finishStopAll(ticket, true));
+
+    motion.join();
+    EXPECT_EQ(motion_status.error_code(), grpc::StatusCode::ABORTED);
+    EXPECT_FALSE(motion_response.header().success());
+    EXPECT_FALSE(motion_response.status().emergency_stopped());
+    EXPECT_GE(protocol_->quick_stop_count_.load(), 1);
+    EXPECT_GT(protocol_->last_quick_stop_order_.load(),
+              protocol_->profile_command_order_.load());
+
+    protocol_->hold_position_ = false;
+    api::ProfilePositionRequest resumed_request;
+    *resumed_request.mutable_target() = makeTarget();
+    resumed_request.set_target_position_rad(0.5);
+    resumed_request.set_max_velocity_rad_s(1.0);
+    resumed_request.set_acceleration_rad_s2(1.0);
+    resumed_request.mutable_wait()->set_settle_sample_count(1);
+    grpc::ServerContext resumed_context;
+    api::MotorCommandResponse resumed_response;
+    const auto resumed_status = service_->profilePosition(
+        &resumed_context, &resumed_request, &resumed_response);
+    ASSERT_TRUE(resumed_status.ok()) << resumed_status.error_message();
+    EXPECT_TRUE(resumed_response.header().success());
+    EXPECT_FALSE(resumed_response.status().emergency_stopped());
+}
+
+TEST_F(MotorServiceTest, FailedSystemStopAllRemainsFailClosedUntilRecovery)
+{
+    // Resolve the motor once so its control state is registered even though no
+    // motion RPC is active when StopAll starts.
+    api::GetMotorStatusRequest status_request;
+    *status_request.mutable_target() = makeTarget();
+    grpc::ServerContext status_context;
+    api::GetMotorStatusResponse status_response;
+    ASSERT_TRUE(service_->getStatus(
+        &status_context, &status_request, &status_response).ok());
+
+    auto& coordinator = globalMotorActivityCoordinator();
+    protocol_->quick_stop_success_ = false;
+    const auto failed_ticket = coordinator.beginStopAll();
+    ASSERT_TRUE(failed_ticket.valid());
+    std::string stop_error;
+    EXPECT_FALSE(coordinator.stopAndWait(
+        failed_ticket, std::chrono::seconds(1), &stop_error));
+    EXPECT_FALSE(stop_error.empty());
+    EXPECT_FALSE(coordinator.finishStopAll(failed_ticket, false));
+
+    api::ProfilePositionRequest blocked_request;
+    *blocked_request.mutable_target() = makeTarget();
+    blocked_request.set_target_position_rad(1.0);
+    blocked_request.set_max_velocity_rad_s(1.0);
+    blocked_request.set_acceleration_rad_s2(1.0);
+    grpc::ServerContext blocked_context;
+    api::MotorCommandResponse blocked_response;
+    const auto blocked_status = service_->profilePosition(
+        &blocked_context, &blocked_request, &blocked_response);
+    EXPECT_EQ(blocked_status.error_code(), grpc::StatusCode::ABORTED);
+    EXPECT_NE(blocked_status.error_message().find("StopAll"),
+              std::string::npos);
+
+    protocol_->quick_stop_success_ = true;
+    const auto recovery_ticket = coordinator.beginStopAll();
+    ASSERT_TRUE(recovery_ticket.valid());
+    stop_error.clear();
+    ASSERT_TRUE(coordinator.stopAndWait(
+        recovery_ticket, std::chrono::seconds(1), &stop_error))
+        << stop_error;
+    ASSERT_TRUE(coordinator.finishStopAll(recovery_ticket, true));
+
+    blocked_request.mutable_wait()->set_settle_sample_count(1);
+    grpc::ServerContext resumed_context;
+    api::MotorCommandResponse resumed_response;
+    const auto resumed_status = service_->profilePosition(
+        &resumed_context, &blocked_request, &resumed_response);
+    ASSERT_TRUE(resumed_status.ok()) << resumed_status.error_message();
+    EXPECT_TRUE(resumed_response.header().success());
+    EXPECT_FALSE(resumed_response.status().emergency_stopped());
+}
+
 } // namespace
 } // namespace cmvr::service

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

@@ -0,0 +1,262 @@
+#include "service/grpc/include/media_activity_coordinator.h"
+
+#include <atomic>
+#include <chrono>
+#include <future>
+#include <iostream>
+#include <stdexcept>
+#include <thread>
+
+namespace {
+
+bool check(const bool condition, const char* expression, const int line)
+{
+    if (condition) {
+        return true;
+    }
+    std::cerr << "CHECK failed at line " << line << ": " << expression << '\n';
+    return false;
+}
+
+#define CHECK_TRUE(expression) \
+    do { \
+        if (!check(static_cast<bool>(expression), #expression, __LINE__)) { \
+            return 1; \
+        } \
+    } while (false)
+
+} // namespace
+
+int main()
+{
+    using namespace std::chrono_literals;
+
+    {
+        cmvr::service::MediaActivityCoordinator dispatch_coordinator;
+        auto dispatch_session = dispatch_coordinator.beginSession();
+        CHECK_TRUE(dispatch_session);
+
+        std::promise<void> dispatch_entered;
+        auto dispatch_entered_future = dispatch_entered.get_future();
+        std::promise<void> release_dispatch;
+        auto release_dispatch_future = release_dispatch.get_future();
+        auto dispatch_future = std::async(std::launch::async, [&] {
+            return dispatch_session.runIfCurrent([&] {
+                dispatch_entered.set_value();
+                release_dispatch_future.wait();
+            });
+        });
+        dispatch_entered_future.wait();
+
+        const auto dispatch_ticket = dispatch_coordinator.beginStopAll();
+        CHECK_TRUE(dispatch_ticket.valid());
+        CHECK_TRUE(!dispatch_coordinator.waitForStopped(
+            dispatch_ticket, 20ms));
+
+        release_dispatch.set_value();
+        CHECK_TRUE(dispatch_future.get());
+
+        dispatch_session.reset();
+        CHECK_TRUE(dispatch_coordinator.waitForStopped(dispatch_ticket, 100ms));
+        CHECK_TRUE(dispatch_coordinator.finishStopAll(dispatch_ticket, true));
+    }
+
+    {
+        cmvr::service::MediaActivityCoordinator generation_coordinator;
+        auto old_generation_session = generation_coordinator.beginSession();
+        CHECK_TRUE(old_generation_session);
+
+        const auto generation_ticket = generation_coordinator.beginStopAll();
+        CHECK_TRUE(generation_ticket.valid());
+
+        std::atomic<int> dispatched_operations{0};
+        CHECK_TRUE(!old_generation_session.runIfCurrent(
+            [&] { ++dispatched_operations; }));
+        CHECK_TRUE(dispatched_operations.load(std::memory_order_acquire) == 0);
+
+        old_generation_session.reset();
+        CHECK_TRUE(
+            generation_coordinator.waitForStopped(generation_ticket, 100ms));
+        CHECK_TRUE(generation_coordinator.finishStopAll(generation_ticket, true));
+
+        auto current_generation_session = generation_coordinator.beginSession();
+        CHECK_TRUE(current_generation_session);
+        CHECK_TRUE(current_generation_session.runIfCurrent(
+            [&] { ++dispatched_operations; }));
+        CHECK_TRUE(dispatched_operations.load(std::memory_order_acquire) == 1);
+    }
+
+    {
+        cmvr::service::MediaActivityCoordinator deferred_coordinator;
+        std::atomic<int> deferred_cancel_calls{0};
+        auto deferred_session = deferred_coordinator.beginSession(
+            [&] { ++deferred_cancel_calls; });
+        CHECK_TRUE(deferred_session);
+
+        const auto deferred_ticket =
+            deferred_coordinator.beginStopAll(true);
+        CHECK_TRUE(deferred_ticket.valid());
+        CHECK_TRUE(deferred_session.cancelled());
+        CHECK_TRUE(!deferred_session.runIfCurrent([] {}));
+        CHECK_TRUE(
+            deferred_cancel_calls.load(std::memory_order_acquire) == 0);
+        CHECK_TRUE(
+            deferred_coordinator.requestCancellation(deferred_ticket));
+        CHECK_TRUE(
+            deferred_cancel_calls.load(std::memory_order_acquire) == 1);
+        CHECK_TRUE(
+            deferred_coordinator.requestCancellation(deferred_ticket));
+        CHECK_TRUE(
+            deferred_cancel_calls.load(std::memory_order_acquire) == 1);
+
+        deferred_session.reset();
+        CHECK_TRUE(
+            deferred_coordinator.waitForStopped(deferred_ticket, 100ms));
+        CHECK_TRUE(
+            deferred_coordinator.finishStopAll(deferred_ticket, true));
+        CHECK_TRUE(
+            !deferred_coordinator.requestCancellation(deferred_ticket));
+    }
+
+    {
+        cmvr::service::MediaActivityCoordinator parallel_coordinator;
+        std::promise<void> blocked_cancel_entered;
+        auto blocked_cancel_entered_future =
+            blocked_cancel_entered.get_future();
+        std::promise<void> release_blocked_cancel;
+        auto release_blocked_cancel_future =
+            release_blocked_cancel.get_future().share();
+        std::promise<void> peer_cancel_entered;
+        auto peer_cancel_entered_future = peer_cancel_entered.get_future();
+        std::atomic<int> blocked_cancel_calls{0};
+        std::atomic<int> peer_cancel_calls{0};
+
+        auto blocked_session = parallel_coordinator.beginSession([&] {
+            ++blocked_cancel_calls;
+            blocked_cancel_entered.set_value();
+            release_blocked_cancel_future.wait();
+        });
+        auto peer_session = parallel_coordinator.beginSession([&] {
+            ++peer_cancel_calls;
+            peer_cancel_entered.set_value();
+        });
+        CHECK_TRUE(blocked_session && peer_session);
+
+        const auto ticket = parallel_coordinator.beginStopAll(true);
+        std::vector<cmvr::service::DeferredStopOperation> operations;
+        std::string collect_error;
+        CHECK_TRUE(parallel_coordinator.collectCancellationOperations(
+            ticket, operations, &collect_error));
+        CHECK_TRUE(collect_error.empty());
+        CHECK_TRUE(operations.size() == 2);
+        CHECK_TRUE(operations[0].resource_key != operations[1].resource_key);
+
+        auto first = std::async(std::launch::async, operations[0].operation);
+        auto second = std::async(std::launch::async, operations[1].operation);
+        CHECK_TRUE(blocked_cancel_entered_future.wait_for(100ms) ==
+                   std::future_status::ready);
+        CHECK_TRUE(peer_cancel_entered_future.wait_for(100ms) ==
+                   std::future_status::ready);
+        release_blocked_cancel.set_value();
+        CHECK_TRUE(first.get().success);
+        CHECK_TRUE(second.get().success);
+        CHECK_TRUE(operations[0].operation().success);
+        CHECK_TRUE(operations[1].operation().success);
+        CHECK_TRUE(blocked_cancel_calls.load() == 1);
+        CHECK_TRUE(peer_cancel_calls.load() == 1);
+
+        blocked_session.reset();
+        peer_session.reset();
+        CHECK_TRUE(parallel_coordinator.waitForStopped(ticket, 100ms));
+        CHECK_TRUE(parallel_coordinator.finishStopAll(ticket, true));
+        CHECK_TRUE(!parallel_coordinator.collectCancellationOperations(
+            ticket, operations, &collect_error));
+        CHECK_TRUE(!collect_error.empty());
+    }
+
+    {
+        cmvr::service::MediaActivityCoordinator exception_coordinator;
+        std::atomic<int> throwing_cancel_calls{0};
+        auto throwing_session = exception_coordinator.beginSession([&] {
+            ++throwing_cancel_calls;
+            throw std::runtime_error("cancel failed");
+        });
+        CHECK_TRUE(throwing_session);
+        const auto ticket = exception_coordinator.beginStopAll(true);
+        std::vector<cmvr::service::DeferredStopOperation> operations;
+        CHECK_TRUE(exception_coordinator.collectCancellationOperations(
+            ticket, operations));
+        CHECK_TRUE(operations.size() == 1);
+        const auto result = operations[0].operation();
+        const auto cached_result = operations[0].operation();
+        CHECK_TRUE(!result.success);
+        CHECK_TRUE(result.detail.find("cancel failed") != std::string::npos);
+        CHECK_TRUE(cached_result.detail == result.detail);
+        CHECK_TRUE(throwing_cancel_calls.load() == 1);
+        throwing_session.reset();
+        CHECK_TRUE(exception_coordinator.waitForStopped(ticket, 100ms));
+        CHECK_TRUE(!exception_coordinator.finishStopAll(ticket, false));
+    }
+
+    {
+        cmvr::service::DeferredStopOperation detached_operation;
+        cmvr::service::MediaActivityCoordinator::Session retained_session;
+        std::atomic<int> detached_cancel_calls{0};
+        {
+            cmvr::service::MediaActivityCoordinator ephemeral_coordinator;
+            retained_session = ephemeral_coordinator.beginSession(
+                [&] { ++detached_cancel_calls; });
+            const auto ticket =
+                ephemeral_coordinator.beginStopAll(true);
+            std::vector<cmvr::service::DeferredStopOperation> operations;
+            CHECK_TRUE(
+                ephemeral_coordinator.collectCancellationOperations(
+                    ticket, operations));
+            CHECK_TRUE(operations.size() == 1);
+            detached_operation = std::move(operations[0]);
+        }
+        CHECK_TRUE(detached_operation.operation().success);
+        CHECK_TRUE(detached_cancel_calls.load() == 1);
+        retained_session.reset();
+    }
+
+    cmvr::service::MediaActivityCoordinator coordinator;
+    std::atomic<int> cancel_calls{0};
+
+    auto old_session = coordinator.beginSession([&] { ++cancel_calls; });
+    CHECK_TRUE(old_session);
+    CHECK_TRUE(old_session.claimExclusiveResource("speaker:test"));
+
+    auto competing_session = coordinator.beginSession();
+    CHECK_TRUE(competing_session);
+    CHECK_TRUE(!competing_session.claimExclusiveResource("speaker:test"));
+    competing_session.reset();
+
+    const auto ticket = coordinator.beginStopAll();
+    CHECK_TRUE(ticket.valid());
+    CHECK_TRUE(cancel_calls.load(std::memory_order_acquire) == 1);
+    CHECK_TRUE(old_session.cancelled());
+    CHECK_TRUE(!old_session.runIfCurrent([] {}));
+    CHECK_TRUE(!coordinator.beginSession());
+    CHECK_TRUE(!coordinator.waitForStopped(ticket, 5ms));
+
+    old_session.reset();
+    CHECK_TRUE(coordinator.waitForStopped(ticket, 100ms));
+    CHECK_TRUE(coordinator.finishStopAll(ticket, true));
+
+    auto resumed_session = coordinator.beginSession();
+    CHECK_TRUE(resumed_session);
+    CHECK_TRUE(resumed_session.claimExclusiveResource("speaker:test"));
+
+    const auto concurrent_ticket_a = coordinator.beginStopAll();
+    const auto concurrent_ticket_b = coordinator.beginStopAll();
+    resumed_session.reset();
+    CHECK_TRUE(coordinator.waitForStopped(concurrent_ticket_a, 100ms));
+    CHECK_TRUE(coordinator.finishStopAll(concurrent_ticket_a, true));
+    CHECK_TRUE(!coordinator.beginSession());
+    CHECK_TRUE(coordinator.finishStopAll(concurrent_ticket_b, true));
+    CHECK_TRUE(coordinator.beginSession());
+
+    std::cout << "media_activity_coordinator_test: PASS\n";
+    return 0;
+}

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

@@ -0,0 +1,266 @@
+#include "service/grpc/include/motor_activity_coordinator.h"
+
+#include <atomic>
+#include <chrono>
+#include <future>
+#include <iostream>
+#include <stdexcept>
+#include <thread>
+
+namespace {
+
+bool check(const bool condition, const char* expression, const int line)
+{
+    if (condition) {
+        return true;
+    }
+    std::cerr << "CHECK failed at line " << line << ": " << expression << '\n';
+    return false;
+}
+
+#define CHECK_TRUE(expression) \
+    do { \
+        if (!check(static_cast<bool>(expression), #expression, __LINE__)) { \
+            return 1; \
+        } \
+    } while (false)
+
+} // namespace
+
+int main()
+{
+    using namespace std::chrono_literals;
+    using cmvr::service::MotorActivityCoordinator;
+
+    MotorActivityCoordinator coordinator;
+    std::atomic<int> cancel_calls{0};
+    std::atomic<int> quick_stop_calls{0};
+    std::atomic<bool> busy{true};
+
+    auto registration = coordinator.registerControl(
+        [&] { ++cancel_calls; },
+        [&] {
+            ++quick_stop_calls;
+            return true;
+        },
+        [&] { return !busy.load(std::memory_order_acquire); },
+        "test motor");
+    CHECK_TRUE(registration);
+
+    const auto ticket = coordinator.beginStopAll();
+    CHECK_TRUE(ticket.valid());
+    CHECK_TRUE(cancel_calls.load(std::memory_order_acquire) == 1);
+    CHECK_TRUE(!coordinator.lockAdmission().accepting());
+
+    std::string initial_error;
+    CHECK_TRUE(coordinator.requestStop(ticket, &initial_error));
+    CHECK_TRUE(initial_error.empty());
+    CHECK_TRUE(quick_stop_calls.load(std::memory_order_acquire) == 1);
+
+    auto stop_future = std::async(std::launch::async, [&] {
+        std::string error;
+        return coordinator.waitForStopped(ticket, 1s, &error);
+    });
+    CHECK_TRUE(stop_future.wait_for(20ms) == std::future_status::timeout);
+
+    busy.store(false, std::memory_order_release);
+    coordinator.notifyStateChanged();
+    CHECK_TRUE(stop_future.get());
+    CHECK_TRUE(coordinator.finishStopAll(ticket, true));
+    CHECK_TRUE(coordinator.lockAdmission().accepting());
+
+    std::atomic<int> quick_stops_in_flight{0};
+    std::atomic<int> maximum_quick_stops{0};
+    auto make_serial_registration = [&](const char* description) {
+        return coordinator.registerControl(
+            [] {},
+            [&] {
+                const int active = ++quick_stops_in_flight;
+                int maximum = maximum_quick_stops.load();
+                while (maximum < active &&
+                       !maximum_quick_stops.compare_exchange_weak(
+                           maximum, active)) {
+                }
+                std::this_thread::sleep_for(5ms);
+                --quick_stops_in_flight;
+                return true;
+            },
+            [] { return true; },
+            description);
+    };
+    auto serial_a = make_serial_registration("serial motor a");
+    auto serial_b = make_serial_registration("serial motor b");
+    CHECK_TRUE(serial_a && serial_b);
+
+    const auto serial_ticket = coordinator.beginStopAll();
+    CHECK_TRUE(serial_ticket.valid());
+    CHECK_TRUE(coordinator.stopAndWait(serial_ticket, 1s));
+    CHECK_TRUE(maximum_quick_stops.load(std::memory_order_acquire) == 1);
+    CHECK_TRUE(coordinator.finishStopAll(serial_ticket, true));
+
+    {
+        MotorActivityCoordinator parallel_coordinator;
+        std::promise<void> blocked_stop_entered;
+        auto blocked_stop_entered_future =
+            blocked_stop_entered.get_future();
+        std::promise<void> release_blocked_stop;
+        auto release_blocked_stop_future =
+            release_blocked_stop.get_future().share();
+        std::promise<void> peer_stop_entered;
+        auto peer_stop_entered_future = peer_stop_entered.get_future();
+        std::atomic<int> blocked_cancel_calls{0};
+        std::atomic<int> blocked_stop_calls{0};
+        std::atomic<int> peer_cancel_calls{0};
+        std::atomic<int> peer_stop_calls{0};
+
+        auto blocked = parallel_coordinator.registerControl(
+            [&] { ++blocked_cancel_calls; },
+            [&] {
+                if (blocked_cancel_calls.load() != 1) {
+                    return false;
+                }
+                ++blocked_stop_calls;
+                blocked_stop_entered.set_value();
+                release_blocked_stop_future.wait();
+                return true;
+            },
+            [] { return true; },
+            "blocked motor");
+        auto peer = parallel_coordinator.registerControl(
+            [&] { ++peer_cancel_calls; },
+            [&] {
+                if (peer_cancel_calls.load() != 1) {
+                    return false;
+                }
+                ++peer_stop_calls;
+                peer_stop_entered.set_value();
+                return true;
+            },
+            [] { return true; },
+            "peer motor");
+        CHECK_TRUE(blocked && peer);
+
+        const auto parallel_ticket =
+            parallel_coordinator.beginStopAll(true);
+        CHECK_TRUE(parallel_ticket.valid());
+        CHECK_TRUE(blocked_cancel_calls.load() == 0);
+        CHECK_TRUE(peer_cancel_calls.load() == 0);
+        std::vector<cmvr::service::DeferredStopOperation> operations;
+        std::string collect_error;
+        CHECK_TRUE(parallel_coordinator.collectStopOperations(
+            parallel_ticket, operations, &collect_error));
+        CHECK_TRUE(collect_error.empty());
+        CHECK_TRUE(operations.size() == 2);
+        CHECK_TRUE(operations[0].resource_key != operations[1].resource_key);
+
+        auto first = std::async(std::launch::async, operations[0].operation);
+        auto second = std::async(std::launch::async, operations[1].operation);
+        CHECK_TRUE(blocked_stop_entered_future.wait_for(100ms) ==
+                   std::future_status::ready);
+        CHECK_TRUE(peer_stop_entered_future.wait_for(100ms) ==
+                   std::future_status::ready);
+        release_blocked_stop.set_value();
+        CHECK_TRUE(first.get().success);
+        CHECK_TRUE(second.get().success);
+        CHECK_TRUE(operations[0].operation().success);
+        CHECK_TRUE(operations[1].operation().success);
+        CHECK_TRUE(blocked_cancel_calls.load() == 1);
+        CHECK_TRUE(blocked_stop_calls.load() == 1);
+        CHECK_TRUE(peer_cancel_calls.load() == 1);
+        CHECK_TRUE(peer_stop_calls.load() == 1);
+        CHECK_TRUE(parallel_coordinator.finishStopAll(
+            parallel_ticket, true));
+    }
+
+    {
+        MotorActivityCoordinator exception_coordinator;
+        std::atomic<int> cancel_calls_for_exception{0};
+        std::atomic<int> stop_calls_after_exception{0};
+        auto throwing = exception_coordinator.registerControl(
+            [&] {
+                ++cancel_calls_for_exception;
+                throw std::runtime_error("cancel failed");
+            },
+            [&] {
+                ++stop_calls_after_exception;
+                throw std::runtime_error("quick-stop failed");
+                return false;
+            },
+            [] { return true; },
+            "throwing motor");
+        CHECK_TRUE(throwing);
+        const auto exception_ticket =
+            exception_coordinator.beginStopAll(true);
+        std::vector<cmvr::service::DeferredStopOperation> operations;
+        CHECK_TRUE(exception_coordinator.collectStopOperations(
+            exception_ticket, operations));
+        CHECK_TRUE(operations.size() == 1);
+        const auto first_result = operations[0].operation();
+        const auto cached_result = operations[0].operation();
+        CHECK_TRUE(!first_result.success);
+        CHECK_TRUE(first_result.detail.find("cancel failed") !=
+                   std::string::npos);
+        CHECK_TRUE(first_result.detail.find("quick-stop failed") !=
+                   std::string::npos);
+        CHECK_TRUE(cached_result.detail == first_result.detail);
+        CHECK_TRUE(cancel_calls_for_exception.load() == 1);
+        CHECK_TRUE(stop_calls_after_exception.load() == 1);
+        CHECK_TRUE(!exception_coordinator.finishStopAll(
+            exception_ticket, false));
+    }
+
+    {
+        cmvr::service::DeferredStopOperation detached_operation;
+        MotorActivityCoordinator::Registration retained_registration;
+        std::atomic<int> detached_cancel_calls{0};
+        std::atomic<int> detached_stop_calls{0};
+        {
+            MotorActivityCoordinator ephemeral_coordinator;
+            retained_registration = ephemeral_coordinator.registerControl(
+                [&] { ++detached_cancel_calls; },
+                [&] {
+                    ++detached_stop_calls;
+                    return true;
+                },
+                [] { return true; },
+                "detached motor");
+            const auto ticket =
+                ephemeral_coordinator.beginStopAll(true);
+            std::vector<cmvr::service::DeferredStopOperation> operations;
+            CHECK_TRUE(ephemeral_coordinator.collectStopOperations(
+                ticket, operations));
+            CHECK_TRUE(operations.size() == 1);
+            detached_operation = std::move(operations[0]);
+        }
+        CHECK_TRUE(detached_operation.operation().success);
+        CHECK_TRUE(detached_cancel_calls.load() == 1);
+        CHECK_TRUE(detached_stop_calls.load() == 1);
+        retained_registration.reset();
+    }
+
+    std::atomic<bool> stop_succeeds{false};
+    auto failing = coordinator.registerControl(
+        [] {},
+        [&] { return stop_succeeds.load(std::memory_order_acquire); },
+        [] { return true; },
+        "failing motor");
+    CHECK_TRUE(failing);
+
+    const auto failed_ticket = coordinator.beginStopAll();
+    CHECK_TRUE(failed_ticket.valid());
+    std::string error;
+    CHECK_TRUE(!coordinator.stopAndWait(failed_ticket, 100ms, &error));
+    CHECK_TRUE(!error.empty());
+    CHECK_TRUE(!coordinator.finishStopAll(failed_ticket, false));
+    CHECK_TRUE(!coordinator.lockAdmission().accepting());
+
+    stop_succeeds.store(true, std::memory_order_release);
+    const auto recovery_ticket = coordinator.beginStopAll();
+    CHECK_TRUE(recovery_ticket.valid());
+    CHECK_TRUE(coordinator.stopAndWait(recovery_ticket, 1s, &error));
+    CHECK_TRUE(coordinator.finishStopAll(recovery_ticket, true));
+    CHECK_TRUE(coordinator.lockAdmission().accepting());
+
+    std::cout << "motor_activity_coordinator_test: PASS\n";
+    return 0;
+}

cmvr-es/service/quic_edge/CMakeLists.txt

@@ -23,6 +23,28 @@ target_link_libraries(quic_edge_service
 )
 
 add_library(cmvr_es::quic_edge_service ALIAS quic_edge_service)
+
+if(BUILD_TESTING)
+    add_executable(quic_edge_protocol_test tests/quic_edge_protocol_test.cpp)
+    target_compile_features(quic_edge_protocol_test PRIVATE cxx_std_17)
+    target_link_libraries(quic_edge_protocol_test PRIVATE
+            cmvr_es::quic_edge_service
+            cmvr_es::media_source_hub
+            cmvr_es::stop_all_admission_gate
+            Threads::Threads
+    )
+    add_test(NAME quic_edge_protocol_test COMMAND quic_edge_protocol_test)
+    set(_quic_edge_test_environment
+            "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}")
+    if(CMVR_TEST_SYSTEM_LIBSTDCXX)
+        list(APPEND _quic_edge_test_environment
+                "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
+    endif()
+    set_tests_properties(quic_edge_protocol_test PROPERTIES
+            TIMEOUT 15
+            ENVIRONMENT "${_quic_edge_test_environment}")
+endif()
+
 install(TARGETS quic_edge_service
         ARCHIVE DESTINATION lib
         LIBRARY DESTINATION lib)

cmvr-es/service/quic_edge/include/quic_edge_service.h

@@ -88,6 +88,10 @@ public:
 
     bool initialize(std::string* error);
     bool start(std::string* error);
+    // Releases the current media subscriptions without stopping the QUIC
+    // transport, node registration, heartbeat loop, or service worker. The
+    // media worker automatically subscribes again when admission permits it.
+    bool interruptMediaActivities();
     void stop();
 
     QuicEdgeServiceState state() const;
@@ -124,8 +128,10 @@ private:
     bool dispatchControlFrame(const std::vector<std::uint8_t>& frame,
                               std::string* error);
     bool openMediaSession(std::vector<ActiveTrack>* tracks,
+                          std::uint64_t activity_generation,
                           std::string* error);
-    void refreshMediaTracks(std::vector<ActiveTrack>* tracks);
+    void refreshMediaTracks(std::vector<ActiveTrack>* tracks,
+                            std::uint64_t activity_generation);
     bool hasEnabledMediaTracks() const;
     bool ensureSourceRegistered(const config::QuicEdgeTrackConfig& track,
                                 const std::string& source_track_id,
@@ -161,6 +167,7 @@ private:
     mutable std::mutex mutex_;
     std::condition_variable stop_cv_;
     std::condition_variable media_stop_cv_;
+    std::condition_variable media_activity_cv_;
     QuicEdgeServiceState state_{QuicEdgeServiceState::UNINITIALIZED};
     std::string last_error_;
     std::string last_media_error_;
@@ -174,6 +181,9 @@ private:
     std::size_t active_media_tracks_{0};
     bool stop_requested_{false};
     bool media_stop_requested_{false};
+    bool media_worker_running_{false};
+    std::uint64_t media_activity_generation_{1U};
+    std::uint64_t media_activity_quiesced_generation_{1U};
     bool media_connection_failed_{false};
     std::string media_connection_error_;
     std::thread worker_;

cmvr-es/service/quic_edge/src/quic_edge_service.cpp

@@ -635,6 +635,31 @@ bool QuicEdgeService::start(std::string* error)
     return true;
 }
 
+bool QuicEdgeService::interruptMediaActivities()
+{
+    std::unique_lock lock(mutex_);
+    if (state_ == QuicEdgeServiceState::FAILED) {
+        return false;
+    }
+    if (media_activity_generation_ ==
+        std::numeric_limits<std::uint64_t>::max()) {
+        media_activity_generation_ = 1U;
+        media_activity_quiesced_generation_ = 0U;
+    } else {
+        ++media_activity_generation_;
+    }
+    const std::uint64_t requested_generation = media_activity_generation_;
+    media_stop_cv_.notify_all();
+
+    media_activity_cv_.wait(lock, [this, requested_generation] {
+        return !media_worker_running_ || stop_requested_ ||
+            media_activity_quiesced_generation_ >= requested_generation;
+    });
+    return state_ != QuicEdgeServiceState::FAILED &&
+        (!media_worker_running_ ||
+         media_activity_quiesced_generation_ >= requested_generation);
+}
+
 void QuicEdgeService::stop()
 {
     std::lock_guard lifecycle_lock(lifecycle_mutex_);
@@ -931,10 +956,18 @@ bool QuicEdgeService::startMediaWorker(std::string* error)
         media_connection_failed_ = false;
         media_connection_error_.clear();
         active_media_tracks_ = 0U;
+        media_worker_running_ = true;
     }
     try {
         media_worker_ = std::thread(&QuicEdgeService::runMedia, this);
     } catch (const std::exception& exception) {
+        {
+            std::lock_guard lock(mutex_);
+            media_worker_running_ = false;
+            media_activity_quiesced_generation_ =
+                media_activity_generation_;
+        }
+        media_activity_cv_.notify_all();
         const std::string message =
             std::string("failed to start QUIC media worker: ") + exception.what();
         recordMediaConnectionFailure(message);
@@ -952,8 +985,13 @@ void QuicEdgeService::stopMediaWorker()
     }
     media_stop_cv_.notify_all();
     if (media_worker_.joinable()) media_worker_.join();
-    std::lock_guard lock(mutex_);
-    active_media_tracks_ = 0U;
+    {
+        std::lock_guard lock(mutex_);
+        active_media_tracks_ = 0U;
+        media_worker_running_ = false;
+        media_activity_quiesced_generation_ = media_activity_generation_;
+    }
+    media_activity_cv_.notify_all();
 }
 
 void QuicEdgeService::recordMediaConnectionFailure(const std::string& error)
@@ -974,12 +1012,36 @@ void QuicEdgeService::recordMediaConnectionFailure(const std::string& error)
 void QuicEdgeService::runMedia()
 {
     std::vector<ActiveTrack> tracks;
+    std::uint64_t activity_generation = 0U;
+    {
+        std::lock_guard lock(mutex_);
+        activity_generation = media_activity_generation_;
+        media_activity_quiesced_generation_ = activity_generation;
+    }
+    media_activity_cv_.notify_all();
     try {
         next_media_source_retry_ = std::chrono::steady_clock::now();
         while (true) {
+            std::uint64_t requested_generation = 0U;
             {
                 std::lock_guard lock(mutex_);
                 if (stop_requested_ || media_stop_requested_) break;
+                requested_generation = media_activity_generation_;
+            }
+            if (requested_generation != activity_generation) {
+                // Clearing the tracks is the publication barrier for this
+                // generation: subscriptions are released before StopAll is
+                // told that old QUIC media activity has quiesced.
+                tracks.clear();
+                {
+                    std::lock_guard lock(mutex_);
+                    active_media_tracks_ = 0U;
+                    activity_generation = requested_generation;
+                    media_activity_quiesced_generation_ =
+                        requested_generation;
+                }
+                media_activity_cv_.notify_all();
+                continue;
             }
             if (!transport_->isConnected()) break;
 
@@ -992,7 +1054,8 @@ void QuicEdgeService::runMedia()
                             return !track.subscription.valid();
                         }),
                     tracks.end());
-                if (!openMediaSession(&tracks, &error)) {
+                if (!openMediaSession(
+                        &tracks, activity_generation, &error)) {
                     recordMediaConnectionFailure(error);
                     break;
                 }
@@ -1039,8 +1102,13 @@ void QuicEdgeService::runMedia()
         recordMediaConnectionFailure("unknown QUIC media worker exception");
     }
     tracks.clear();
-    std::lock_guard lock(mutex_);
-    active_media_tracks_ = 0U;
+    {
+        std::lock_guard lock(mutex_);
+        active_media_tracks_ = 0U;
+        media_worker_running_ = false;
+        media_activity_quiesced_generation_ = media_activity_generation_;
+    }
+    media_activity_cv_.notify_all();
 }
 
 bool QuicEdgeService::performRegistration(ControlFrameDecoder* decoder,
@@ -1326,7 +1394,9 @@ bool QuicEdgeService::dispatchControlFrame(
     return false;
 }
 
-bool QuicEdgeService::openMediaSession(std::vector<ActiveTrack>* tracks,
+bool QuicEdgeService::openMediaSession(
+    std::vector<ActiveTrack>* tracks,
+    const std::uint64_t activity_generation,
                                        std::string* error)
 {
     if (!tracks) {
@@ -1351,11 +1421,13 @@ bool QuicEdgeService::openMediaSession(std::vector<ActiveTrack>* tracks,
             ++stats_.media_sessions_opened;
         }
     }
-    refreshMediaTracks(tracks);
+    refreshMediaTracks(tracks, activity_generation);
     return true;
 }
 
-void QuicEdgeService::refreshMediaTracks(std::vector<ActiveTrack>* tracks)
+void QuicEdgeService::refreshMediaTracks(
+    std::vector<ActiveTrack>* tracks,
+    const std::uint64_t activity_generation)
 {
     if (!tracks) return;
     for (const auto& track_config : config_.tracks()) {
@@ -1379,9 +1451,10 @@ void QuicEdgeService::refreshMediaTracks(std::vector<ActiveTrack>* tracks)
         track.subscription = media_hub_->subscribe(
             track.source_track_id,
             media::MediaSourceHub::StartPosition::LATEST_AVAILABLE,
-            [this] {
+            [this, activity_generation] {
                 std::lock_guard lock(mutex_);
-                return stop_requested_ || media_stop_requested_;
+                return stop_requested_ || media_stop_requested_ ||
+                    media_activity_generation_ != activity_generation;
             });
         if (!track.subscription.valid()) {
             recordMediaError(

cmvr-es/service/quic_edge/tests/quic_edge_protocol_test.cpp

@@ -18,6 +18,7 @@
 #include "service/quic_edge/include/control_framing.h"
 #include "service/quic_edge/include/datagram_packetizer.h"
 #include "service/quic_edge/include/quic_edge_service.h"
+#include "service/stop_all/include/stop_all_admission_gate.h"
 
 namespace {
 
@@ -576,6 +577,154 @@ bool testServiceWithSharedHub()
     return true;
 }
 
+bool testMediaActivityInterruptPreservesPresenceAndResumes()
+{
+    const std::string track_id = "camera-stop-all/video/color";
+    service::StopAllAdmissionGate admission;
+    media::MediaSourceHub hub(&admission);
+    media::MediaSourceHub::FrameSink sink;
+    std::mutex sink_mutex;
+    std::atomic<bool> source_started{false};
+    std::atomic<std::uint64_t> source_starts{0U};
+    std::atomic<std::uint64_t> source_stops{0U};
+    media::MediaSourceHub::SourceCallbacks callbacks;
+    callbacks.start = [&](const media::MediaSourceHub::FrameSink& value,
+                          const media::MediaSourceHub::CancelPredicate&) {
+        {
+            std::lock_guard lock(sink_mutex);
+            sink = value;
+        }
+        source_started.store(true);
+        ++source_starts;
+        return true;
+    };
+    callbacks.stop = [&]() {
+        source_started.store(false);
+        ++source_stops;
+    };
+    callbacks.request_key_frame = [] { return true; };
+    const auto descriptor = videoDescriptor(track_id);
+    CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 8U));
+
+    auto transport = std::make_unique<FakeTransport>();
+    FakeTransport* transport_view = transport.get();
+    quic_edge::QuicEdgeService edge_service(
+        validConfig(track_id), std::move(transport), hub);
+    std::string error;
+    CHECK_TRUE(edge_service.initialize(&error));
+    CHECK_TRUE(edge_service.start(&error));
+    CHECK_TRUE(waitUntil([&] {
+        return edge_service.state() == quic_edge::QuicEdgeServiceState::ONLINE &&
+               edge_service.status().registered && source_started.load() &&
+               hub.subscriberCount(track_id) == 1U;
+    }));
+
+    auto publish = [&](const std::uint64_t sequence) {
+        media::MediaSourceHub::FrameSink publisher;
+        {
+            std::lock_guard lock(sink_mutex);
+            publisher = sink;
+        }
+        if (!publisher) return false;
+        media::MediaFrame::Config frame;
+        frame.descriptor = descriptor;
+        frame.payload.resize(256U, 0x5aU);
+        frame.sequence = sequence;
+        frame.capture_time_ns = sequence * 1000000U;
+        frame.key_frame = true;
+        publisher(media::makeMediaFrame(std::move(frame)));
+        return true;
+    };
+
+    CHECK_TRUE(publish(1U));
+    CHECK_TRUE(waitUntil([&] {
+        return transport_view->datagramBatchCount() == 1U;
+    }));
+    const auto heartbeat_before_stop = transport_view->heartbeatCount();
+
+    const auto ticket = admission.beginStopAll();
+    CHECK_TRUE(edge_service.interruptMediaActivities());
+    CHECK_TRUE(hub.subscriberCount(track_id) == 0U);
+    CHECK_TRUE(!source_started.load());
+    CHECK_TRUE(source_stops.load() == 1U);
+    CHECK_TRUE(edge_service.state() == quic_edge::QuicEdgeServiceState::ONLINE);
+    CHECK_TRUE(edge_service.status().registered);
+    CHECK_TRUE(transport_view->connectCount() == 1U);
+
+    // A publisher retained by the stopped generation must no longer enqueue
+    // frames while StopAll admission is closed.
+    CHECK_TRUE(publish(2U));
+    std::this_thread::sleep_for(std::chrono::milliseconds(30));
+    CHECK_TRUE(transport_view->datagramBatchCount() == 1U);
+    CHECK_TRUE(admission.finishStopAll(ticket, true));
+
+    CHECK_TRUE(waitUntil([&] {
+        return source_starts.load() == 2U && source_started.load() &&
+               hub.subscriberCount(track_id) == 1U;
+    }));
+    CHECK_TRUE(publish(3U));
+    CHECK_TRUE(waitUntil([&] {
+        return transport_view->datagramBatchCount() == 2U;
+    }));
+    CHECK_TRUE(waitUntil([&] {
+        return transport_view->heartbeatCount() > heartbeat_before_stop;
+    }));
+    CHECK_TRUE(edge_service.stats().registrations_accepted == 1U);
+    CHECK_TRUE(transport_view->connectCount() == 1U);
+    CHECK_TRUE(edge_service.state() == quic_edge::QuicEdgeServiceState::ONLINE);
+    edge_service.stop();
+    return true;
+}
+
+bool testMediaActivityInterruptCancelsStartingSubscription()
+{
+    const std::string track_id = "slow-stop-all/video/color";
+    service::StopAllAdmissionGate admission;
+    media::MediaSourceHub hub(&admission);
+    std::atomic<bool> start_entered{false};
+    std::atomic<bool> start_cancelled{false};
+    media::MediaSourceHub::SourceCallbacks callbacks;
+    callbacks.start = [&](const media::MediaSourceHub::FrameSink&,
+                          const media::MediaSourceHub::CancelPredicate& cancelled) {
+        start_entered.store(true);
+        while (!cancelled()) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(2));
+        }
+        start_cancelled.store(true);
+        return false;
+    };
+    callbacks.stop = [] {};
+    CHECK_TRUE(hub.registerSource(
+        videoDescriptor(track_id), std::move(callbacks), 8U));
+
+    auto transport = std::make_unique<FakeTransport>();
+    FakeTransport* transport_view = transport.get();
+    quic_edge::QuicEdgeService edge_service(
+        validConfig(track_id), std::move(transport), hub);
+    std::string error;
+    CHECK_TRUE(edge_service.initialize(&error));
+    CHECK_TRUE(edge_service.start(&error));
+    CHECK_TRUE(waitUntil([&] {
+        return start_entered.load() && edge_service.status().registered;
+    }));
+
+    const auto ticket = admission.beginStopAll();
+    auto interrupt = std::async(std::launch::async, [&edge_service] {
+        return edge_service.interruptMediaActivities();
+    });
+    CHECK_TRUE(interrupt.wait_for(std::chrono::milliseconds(500)) ==
+               std::future_status::ready);
+    CHECK_TRUE(interrupt.get());
+    CHECK_TRUE(start_cancelled.load());
+    CHECK_TRUE(hub.subscriberCount(track_id) == 0U);
+    CHECK_TRUE(edge_service.state() == quic_edge::QuicEdgeServiceState::ONLINE);
+    CHECK_TRUE(edge_service.status().registered);
+    CHECK_TRUE(transport_view->connectCount() == 1U);
+    CHECK_TRUE(admission.finishStopAll(ticket, true));
+    edge_service.stop();
+    return true;
+}
+
 bool testMissingInjectedSourceRetriesSafely()
 {
     media::MediaSourceHub hub;
@@ -1048,7 +1197,10 @@ bool testRobotIdIsRequired()
 int main()
 {
     if (!testControlFraming() || !testPacketizer() ||
-        !testServiceWithSharedHub() || !testMissingInjectedSourceRetriesSafely() ||
+        !testServiceWithSharedHub() ||
+        !testMediaActivityInterruptPreservesPresenceAndResumes() ||
+        !testMediaActivityInterruptCancelsStartingSubscription() ||
+        !testMissingInjectedSourceRetriesSafely() ||
         !testPresenceOnlyWithoutMedia() ||
         !testDeviceManagerSnapshotInHeartbeat() ||
         !testAllDeviceKindAndStateMappings() ||

cmvr-es/service/stop_all/CMakeLists.txt

@@ -0,0 +1,28 @@
+add_library(stop_all_admission_gate STATIC
+        src/stop_all_admission_gate.cpp
+)
+
+target_compile_features(stop_all_admission_gate PUBLIC cxx_std_17)
+target_include_directories(stop_all_admission_gate
+        PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}/../..
+)
+
+add_library(cmvr_es::stop_all_admission_gate ALIAS stop_all_admission_gate)
+
+add_library(camera_operational_activity_registry STATIC
+        ../grpc/src/camera_operational_activity_registry.cpp
+)
+
+target_compile_features(camera_operational_activity_registry PUBLIC cxx_std_17)
+target_include_directories(camera_operational_activity_registry
+        PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}/../..
+)
+target_link_libraries(camera_operational_activity_registry
+        PUBLIC
+        cmvr_es::stop_all_admission_gate
+)
+
+add_library(cmvr_es::camera_operational_activity_registry ALIAS
+        camera_operational_activity_registry)