fix(aubo): make stop release control safely

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

@@ -67,6 +67,19 @@ if(BUILD_TESTING)
     )
     set_tests_properties(aubo_arm_motion_result_test PROPERTIES TIMEOUT 10)
 
+    add_executable(aubo_motion_state_test
+            tests/aubo_motion_state_test.cpp
+    )
+    target_include_directories(aubo_motion_state_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+    )
+    add_test(
+            NAME aubo_motion_state_test
+            COMMAND aubo_motion_state_test
+    )
+    set_tests_properties(aubo_motion_state_test PROPERTIES TIMEOUT 10)
+
     add_executable(aubo_arm_json_command_test
             tests/aubo_arm_json_command_test.cpp
     )

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

@@ -1,6 +1,7 @@
 #include "devices/arm/aubo_arm/aubo_arm.h"
 
 #include "devices/arm/aubo_arm/aubo_motion_result.h"
+#include "devices/arm/aubo_arm/aubo_motion_state.h"
 
 #include <algorithm>
 #include <cctype>
@@ -18,11 +19,133 @@
 namespace cmvr::device {
 namespace {
 
-struct BusyGuard {
+class MotionOwnerGuard final {
-    std::atomic<bool>& busy;
+public:
-    ~BusyGuard() { busy.store(false); }
+    MotionOwnerGuard(
+        aubo_internal::MotionState& state,
+        std::atomic<bool>& busy,
+        const aubo_internal::MotionToken token)
+        : state_(state), busy_(busy), token_(token)
+    {
+    }
+
+    ~MotionOwnerGuard() noexcept
+    {
+        try {
+            if (requires_settlement_ && !settled_) {
+                state_.failMotion(token_);
+            } else {
+                state_.finish(token_, finish_mode_);
+            }
+            busy_.store(state_.busy());
+        } catch (...) {
+            // A failed state lock must not terminate an RPC unwind. Preserve
+            // the conservative externally visible state instead.
+            busy_.store(true);
+        }
+    }
+
+    MotionOwnerGuard(const MotionOwnerGuard&) = delete;
+    MotionOwnerGuard& operator=(const MotionOwnerGuard&) = delete;
+
+    void requireExplicitSettlement() noexcept
+    {
+        requires_settlement_ = true;
+    }
+
+    void settle() noexcept { settled_ = true; }
+
+    void clearOnFinish() noexcept
+    {
+        finish_mode_ = aubo_internal::MotionFinishMode::Clear;
+    }
+
+    void retainKind() noexcept
+    {
+        finish_mode_ = aubo_internal::MotionFinishMode::Retain;
+    }
+
+private:
+    aubo_internal::MotionState& state_;
+    std::atomic<bool>& busy_;
+    aubo_internal::MotionToken token_;
+    aubo_internal::MotionFinishMode finish_mode_{
+        aubo_internal::MotionFinishMode::RestorePrevious};
+    bool requires_settlement_{false};
+    bool settled_{false};
 };
 
+class StopStateGuard final {
+public:
+    StopStateGuard(
+        aubo_internal::MotionState& state,
+        std::atomic<bool>& busy)
+        : state_(state), busy_(busy)
+    {
+    }
+
+    ~StopStateGuard() noexcept
+    {
+        if (!completed_) {
+            try {
+                state_.failStop();
+            } catch (...) {
+                // Keep the facade fail-closed even if state cleanup fails.
+            }
+            busy_.store(true);
+        }
+    }
+
+    StopStateGuard(const StopStateGuard&) = delete;
+    StopStateGuard& operator=(const StopStateGuard&) = delete;
+
+    bool complete()
+    {
+        if (!state_.completeStop()) {
+            return false;
+        }
+        busy_.store(false);
+        completed_ = true;
+        return true;
+    }
+
+private:
+    aubo_internal::MotionState& state_;
+    std::atomic<bool>& busy_;
+    bool completed_{false};
+};
+
+const char* motionStartFailure(
+    const aubo_internal::MotionStartStatus status) noexcept
+{
+    switch (status) {
+    case aubo_internal::MotionStartStatus::Invalid:
+        return "invalid motion type";
+    case aubo_internal::MotionStartStatus::Busy:
+        return "another motion is active";
+    case aubo_internal::MotionStartStatus::Stopping:
+        return "a stop operation is in progress";
+    case aubo_internal::MotionStartStatus::Blocked:
+        return "the previous stop did not complete; retry stopMotion or reconnect";
+    case aubo_internal::MotionStartStatus::Started:
+        break;
+    }
+    return "unknown motion state";
+}
+
+const char* motionKindName(const aubo_internal::MotionKind kind) noexcept
+{
+    switch (kind) {
+    case aubo_internal::MotionKind::Joint:
+        return "joint";
+    case aubo_internal::MotionKind::Linear:
+        return "linear";
+    case aubo_internal::MotionKind::None:
+        break;
+    }
+    return "unknown";
+}
+
 std::vector<std::string> defaultJointNames(const std::size_t dof)
 {
     std::vector<std::string> names;
@@ -182,21 +305,40 @@ bool waitForRobotMode(const RobotInterfacePtr& robot_interface,
     return false;
 }
 
-int waitArrival(const RobotInterfacePtr& robot_interface)
+template <typename IsCancelled>
+aubo_internal::MotionWaitResult waitArrival(
+    const RobotInterfacePtr& robot_interface,
+    IsCancelled&& is_cancelled)
 {
     int retry_count = 0;
+    if (is_cancelled()) {
+        return aubo_internal::MotionWaitResult::Cancelled;
+    }
     int exec_id = robot_interface->getMotionControl()->getExecId();
     while (exec_id == -1 && retry_count++ < 5) {
+        if (is_cancelled()) {
+            return aubo_internal::MotionWaitResult::Cancelled;
+        }
         std::this_thread::sleep_for(std::chrono::milliseconds(50));
+        if (is_cancelled()) {
+            return aubo_internal::MotionWaitResult::Cancelled;
+        }
         exec_id = robot_interface->getMotionControl()->getExecId();
     }
     if (exec_id == -1) {
-        return -1;
+        return is_cancelled()
+            ? aubo_internal::MotionWaitResult::Cancelled
+            : aubo_internal::MotionWaitResult::Failed;
     }
     while (robot_interface->getMotionControl()->getExecId() != -1) {
+        if (is_cancelled()) {
+            return aubo_internal::MotionWaitResult::Cancelled;
+        }
         std::this_thread::sleep_for(std::chrono::milliseconds(50));
     }
-    return 0;
+    return is_cancelled()
+        ? aubo_internal::MotionWaitResult::Cancelled
+        : aubo_internal::MotionWaitResult::Completed;
 }
 
 bool waitServoModeSelect(const RobotInterfacePtr& robot_interface, const int mode)
@@ -228,6 +370,8 @@ CartesianPose poseFromVector(const std::vector<double>& values)
 
 struct AuboArm::SdkState {
     std::shared_ptr<arcs::aubo_sdk::RpcClient> rpc_client;
+    std::shared_ptr<aubo_internal::MotionState> motion_state{
+        std::make_shared<aubo_internal::MotionState>()};
 };
 
 AuboArm::AuboArm(const config::RobotArmConfig& cfg)
@@ -423,7 +567,7 @@ ArmState AuboArm::getRobotState() const
     state.connected = connected_.load();
     state.powered_on = state.connected;
     state.brake_released = state.connected;
-    state.moving = busy_.load();
+    state.moving = busy();
     state.robot_mode = getRobotMode();
     state.safety_mode = getSafetyMode();
     state.control_mode = getControlMode();
@@ -515,7 +659,16 @@ RobotMode AuboArm::getRobotMode() const
     if (emergency_stopped_) {
         return RobotMode::Stopped;
     }
-    return busy_.load() ? RobotMode::Running : RobotMode::Idle;
+    return busy() ? RobotMode::Running : RobotMode::Idle;
+}
+
+bool AuboArm::busy() const
+{
+    std::lock_guard lock(mutex_);
+    if (sdk_) {
+        return sdk_->motion_state->busy();
+    }
+    return false;
 }
 
 Result AuboArm::torqueOn()
@@ -612,37 +765,60 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o
     if (!validDof_(target.position.size(), error)) {
         return Result::failure(ArmErrorCode::InvalidDof, error);
     }
-    const auto ready = ensureConnected_("moveJ");
-    if (!ready.ok()) {
-        return ready;
-    }
-    if (busy_.exchange(true)) {
-        return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_);
-    }
-    BusyGuard busy_guard{busy_};
-
     try {
-        const auto robot_names = sdk_->rpc_client->getRobotNames();
+        std::unique_lock submit_lock(mutex_);
+        const auto locked_ready = ensureConnected_("moveJ");
+        if (!locked_ready.ok()) {
+            return locked_ready;
+        }
+        const auto rpc_client = sdk_->rpc_client;
+        const auto motion_state = sdk_->motion_state;
+        const auto motion = motion_state->begin(
+            aubo_internal::MotionKind::Joint);
+        if (!motion.started()) {
+            return Result::failure(
+                ArmErrorCode::RobotNotReady,
+                "[AuboArm] moveJ rejected: " +
+                    std::string(motionStartFailure(motion.status)) +
+                    ", id=" + id_);
+        }
+        busy_.store(true);
+        MotionOwnerGuard motion_owner{
+            *motion_state, busy_, motion.token};
+
+        const auto robot_names = rpc_client->getRobotNames();
         if (robot_names.empty()) {
             return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
         }
-        auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
+        auto robot_interface = rpc_client->getRobotInterface(robot_names.front());
         if (!robot_interface) {
             return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
         }
         auto motion_control = robot_interface->getMotionControl();
         motion_control->setSpeedFraction(speed_scaling_);
+        motion_owner.requireExplicitSettlement();
         const int ret = motion_control->moveJoint(
             target.position,
             options.acceleration > 0.0 ? options.acceleration : 0.5,
             options.velocity > 0.0 ? options.velocity : 0.5,
             options.blend_radius,
             0);
+        if (ret == arcs::common_interface::AUBO_OK) {
+            submit_lock.unlock();
+        } else {
+            motion_owner.settle();
+        }
         const auto outcome = aubo_internal::resolveMotionCommand(
             ret,
             arcs::common_interface::AUBO_OK,
             arcs::common_interface::AUBO_REQUEST_IGNORE,
-            [&robot_interface]() { return waitArrival(robot_interface); });
+            [&robot_interface, motion_state, token = motion.token]() {
+                return waitArrival(
+                    robot_interface,
+                    [motion_state, token]() {
+                        return motion_state->cancelled(token);
+                    });
+            });
         switch (outcome) {
         case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion:
             CMVR_LOG(DEBUG) << "[AuboArm] moveJ completed without motion: sdk ret="
@@ -650,7 +826,14 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o
                             << arcs::common_interface::returnValue2Str(ret) << ")";
             return Result::success();
         case aubo_internal::MotionCommandOutcome::CompletedAfterMotion:
+            motion_owner.clearOnFinish();
+            motion_owner.settle();
             return Result::success();
+        case aubo_internal::MotionCommandOutcome::Cancelled:
+            motion_owner.settle();
+            return Result::failure(
+                ArmErrorCode::CommandRejected,
+                "[AuboArm] moveJ stopped by stopMotion");
         case aubo_internal::MotionCommandOutcome::SubmitFailed:
             return Result::failure(
                 ArmErrorCode::CommandFailed,
@@ -671,18 +854,30 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration
     if (!validDof_(velocity.velocity.size(), error)) {
         return Result::failure(ArmErrorCode::InvalidDof, error);
     }
-    const auto ready = ensureConnected_("speedJ");
-    if (!ready.ok()) {
-        return ready;
-    }
-    if (busy_.exchange(true)) {
-        return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_);
-    }
-    BusyGuard busy_guard{busy_};
-
     try {
+        std::unique_lock submit_lock(mutex_);
+        const auto locked_ready = ensureConnected_("speedJ");
+        if (!locked_ready.ok()) {
+            return locked_ready;
+        }
+        const auto rpc_client = sdk_->rpc_client;
+        const auto motion_state = sdk_->motion_state;
+        const auto motion = motion_state->begin(
+            aubo_internal::MotionKind::Joint, true);
+        if (!motion.started()) {
+            return Result::failure(
+                ArmErrorCode::RobotNotReady,
+                "[AuboArm] speedJ rejected: " +
+                    std::string(motionStartFailure(motion.status)) +
+                    ", id=" + id_);
+        }
+        busy_.store(true);
+        MotionOwnerGuard motion_owner{
+            *motion_state, busy_, motion.token};
+
         Result interface_result;
-        auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "speedJ", interface_result);
+        auto robot_interface = getPrimaryRobotInterface(
+            rpc_client, "speedJ", interface_result);
         if (!interface_result.ok()) {
             return interface_result;
         }
@@ -690,14 +885,31 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration
         robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_);
         const double resolved_acceleration = acceleration > 0.0 ? acceleration : 1.5;
         const double resolved_duration = duration > 0.0 ? duration : 100.0;
+        motion_owner.requireExplicitSettlement();
+        submit_lock.unlock();
+        if (motion_state->cancelled(motion.token)) {
+            motion_owner.settle();
+            return Result::failure(
+                ArmErrorCode::CommandRejected,
+                "[AuboArm] speedJ stopped by stopMotion before submission");
+        }
         const int ret = robot_interface->getMotionControl()->speedJoint(
             velocity.velocity,
             resolved_acceleration,
             resolved_duration);
+        if (motion_state->cancelled(motion.token)) {
+            motion_owner.settle();
+            return Result::failure(
+                ArmErrorCode::CommandRejected,
+                "[AuboArm] speedJ stopped by stopMotion");
+        }
         if (ret != 0) {
+            motion_owner.settle();
             return Result::failure(ArmErrorCode::CommandFailed,
                                    "[AuboArm] speedJ failed: ret=" + std::to_string(ret));
         }
+        motion_owner.retainKind();
+        motion_owner.settle();
         return Result::success();
     } catch (const std::exception& e) {
         return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] speedJ failed: ") + e.what());
@@ -706,46 +918,38 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration
 
 Result AuboArm::stopJ(double acceleration)
 {
-    if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
+    return stopMotion_(MotionStopKind::Joint, acceleration);
-        return Result::success();
-    }
-    try {
-        Result interface_result;
-        auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "stopJ", interface_result);
-        if (!interface_result.ok()) {
-            return interface_result;
-        }
-        const double resolved_acceleration = acceleration > 0.0 ? acceleration : 31.0;
-        const int ret = robot_interface->getMotionControl()->stopJoint(resolved_acceleration);
-        busy_.store(false);
-        if (ret != 0) {
-            return Result::failure(ArmErrorCode::CommandFailed,
-                                   "[AuboArm] stopJ failed: ret=" + std::to_string(ret));
-        }
-        return Result::success();
-    } catch (const std::exception& e) {
-        return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopJ failed: ") + e.what());
-    }
 }
 
 Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame)
 {
     (void)frame;
-    const auto ready = ensureConnected_("moveL");
-    if (!ready.ok()) {
-        return ready;
-    }
-    if (busy_.exchange(true)) {
-        return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_);
-    }
-    BusyGuard busy_guard{busy_};
-
     try {
-        const auto robot_names = sdk_->rpc_client->getRobotNames();
+        std::unique_lock submit_lock(mutex_);
+        const auto locked_ready = ensureConnected_("moveL");
+        if (!locked_ready.ok()) {
+            return locked_ready;
+        }
+        const auto rpc_client = sdk_->rpc_client;
+        const auto motion_state = sdk_->motion_state;
+        const auto motion = motion_state->begin(
+            aubo_internal::MotionKind::Linear);
+        if (!motion.started()) {
+            return Result::failure(
+                ArmErrorCode::RobotNotReady,
+                "[AuboArm] moveL rejected: " +
+                    std::string(motionStartFailure(motion.status)) +
+                    ", id=" + id_);
+        }
+        busy_.store(true);
+        MotionOwnerGuard motion_owner{
+            *motion_state, busy_, motion.token};
+
+        const auto robot_names = rpc_client->getRobotNames();
         if (robot_names.empty()) {
             return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
         }
-        auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
+        auto robot_interface = rpc_client->getRobotInterface(robot_names.front());
         if (!robot_interface) {
             return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
         }
@@ -754,17 +958,29 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options,
         std::vector<double> tcp_offset(6, 0.0);
         robot_interface->getRobotConfig()->setTcpOffset(tcp_offset);
         std::vector<double> pose{target.x, target.y, target.z, target.rx, target.ry, target.rz};
+        motion_owner.requireExplicitSettlement();
         const int ret = motion_control->moveLine(
             pose,
             options.acceleration > 0.0 ? options.acceleration : 0.5,
             options.velocity > 0.0 ? options.velocity : 0.25,
             options.blend_radius,
             0);
+        if (ret == arcs::common_interface::AUBO_OK) {
+            submit_lock.unlock();
+        } else {
+            motion_owner.settle();
+        }
         const auto outcome = aubo_internal::resolveMotionCommand(
             ret,
             arcs::common_interface::AUBO_OK,
             arcs::common_interface::AUBO_REQUEST_IGNORE,
-            [&robot_interface]() { return waitArrival(robot_interface); });
+            [&robot_interface, motion_state, token = motion.token]() {
+                return waitArrival(
+                    robot_interface,
+                    [motion_state, token]() {
+                        return motion_state->cancelled(token);
+                    });
+            });
         switch (outcome) {
         case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion:
             CMVR_LOG(DEBUG) << "[AuboArm] moveL completed without motion: sdk ret="
@@ -772,7 +988,14 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options,
                             << arcs::common_interface::returnValue2Str(ret) << ")";
             return Result::success();
         case aubo_internal::MotionCommandOutcome::CompletedAfterMotion:
+            motion_owner.clearOnFinish();
+            motion_owner.settle();
             return Result::success();
+        case aubo_internal::MotionCommandOutcome::Cancelled:
+            motion_owner.settle();
+            return Result::failure(
+                ArmErrorCode::CommandRejected,
+                "[AuboArm] moveL stopped by stopMotion");
         case aubo_internal::MotionCommandOutcome::SubmitFailed:
             return Result::failure(
                 ArmErrorCode::CommandFailed,
@@ -789,18 +1012,30 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options,
 
 Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame)
 {
-    const auto ready = ensureConnected_("speedL");
-    if (!ready.ok()) {
-        return ready;
-    }
-    if (busy_.exchange(true)) {
-        return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_);
-    }
-    BusyGuard busy_guard{busy_};
-
     try {
+        std::unique_lock submit_lock(mutex_);
+        const auto locked_ready = ensureConnected_("speedL");
+        if (!locked_ready.ok()) {
+            return locked_ready;
+        }
+        const auto rpc_client = sdk_->rpc_client;
+        const auto motion_state = sdk_->motion_state;
+        const auto motion = motion_state->begin(
+            aubo_internal::MotionKind::Linear, true);
+        if (!motion.started()) {
+            return Result::failure(
+                ArmErrorCode::RobotNotReady,
+                "[AuboArm] speedL rejected: " +
+                    std::string(motionStartFailure(motion.status)) +
+                    ", id=" + id_);
+        }
+        busy_.store(true);
+        MotionOwnerGuard motion_owner{
+            *motion_state, busy_, motion.token};
+
         Result interface_result;
-        auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "speedL", interface_result);
+        auto robot_interface = getPrimaryRobotInterface(
+            rpc_client, "speedL", interface_result);
         if (!interface_result.ok()) {
             return interface_result;
         }
@@ -820,8 +1055,10 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
             tool_frame[0] = 0.0;
             tool_frame[1] = 0.0;
             tool_frame[2] = 0.0;
-            line_speed = sdk_->rpc_client->getMath()->poseTrans(tool_frame, line_speed);
+            line_speed = rpc_client->getMath()->poseTrans(
-            angular_speed = sdk_->rpc_client->getMath()->poseTrans(tool_frame, angular_speed);
+                tool_frame, line_speed);
+            angular_speed = rpc_client->getMath()->poseTrans(
+                tool_frame, angular_speed);
         } else if (frame == FrameType::User) {
             return Result::failure(ArmErrorCode::UnsupportedCommand,
                                    "[AuboArm] speedL User frame requires a configured user coordinate frame");
@@ -838,14 +1075,31 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
 
         const double resolved_acceleration = acceleration > 0.0 ? acceleration : 1.2;
         const double resolved_duration = duration > 0.0 ? duration : 100.0;
+        motion_owner.requireExplicitSettlement();
+        submit_lock.unlock();
+        if (motion_state->cancelled(motion.token)) {
+            motion_owner.settle();
+            return Result::failure(
+                ArmErrorCode::CommandRejected,
+                "[AuboArm] speedL stopped by stopMotion before submission");
+        }
         const int ret = robot_interface->getMotionControl()->speedLine(
             speed,
             resolved_acceleration,
             resolved_duration);
+        if (motion_state->cancelled(motion.token)) {
+            motion_owner.settle();
+            return Result::failure(
+                ArmErrorCode::CommandRejected,
+                "[AuboArm] speedL stopped by stopMotion");
+        }
         if (ret != 0) {
+            motion_owner.settle();
             return Result::failure(ArmErrorCode::CommandFailed,
                                    "[AuboArm] speedL failed: ret=" + std::to_string(ret));
         }
+        motion_owner.retainKind();
+        motion_owner.settle();
         return Result::success();
     } catch (const std::exception& e) {
         return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] speedL failed: ") + e.what());
@@ -854,44 +1108,165 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
 
 Result AuboArm::stopL(std::optional<double> acceleration)
 {
-    if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
+    return stopMotion_(
-        return Result::success();
+        MotionStopKind::Linear,
-    }
+        acceleration.has_value() ? *acceleration : 0.0);
-    try {
-        Result interface_result;
-        auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "stopL", interface_result);
-        if (!interface_result.ok()) {
-            return interface_result;
-        }
-        const double resolved_acceleration =
-            acceleration.has_value() && *acceleration > 0.0 ? *acceleration : 10.0;
-        const int ret = robot_interface->getMotionControl()->stopLine(resolved_acceleration, resolved_acceleration);
-        busy_.store(false);
-        if (ret != 0) {
-            return Result::failure(ArmErrorCode::CommandFailed,
-                                   "[AuboArm] stopL failed: ret=" + std::to_string(ret));
-        }
-        return Result::success();
-    } catch (const std::exception& e) {
-        return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopL failed: ") + e.what());
-    }
 }
 
 Result AuboArm::stopMotion()
 {
+    return stopMotion_(MotionStopKind::Automatic, 0.0);
+}
+
+Result AuboArm::stopMotion_(
+    const MotionStopKind requested_kind,
+    const double acceleration)
+{
+    if (!connected_.load()) {
+        return Result::success();
+    }
+
+    try {
+        std::unique_lock submit_lock(mutex_);
         if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
             return Result::success();
         }
-    try {
+
-        const auto robot_names = sdk_->rpc_client->getRobotNames();
+        const auto motion_state = sdk_->motion_state;
-        if (robot_names.empty()) {
+        aubo_internal::MotionKind forced_kind =
+            aubo_internal::MotionKind::None;
+        if (requested_kind == MotionStopKind::Joint) {
+            forced_kind = aubo_internal::MotionKind::Joint;
+        } else if (requested_kind == MotionStopKind::Linear) {
+            forced_kind = aubo_internal::MotionKind::Linear;
+        }
+
+        const auto stop_request =
+            motion_state->beginStop(forced_kind);
+        if (!stop_request.started()) {
+            return Result::failure(
+                ArmErrorCode::RobotNotReady,
+                "[AuboArm] stopMotion rejected: another stop operation is in progress");
+        }
+        busy_.store(true);
+        StopStateGuard stop_state_guard{
+            *motion_state, busy_};
+
+        Result interface_result;
+        auto robot_interface = getPrimaryRobotInterface(
+            sdk_->rpc_client, "stopMotion", interface_result);
+        if (!interface_result.ok()) {
+            return interface_result;
+        }
+
+        auto motion_control = robot_interface->getMotionControl();
+        auto robot_state = robot_interface->getRobotState();
+        int last_exec_id = motion_control->getExecId();
+        bool last_steady = robot_state->isSteady();
+        const bool requires_vendor_stop =
+            stop_request.tracked_motion ||
+            last_exec_id != -1 ||
+            !last_steady;
+
+        if (requires_vendor_stop &&
+            stop_request.kind == aubo_internal::MotionKind::None) {
+            return Result::failure(
+                ArmErrorCode::CommandFailed,
+                "[AuboArm] stopMotion failed: controller is moving but the "
+                "active direct-motion type is unknown");
+        }
+
+        const auto issue_vendor_stop = [&]() -> Result {
+            int ret = 0;
+            if (stop_request.kind == aubo_internal::MotionKind::Joint) {
+                const double resolved_acceleration =
+                    acceleration > 0.0 ? acceleration : 31.0;
+                ret = motion_control->stopJoint(resolved_acceleration);
+            } else {
+                const double resolved_acceleration =
+                    acceleration > 0.0 ? acceleration : 10.0;
+                ret = motion_control->stopLine(
+                    resolved_acceleration, resolved_acceleration);
+            }
+            if (ret != arcs::common_interface::AUBO_OK) {
+                return Result::failure(
+                    ArmErrorCode::CommandFailed,
+                    "[AuboArm] stopMotion failed: " +
+                        std::string(motionKindName(stop_request.kind)) +
+                        " stop sdk ret=" + std::to_string(ret) +
+                        " (" +
+                        arcs::common_interface::returnValue2Str(ret) +
+                        ")");
+            }
             return Result::success();
+        };
+
+        if (requires_vendor_stop) {
+            const auto stop_result = issue_vendor_stop();
+            if (!stop_result.ok()) {
+                return stop_result;
             }
-        auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
-        if (robot_interface) {
-            robot_interface->getMotionControl()->stopMove(true, true);
         }
-        busy_.store(false);
+
+        constexpr auto kStopTimeout = std::chrono::seconds(5);
+        constexpr auto kPollInterval = std::chrono::milliseconds(50);
+        constexpr int kStableSamples = 3;
+        const auto deadline =
+            std::chrono::steady_clock::now() + kStopTimeout;
+        int stable_samples = 0;
+        bool idle_since_stop = last_exec_id == -1 && last_steady;
+        bool owner_active =
+            motion_state->ownerActive(stop_request.active_token);
+        while (std::chrono::steady_clock::now() < deadline) {
+            last_exec_id = motion_control->getExecId();
+            last_steady = robot_state->isSteady();
+            owner_active =
+                motion_state->ownerActive(stop_request.active_token);
+            const bool physically_idle =
+                last_exec_id == -1 && last_steady;
+            if (!physically_idle && idle_since_stop) {
+                if (stop_request.kind ==
+                    aubo_internal::MotionKind::None) {
+                    return Result::failure(
+                        ArmErrorCode::CommandFailed,
+                        "[AuboArm] stopMotion failed: motion started "
+                        "after an idle observation but its type is unknown");
+                }
+                const auto stop_result = issue_vendor_stop();
+                if (!stop_result.ok()) {
+                    return stop_result;
+                }
+                idle_since_stop = false;
+            }
+            if (physically_idle && !owner_active) {
+                if (++stable_samples >= kStableSamples) {
+                    break;
+                }
+            } else {
+                stable_samples = 0;
+            }
+            if (physically_idle) {
+                idle_since_stop = true;
+            }
+            std::this_thread::sleep_for(kPollInterval);
+        }
+        if (stable_samples < kStableSamples) {
+            return Result::failure(
+                ArmErrorCode::Timeout,
+                "[AuboArm] stopMotion failed: timeout waiting for " +
+                    std::string(motionKindName(stop_request.kind)) +
+                    " motion to stop, generation=" +
+                    std::to_string(stop_request.active_token.generation) +
+                    ", exec_id=" + std::to_string(last_exec_id) +
+                    ", steady=" + (last_steady ? "true" : "false") +
+                    ", owner_active=" +
+                    (owner_active ? "true" : "false"));
+        }
+        if (!stop_state_guard.complete()) {
+            return Result::failure(
+                ArmErrorCode::CommandFailed,
+                "[AuboArm] stopMotion failed: cancelled motion handler is still active");
+        }
         return Result::success();
     } catch (const std::exception& e) {
         return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopMotion failed: ") + e.what());
@@ -1226,7 +1601,6 @@ Result AuboArm::stopProgram()
     }
     try {
         const int ret = sdk_->rpc_client->getRuntimeMachine()->abort();
-        busy_.store(false);
         if (ret != 0) {
             return Result::failure(ArmErrorCode::CommandFailed,
                                    "[AuboArm] stopProgram failed: ret=" + std::to_string(ret));

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

@@ -80,12 +80,19 @@ public:
     CartesianPose fk(const std::string& base_link, const std::string& ee_link) override;
     CartesianPose fk(bool is_tcp = true) override;
     CartesianVelocity getSpeedLCommandTwistBase() const override { return {}; }
-    bool busy() const override { return busy_.load(); }
+    bool busy() const override;
 
 private:
+    enum class MotionStopKind {
+        Automatic,
+        Joint,
+        Linear,
+    };
+
     Result unsupported_(const std::string& name) const;
     bool validDof_(std::size_t size, std::string& error) const;
     Result ensureConnected_(const std::string& context) const;
+    Result stopMotion_(MotionStopKind kind, double acceleration);
 
     struct SdkState;
 

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

@@ -6,10 +6,17 @@ namespace cmvr::device::aubo_internal {
 enum class MotionCommandOutcome {
     CompletedWithoutMotion,
     CompletedAfterMotion,
+    Cancelled,
     SubmitFailed,
     CompletionFailed,
 };
 
+enum class MotionWaitResult {
+    Completed,
+    Cancelled,
+    Failed,
+};
+
 template <typename WaitForCompletion>
 MotionCommandOutcome resolveMotionCommand(
     const int return_code,
@@ -23,7 +30,11 @@ MotionCommandOutcome resolveMotionCommand(
     if (return_code != success_code) {
         return MotionCommandOutcome::SubmitFailed;
     }
-    if (wait_for_completion() != 0) {
+    const auto wait_result = wait_for_completion();
+    if (wait_result == MotionWaitResult::Cancelled) {
+        return MotionCommandOutcome::Cancelled;
+    }
+    if (wait_result != MotionWaitResult::Completed) {
         return MotionCommandOutcome::CompletionFailed;
     }
     return MotionCommandOutcome::CompletedAfterMotion;

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

@@ -0,0 +1,265 @@
+#ifndef CMVR_ES_AUBO_MOTION_STATE_H
+#define CMVR_ES_AUBO_MOTION_STATE_H
+
+#include <algorithm>
+#include <chrono>
+#include <condition_variable>
+#include <cstdint>
+#include <mutex>
+
+namespace cmvr::device::aubo_internal {
+
+enum class MotionKind {
+    None,
+    Joint,
+    Linear,
+};
+
+struct MotionToken {
+    std::uint64_t generation{0};
+    MotionKind kind{MotionKind::None};
+
+    bool valid() const noexcept
+    {
+        return generation != 0 && kind != MotionKind::None;
+    }
+};
+
+enum class MotionStartStatus {
+    Started,
+    Invalid,
+    Busy,
+    Stopping,
+    Blocked,
+};
+
+struct MotionStartResult {
+    MotionStartStatus status{MotionStartStatus::Busy};
+    MotionToken token;
+
+    bool started() const noexcept
+    {
+        return status == MotionStartStatus::Started;
+    }
+};
+
+enum class MotionFinishMode {
+    RestorePrevious,
+    Clear,
+    Retain,
+};
+
+enum class StopStartStatus {
+    Started,
+    AlreadyStopping,
+};
+
+struct StopRequest {
+    StopStartStatus status{StopStartStatus::AlreadyStopping};
+    MotionKind kind{MotionKind::None};
+    MotionToken active_token;
+    bool tracked_motion{false};
+
+    bool started() const noexcept
+    {
+        return status == StopStartStatus::Started;
+    }
+};
+
+// Tracks one direct AUBO motion owner. MoveJ/MoveL submissions are serialized
+// through the vendor call. Speed calls release the outer mutex before their
+// potentially blocking SDK call, so the generation cancellation below also
+// closes the stop-vs-speed-submission race.
+class MotionState final {
+public:
+    MotionStartResult begin(
+        const MotionKind kind,
+        const bool replace_retained_same_kind = false)
+    {
+        std::lock_guard lock(mutex_);
+        if (kind == MotionKind::None) {
+            return {MotionStartStatus::Invalid, {}};
+        }
+        if (stop_in_progress_) {
+            return {MotionStartStatus::Stopping, {}};
+        }
+        if (blocked_) {
+            return {MotionStartStatus::Blocked, {}};
+        }
+        if (owner_active_) {
+            return {MotionStartStatus::Busy, {}};
+        }
+        if (last_kind_ != MotionKind::None &&
+            (!replace_retained_same_kind || last_kind_ != kind)) {
+            return {MotionStartStatus::Busy, {}};
+        }
+
+        MotionToken token{++next_generation_, kind};
+        owner_active_ = true;
+        active_token_ = token;
+        previous_kind_ = last_kind_;
+        return {MotionStartStatus::Started, token};
+    }
+
+    void finish(
+        const MotionToken& token,
+        const MotionFinishMode mode = MotionFinishMode::RestorePrevious)
+    {
+        std::lock_guard lock(mutex_);
+        if (!owner_active_ ||
+            active_token_.generation != token.generation) {
+            return;
+        }
+
+        owner_active_ = false;
+        active_token_ = {};
+        if (!stop_in_progress_ && !blocked_) {
+            if (mode == MotionFinishMode::Retain) {
+                last_kind_ = token.kind;
+            } else if (mode == MotionFinishMode::Clear) {
+                last_kind_ = MotionKind::None;
+            } else {
+                last_kind_ = previous_kind_;
+            }
+        }
+        previous_kind_ = MotionKind::None;
+        owner_finished_cv_.notify_all();
+    }
+
+    void failMotion(const MotionToken& token)
+    {
+        std::lock_guard lock(mutex_);
+        if (!owner_active_ ||
+            active_token_.generation != token.generation) {
+            return;
+        }
+        owner_active_ = false;
+        active_token_ = {};
+        last_kind_ = token.kind;
+        previous_kind_ = MotionKind::None;
+        blocked_ = true;
+        owner_finished_cv_.notify_all();
+    }
+
+    StopRequest beginStop(
+        const MotionKind requested_kind = MotionKind::None)
+    {
+        std::lock_guard lock(mutex_);
+        if (stop_in_progress_) {
+            return {};
+        }
+
+        stop_in_progress_ = true;
+        const MotionToken active = owner_active_
+            ? active_token_
+            : MotionToken{};
+        // A successful speedJoint/speedLine call may keep the controller in
+        // velocity mode after the SDK function returns, even when the target
+        // velocity is zero and the robot currently reports steady. Retain that
+        // motion kind until a typed stop has been acknowledged.
+        const bool tracked_motion =
+            active.valid() || last_kind_ != MotionKind::None;
+        if (active.valid()) {
+            cancelled_generation_ = std::max(
+                cancelled_generation_, active.generation);
+        }
+        MotionKind kind = MotionKind::None;
+        if (active.valid()) {
+            kind = active.kind;
+        } else if (last_kind_ != MotionKind::None) {
+            kind = last_kind_;
+        } else if (requested_kind != MotionKind::None) {
+            kind = requested_kind;
+        } else {
+            kind = last_kind_;
+        }
+        if (kind != MotionKind::None) {
+            last_kind_ = kind;
+        }
+        return {
+            StopStartStatus::Started,
+            kind,
+            active,
+            tracked_motion};
+    }
+
+    bool cancelled(const MotionToken& token) const
+    {
+        std::lock_guard lock(mutex_);
+        return token.valid() &&
+               token.generation <= cancelled_generation_;
+    }
+
+    bool waitForOwnerExit(
+        const MotionToken& token,
+        const std::chrono::milliseconds timeout)
+    {
+        if (!token.valid()) {
+            return true;
+        }
+        std::unique_lock lock(mutex_);
+        return owner_finished_cv_.wait_for(
+            lock,
+            timeout,
+            [this, &token]() {
+                return !owner_active_ ||
+                       active_token_.generation != token.generation;
+            });
+    }
+
+    bool ownerActive(const MotionToken& token) const
+    {
+        if (!token.valid()) {
+            return false;
+        }
+        std::lock_guard lock(mutex_);
+        return owner_active_ &&
+               active_token_.generation == token.generation;
+    }
+
+    bool completeStop()
+    {
+        std::lock_guard lock(mutex_);
+        if (owner_active_) {
+            return false;
+        }
+        stop_in_progress_ = false;
+        blocked_ = false;
+        active_token_ = {};
+        last_kind_ = MotionKind::None;
+        previous_kind_ = MotionKind::None;
+        owner_finished_cv_.notify_all();
+        return true;
+    }
+
+    void failStop()
+    {
+        std::lock_guard lock(mutex_);
+        stop_in_progress_ = false;
+        blocked_ = true;
+        owner_finished_cv_.notify_all();
+    }
+
+    bool busy() const
+    {
+        std::lock_guard lock(mutex_);
+        return owner_active_ || stop_in_progress_ || blocked_ ||
+               last_kind_ != MotionKind::None;
+    }
+
+private:
+    mutable std::mutex mutex_;
+    std::condition_variable owner_finished_cv_;
+    std::uint64_t next_generation_{0};
+    std::uint64_t cancelled_generation_{0};
+    MotionToken active_token_;
+    MotionKind last_kind_{MotionKind::None};
+    MotionKind previous_kind_{MotionKind::None};
+    bool owner_active_{false};
+    bool stop_in_progress_{false};
+    bool blocked_{false};
+};
+
+} // namespace cmvr::device::aubo_internal
+
+#endif // CMVR_ES_AUBO_MOTION_STATE_H

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

@@ -18,6 +18,7 @@ namespace {
 int main()
 {
     using cmvr::device::aubo_internal::MotionCommandOutcome;
+    using cmvr::device::aubo_internal::MotionWaitResult;
     using cmvr::device::aubo_internal::resolveMotionCommand;
 
     constexpr int success_code = 0;
@@ -26,7 +27,7 @@ int main()
     int wait_calls = 0;
     const auto wait_succeeded = [&wait_calls]() {
         ++wait_calls;
-        return 0;
+        return MotionWaitResult::Completed;
     };
     CHECK_TRUE(resolveMotionCommand(
                    success_code,
@@ -57,7 +58,7 @@ int main()
     wait_calls = 0;
     const auto wait_failed = [&wait_calls]() {
         ++wait_calls;
-        return -1;
+        return MotionWaitResult::Failed;
     };
     CHECK_TRUE(resolveMotionCommand(
                    success_code,
@@ -66,5 +67,17 @@ int main()
                    wait_failed) == MotionCommandOutcome::CompletionFailed);
     CHECK_TRUE(wait_calls == 1);
 
+    wait_calls = 0;
+    const auto wait_cancelled = [&wait_calls]() {
+        ++wait_calls;
+        return MotionWaitResult::Cancelled;
+    };
+    CHECK_TRUE(resolveMotionCommand(
+                   success_code,
+                   success_code,
+                   request_ignore_code,
+                   wait_cancelled) == MotionCommandOutcome::Cancelled);
+    CHECK_TRUE(wait_calls == 1);
+
     return 0;
 }

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

@@ -0,0 +1,127 @@
+#include "devices/arm/aubo_arm/aubo_motion_state.h"
+
+#include <chrono>
+#include <iostream>
+
+namespace {
+
+#define CHECK_TRUE(condition)                                                        \
+    do {                                                                             \
+        if (!(condition)) {                                                          \
+            std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": "           \
+                      << #condition << std::endl;                                     \
+            return 1;                                                                \
+        }                                                                            \
+    } while (false)
+
+} // namespace
+
+int main()
+{
+    using namespace cmvr::device::aubo_internal;
+
+    MotionState state;
+    CHECK_TRUE(state.begin(MotionKind::None).status ==
+               MotionStartStatus::Invalid);
+    const auto joint = state.begin(MotionKind::Joint);
+    CHECK_TRUE(joint.started());
+    CHECK_TRUE(state.busy());
+    CHECK_TRUE(state.begin(MotionKind::Linear).status ==
+               MotionStartStatus::Busy);
+
+    const auto stop_joint = state.beginStop();
+    CHECK_TRUE(stop_joint.started());
+    CHECK_TRUE(stop_joint.kind == MotionKind::Joint);
+    CHECK_TRUE(stop_joint.active_token.generation ==
+               joint.token.generation);
+    CHECK_TRUE(stop_joint.tracked_motion);
+    CHECK_TRUE(state.cancelled(joint.token));
+    CHECK_TRUE(state.beginStop().status ==
+               StopStartStatus::AlreadyStopping);
+    CHECK_TRUE(state.begin(MotionKind::Linear).status ==
+               MotionStartStatus::Stopping);
+    CHECK_TRUE(!state.waitForOwnerExit(
+        joint.token, std::chrono::milliseconds(1)));
+    CHECK_TRUE(!state.completeStop());
+
+    state.finish(joint.token);
+    CHECK_TRUE(state.waitForOwnerExit(
+        joint.token, std::chrono::milliseconds(1)));
+    CHECK_TRUE(state.completeStop());
+    CHECK_TRUE(!state.busy());
+
+    const auto linear = state.begin(MotionKind::Linear);
+    CHECK_TRUE(linear.started());
+    CHECK_TRUE(!state.cancelled(linear.token));
+    // A delayed guard from the cancelled command must not release a newer one.
+    state.finish(joint.token);
+    CHECK_TRUE(state.busy());
+    state.finish(linear.token, MotionFinishMode::Clear);
+    CHECK_TRUE(!state.busy());
+
+    const auto speed_joint = state.begin(MotionKind::Joint);
+    CHECK_TRUE(speed_joint.started());
+    state.finish(speed_joint.token, MotionFinishMode::Retain);
+    CHECK_TRUE(state.busy());
+    CHECK_TRUE(state.begin(MotionKind::Linear).status ==
+               MotionStartStatus::Busy);
+    const auto rejected_speed_update =
+        state.begin(MotionKind::Joint, true);
+    CHECK_TRUE(rejected_speed_update.started());
+    state.finish(
+        rejected_speed_update.token,
+        MotionFinishMode::RestorePrevious);
+    const auto stop_speed = state.beginStop();
+    CHECK_TRUE(stop_speed.started());
+    CHECK_TRUE(stop_speed.kind == MotionKind::Joint);
+    CHECK_TRUE(stop_speed.tracked_motion);
+    CHECK_TRUE(state.completeStop());
+    CHECK_TRUE(!state.busy());
+
+    const auto idle_stop = state.beginStop();
+    CHECK_TRUE(idle_stop.kind == MotionKind::None);
+    CHECK_TRUE(!idle_stop.tracked_motion);
+    state.failStop();
+    const auto retry_idle_stop = state.beginStop();
+    CHECK_TRUE(retry_idle_stop.kind == MotionKind::None);
+    CHECK_TRUE(!retry_idle_stop.tracked_motion);
+    CHECK_TRUE(state.completeStop());
+
+    const auto mismatched_stop_motion = state.begin(MotionKind::Linear);
+    CHECK_TRUE(mismatched_stop_motion.started());
+    const auto mismatched_stop = state.beginStop(MotionKind::Joint);
+    CHECK_TRUE(mismatched_stop.kind == MotionKind::Linear);
+    state.finish(mismatched_stop_motion.token);
+    CHECK_TRUE(state.completeStop());
+
+    const auto uncertain_motion = state.begin(MotionKind::Joint);
+    CHECK_TRUE(uncertain_motion.started());
+    state.failMotion(uncertain_motion.token);
+    CHECK_TRUE(state.busy());
+    CHECK_TRUE(state.begin(MotionKind::Linear).status ==
+               MotionStartStatus::Blocked);
+    const auto stop_uncertain = state.beginStop();
+    CHECK_TRUE(stop_uncertain.kind == MotionKind::Joint);
+    CHECK_TRUE(stop_uncertain.tracked_motion);
+    CHECK_TRUE(state.completeStop());
+
+    const auto failed_stop_motion = state.begin(MotionKind::Linear);
+    CHECK_TRUE(failed_stop_motion.started());
+    const auto failed_stop = state.beginStop();
+    CHECK_TRUE(failed_stop.kind == MotionKind::Linear);
+    state.failStop();
+    CHECK_TRUE(state.busy());
+    CHECK_TRUE(state.begin(MotionKind::Joint).status ==
+               MotionStartStatus::Blocked);
+    state.finish(failed_stop_motion.token);
+
+    const auto retry = state.beginStop();
+    CHECK_TRUE(retry.started());
+    CHECK_TRUE(retry.kind == MotionKind::Linear);
+    CHECK_TRUE(state.completeStop());
+    const auto recovered = state.begin(MotionKind::Joint);
+    CHECK_TRUE(recovered.started());
+    state.finish(recovered.token, MotionFinishMode::Clear);
+
+    return 0;
+}

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

@@ -41,6 +41,14 @@ public:
         const std::string& resource_id,
         const std::string& owner_id,
         Duration ttl);
+
+    // Atomically invalidates a normal control lease and joins a safety
+    // barrier. Each safety caller receives an independent token; normal
+    // control remains blocked until the last safety token is released.
+    ControlAcquireResult preemptAcquire(
+        const std::string& resource_id,
+        const std::string& owner_id,
+        Duration ttl);
     bool renew(const ControlLeaseToken& token, Duration ttl);
     bool validate(const ControlLeaseToken& token);
     void release(const ControlLeaseToken& token) noexcept;
@@ -60,6 +68,8 @@ private:
         std::string owner_id;
         std::uint64_t generation{0};
         std::chrono::steady_clock::time_point deadline;
+        bool preemptible{true};
+        std::unordered_map<std::uint64_t, std::string> safety_holders;
     };
 
     bool expired_(const Entry& entry) const noexcept;

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

@@ -42,7 +42,50 @@ ControlAcquireResult ControlAuthorityManager::tryAcquire(
         Entry{
             owner_id,
             token.generation,
-            std::chrono::steady_clock::now() + ttl});
+            std::chrono::steady_clock::now() + ttl,
+            true,
+            {}});
+    return {true, std::move(token), {}};
+}
+
+ControlAcquireResult ControlAuthorityManager::preemptAcquire(
+    const std::string& resource_id,
+    const std::string& owner_id,
+    const Duration ttl)
+{
+    if (resource_id.empty() || owner_id.empty() ||
+        ttl <= Duration::zero()) {
+        return {false, {}, "invalid control barrier request"};
+    }
+
+    std::lock_guard lock(mutex_);
+    const auto existing = entries_.find(resource_id);
+    if (existing != entries_.end()) {
+        if (!expired_(existing->second) &&
+            !existing->second.preemptible) {
+            ControlLeaseToken token;
+            token.resource_id = resource_id;
+            token.owner_id = owner_id;
+            token.generation = ++next_generation_;
+            existing->second.safety_holders.emplace(
+                token.generation, token.owner_id);
+            return {true, std::move(token), {}};
+        }
+        entries_.erase(existing);
+    }
+
+    ControlLeaseToken token;
+    token.resource_id = resource_id;
+    token.owner_id = owner_id;
+    token.generation = ++next_generation_;
+    entries_.emplace(
+        resource_id,
+        Entry{
+            owner_id,
+            token.generation,
+            std::chrono::steady_clock::time_point::max(),
+            false,
+            {{token.generation, owner_id}}});
     return {true, std::move(token), {}};
 }
 
@@ -55,15 +98,22 @@ bool ControlAuthorityManager::renew(
     }
     std::lock_guard lock(mutex_);
     const auto found = entries_.find(token.resource_id);
-    if (found == entries_.end() ||
+    if (found == entries_.end() || expired_(found->second)) {
-        expired_(found->second) ||
-        found->second.owner_id != token.owner_id ||
-        found->second.generation != token.generation) {
         if (found != entries_.end() && expired_(found->second)) {
             entries_.erase(found);
         }
         return false;
     }
+    if (!found->second.preemptible) {
+        const auto holder =
+            found->second.safety_holders.find(token.generation);
+        return holder != found->second.safety_holders.end() &&
+               holder->second == token.owner_id;
+    }
+    if (found->second.owner_id != token.owner_id ||
+        found->second.generation != token.generation) {
+        return false;
+    }
     found->second.deadline =
         std::chrono::steady_clock::now() + ttl;
     return true;
@@ -84,6 +134,12 @@ bool ControlAuthorityManager::validate(
         entries_.erase(found);
         return false;
     }
+    if (!found->second.preemptible) {
+        const auto holder =
+            found->second.safety_holders.find(token.generation);
+        return holder != found->second.safety_holders.end() &&
+               holder->second == token.owner_id;
+    }
     return found->second.owner_id == token.owner_id &&
            found->second.generation == token.generation;
 }
@@ -97,8 +153,21 @@ void ControlAuthorityManager::release(
     try {
         std::lock_guard lock(mutex_);
         const auto found = entries_.find(token.resource_id);
-        if (found != entries_.end() &&
+        if (found == entries_.end()) {
-            found->second.owner_id == token.owner_id &&
+            return;
+        }
+        if (!found->second.preemptible) {
+            const auto holder =
+                found->second.safety_holders.find(token.generation);
+            if (holder == found->second.safety_holders.end() ||
+                holder->second != token.owner_id) {
+                return;
+            }
+            found->second.safety_holders.erase(holder);
+            if (found->second.safety_holders.empty()) {
+                entries_.erase(found);
+            }
+        } else if (found->second.owner_id == token.owner_id &&
                    found->second.generation == token.generation) {
             entries_.erase(found);
         }

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

@@ -61,6 +61,38 @@ TEST_F(ControlAuthorityManagerTest, StaleGenerationCannotReleaseNewLease)
     EXPECT_TRUE(manager.validate(current.token));
 }
 
+TEST_F(ControlAuthorityManagerTest,
+       SafetyBarrierAtomicallyPreemptsControlAndRejectsOtherOwners)
+{
+    auto& manager = ControlAuthorityManager::instance();
+    const auto control =
+        manager.tryAcquire("right_arm", "move-session", 100ms);
+    ASSERT_TRUE(control.acquired);
+
+    const auto barrier = manager.preemptAcquire(
+        "right_arm", "stop-operation", 100ms);
+    ASSERT_TRUE(barrier.acquired) << barrier.detail;
+    EXPECT_FALSE(manager.validate(control.token));
+    EXPECT_TRUE(manager.validate(barrier.token));
+
+    const auto move_during_stop =
+        manager.tryAcquire("right_arm", "new-move", 100ms);
+    EXPECT_FALSE(move_during_stop.acquired);
+    const auto second_stop = manager.preemptAcquire(
+        "right_arm", "second-stop", 100ms);
+    ASSERT_TRUE(second_stop.acquired) << second_stop.detail;
+
+    manager.release(control.token);
+    EXPECT_TRUE(manager.validate(barrier.token));
+    manager.release(barrier.token);
+    EXPECT_TRUE(manager.validate(second_stop.token));
+    EXPECT_FALSE(
+        manager.tryAcquire("right_arm", "new-move", 100ms)
+            .acquired);
+    manager.release(second_stop.token);
+    EXPECT_FALSE(manager.isLeased("right_arm"));
+}
+
 TEST_F(ControlAuthorityManagerTest, ExpiryAndRenewUseMonotonicLocalTime)
 {
     auto& manager = ControlAuthorityManager::instance();

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

@@ -126,11 +126,16 @@ grpc::Status setDeviceNotFound(Response* response, const std::string& device_id)
 
 grpc::Status setControlLeaseConflict(
     api::CommandHeader_Feedback* response,
-    const std::string& device_id)
+    const std::string& device_id,
+    const std::string& detail)
 {
-    const std::string message =
+    std::string message =
         "RobotArm control is leased by another active control operation: " +
         device_id;
+    if (!detail.empty()) {
+        CMVR_LOG(WARNING) << "[gRPCArmServiceImpl] control lease conflict, id="
+                          << device_id << ", detail=" << detail;
+    }
     fillFeedback(response, false, message);
     return grpc::Status(
         grpc::StatusCode::FAILED_PRECONDITION, message);
@@ -139,17 +144,19 @@ grpc::Status setControlLeaseConflict(
 template <typename Response>
 grpc::Status setControlLeaseConflict(
     Response* response,
-    const std::string& device_id)
+    const std::string& device_id,
+    const std::string& detail)
 {
     return setControlLeaseConflict(
-        response->mutable_header(), device_id);
+        response->mutable_header(), device_id, detail);
 }
 
 class ScopedUnaryControlLease final {
 public:
     ScopedUnaryControlLease(
         const std::string& device_id,
-        const char* operation)
+        const char* operation,
+        const bool preemptive = false)
         : manager_(control::ControlAuthorityManager::instance())
     {
         static std::atomic<std::uint64_t> sequence{0};
@@ -159,14 +166,15 @@ public:
                 sequence.fetch_add(
                     1U, std::memory_order_relaxed) +
                 1U);
-        auto acquired = manager_.tryAcquire(
+        const auto ttl = std::chrono::duration_cast<
-            device_id,
-            owner,
-            std::chrono::duration_cast<
             control::ControlAuthorityManager::Duration>(
-                std::chrono::hours(24)));
+                std::chrono::hours(24));
+        auto acquired = preemptive
+            ? manager_.preemptAcquire(device_id, owner, ttl)
+            : manager_.tryAcquire(device_id, owner, ttl);
         acquired_ = acquired.acquired;
         token_ = std::move(acquired.token);
+        detail_ = std::move(acquired.detail);
     }
 
     ~ScopedUnaryControlLease()
@@ -175,10 +183,12 @@ public:
     }
 
     bool acquired() const noexcept { return acquired_; }
+    const std::string& detail() const noexcept { return detail_; }
 
 private:
     control::ControlAuthorityManager& manager_;
     control::ControlLeaseToken token_;
+    std::string detail_;
     bool acquired_{false};
 };
 
@@ -199,10 +209,12 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        // A safety-disable command preempts any network teleoperation lease.
+        ScopedUnaryControlLease control_barrier(
-        // The teleoperation executor must fail its next renew before it can
+            device_id, "torqueOff", true);
-        // dispatch another setpoint.
+        if (!control_barrier.acquired()) {
-        control::ControlAuthorityManager::instance().revoke(device_id);
+            return setControlLeaseConflict(
+                response, device_id, control_barrier.detail());
+        }
         const auto result = arm->torqueOff();
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
         if (result.ok()) {
@@ -228,7 +240,8 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*,
         ScopedUnaryControlLease control_lease(
             device_id, "torqueOn");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(response, device_id);
+            return setControlLeaseConflict(
+                response, device_id, control_lease.detail());
         }
         const auto result = arm->torqueOn();
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
@@ -255,7 +268,8 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*,
         ScopedUnaryControlLease control_lease(
             device_id, "moveJ");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(response, device_id);
+            return setControlLeaseConflict(
+                response, device_id, control_lease.detail());
         }
         const auto result = arm->moveJ(toJointPositionCommand(request->target()),
                                        toMotionOptions(request->options()));
@@ -283,7 +297,8 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*,
         ScopedUnaryControlLease control_lease(
             device_id, "moveL");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(response, device_id);
+            return setControlLeaseConflict(
+                response, device_id, control_lease.detail());
         }
         const auto result = arm->moveL(toCartesianPose(request->target()),
                                        toMotionOptions(request->options()),
@@ -312,7 +327,8 @@ grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*,
         ScopedUnaryControlLease control_lease(
             device_id, "speedJ");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(response, device_id);
+            return setControlLeaseConflict(
+                response, device_id, control_lease.detail());
         }
         const auto result = arm->speedJ(toJointVelocityCommand(request->velocity()),
                                         request->acceleration(),
@@ -343,7 +359,8 @@ grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*,
         ScopedUnaryControlLease control_lease(
             device_id, "speedL");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(response, device_id);
+            return setControlLeaseConflict(
+                response, device_id, control_lease.detail());
         }
         const auto result = arm->speedL(toCartesianVelocity(request->velocity()),
                                         request->acceleration(),
@@ -375,7 +392,8 @@ grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext*,
         ScopedUnaryControlLease control_lease(
             device_id, "servoJ");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(response, device_id);
+            return setControlLeaseConflict(
+                response, device_id, control_lease.detail());
         }
         const auto result = arm->servoJ(toJointPositionCommand(request->target()));
         if (result.ok()) {
@@ -399,7 +417,12 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        control::ControlAuthorityManager::instance().revoke(device_id);
+        ScopedUnaryControlLease control_barrier(
+            device_id, "stopMotion", true);
+        if (!control_barrier.acquired()) {
+            return setControlLeaseConflict(
+                response, device_id, control_barrier.detail());
+        }
         const auto result = arm->stopMotion();
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
         if (result.ok()) {
@@ -480,7 +503,8 @@ grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*,
         ScopedUnaryControlLease control_lease(
             device_id, "calibrateZeroQ");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(response, device_id);
+            return setControlLeaseConflict(
+                response, device_id, control_lease.detail());
         }
         const auto result = arm->calibrateZeroQ(request->joint_name());
         if (result.ok()) {
@@ -557,7 +581,8 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context,
         ScopedUnaryControlLease control_lease(
             device_id, "clearFault");
         if (!control_lease.acquired()) {
-            return setControlLeaseConflict(response, device_id);
+            return setControlLeaseConflict(
+                response, device_id, control_lease.detail());
         }
         const auto result = arm->clearFault();
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);

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

@@ -4,16 +4,65 @@
 
 #include "../include/grpc_system_service.h"
 
+#include <atomic>
 #include <chrono>
 #include <cstdint>
+#include <stdexcept>
+#include <string>
+#include <vector>
 
 #include "common/base/logging/logger.h"
+#include "manager/control_authority/include/control_authority_manager.h"
 
 using namespace cmvr::device;
 using namespace cmvr::service;
 
 namespace {
 
+class ScopedControlBarrierSet final {
+public:
+    ~ScopedControlBarrierSet()
+    {
+        auto& authority =
+            cmvr::control::ControlAuthorityManager::instance();
+        for (const auto& token : tokens_) {
+            authority.release(token);
+        }
+    }
+
+    bool acquire(const std::string& device_id, std::string& detail)
+    {
+        static std::atomic<std::uint64_t> sequence{0};
+        const std::string owner =
+            "grpc-system:stop-all:" +
+            std::to_string(
+                sequence.fetch_add(1U, std::memory_order_relaxed) + 1U);
+        const auto result =
+            cmvr::control::ControlAuthorityManager::instance()
+                .preemptAcquire(
+                    device_id,
+                    owner,
+                    std::chrono::duration_cast<
+                        cmvr::control::ControlAuthorityManager::Duration>(
+                            std::chrono::hours(24)));
+        if (!result.acquired) {
+            detail = result.detail;
+            return false;
+        }
+        try {
+            tokens_.push_back(result.token);
+        } catch (...) {
+            cmvr::control::ControlAuthorityManager::instance().release(
+                result.token);
+            throw;
+        }
+        return true;
+    }
+
+private:
+    std::vector<cmvr::control::ControlLeaseToken> tokens_;
+};
+
 std::uint64_t unixTimeMs() noexcept
 {
     const auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
@@ -239,6 +288,22 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context,
     const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response)
 {
     try {
+        const auto snapshot = dmgr_.snapshot();
+        ScopedControlBarrierSet control_barriers;
+        for (const auto& device : snapshot.devices) {
+            if (device.kind == cmvr::device::DeviceKind::Arm) {
+                std::string detail;
+                if (!control_barriers.acquire(device.id, detail)) {
+                    CMVR_LOG(WARNING)
+                        << "[gRPCSystemServiceImpl] (StopAll): failed to "
+                           "acquire arm safety barrier, id="
+                        << device.id << ", detail=" << detail;
+                    throw std::runtime_error(
+                        "StopAll could not acquire the RobotArm safety "
+                        "barrier: " + device.id);
+                }
+            }
+        }
         dmgr_.stop();
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());

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

@@ -1,6 +1,10 @@
 #include "service/grpc/include/grpc_arm_service.h"
 
+#include <chrono>
+#include <condition_variable>
+#include <future>
 #include <memory>
+#include <mutex>
 #include <optional>
 #include <string>
 #include <utility>
@@ -11,6 +15,7 @@
 #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"
 
 namespace cmvr::service {
@@ -57,7 +62,12 @@ public:
     }
 
     device::Result torqueOn() override { return device::Result::success(); }
-    device::Result torqueOff() override { return device::Result::success(); }
+    device::Result torqueOff() override
+    {
+        std::lock_guard lock(motion_mutex_);
+        ++torque_off_calls_;
+        return device::Result::success();
+    }
     device::Result calibrateZeroQ(const std::string&) override
     {
         return device::Result::success();
@@ -82,7 +92,7 @@ public:
     device::Result moveJ(const device::JointPositionCommand&,
                          const device::MotionOptions&) override
     {
-        return device::Result::success();
+        return enterMotion("moveJ", move_j_calls_);
     }
     device::Result speedJ(const device::JointVelocityCommand&,
                           double,
@@ -99,7 +109,7 @@ public:
         const device::MotionOptions&,
         device::FrameType = device::FrameType::Base) override
     {
-        return device::Result::success();
+        return enterMotion("moveL", move_l_calls_);
     }
     device::Result speedL(
         const device::CartesianVelocity&,
@@ -115,9 +125,101 @@ public:
     }
     device::Result stopMotion() override
     {
+        std::unique_lock lock(motion_mutex_);
+        ++stop_motion_calls_;
+        if (block_next_stop_) {
+            block_next_stop_ = false;
+            blocking_stop_started_ = true;
+            stop_started_cv_.notify_all();
+            stop_release_cv_.wait(
+                lock,
+                [this]() { return release_blocking_stop_; });
+        }
         return device::Result::success();
     }
 
+    void blockNextMotion()
+    {
+        std::lock_guard lock(motion_mutex_);
+        block_next_motion_ = true;
+        blocking_motion_started_ = false;
+        release_blocking_motion_ = false;
+        blocking_motion_name_.clear();
+    }
+
+    bool waitForBlockingMotion(
+        const std::string& operation,
+        const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(motion_mutex_);
+        return motion_started_cv_.wait_for(
+            lock,
+            timeout,
+            [this, &operation]() {
+                return blocking_motion_started_ &&
+                       blocking_motion_name_ == operation;
+            });
+    }
+
+    void releaseBlockingMotion()
+    {
+        {
+            std::lock_guard lock(motion_mutex_);
+            release_blocking_motion_ = true;
+        }
+        motion_release_cv_.notify_all();
+    }
+
+    void blockNextStopMotion()
+    {
+        std::lock_guard lock(motion_mutex_);
+        block_next_stop_ = true;
+        blocking_stop_started_ = false;
+        release_blocking_stop_ = false;
+    }
+
+    bool waitForBlockingStop(const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(motion_mutex_);
+        return stop_started_cv_.wait_for(
+            lock,
+            timeout,
+            [this]() { return blocking_stop_started_; });
+    }
+
+    void releaseBlockingStop()
+    {
+        {
+            std::lock_guard lock(motion_mutex_);
+            release_blocking_stop_ = true;
+        }
+        stop_release_cv_.notify_all();
+    }
+
+    int moveJCalls() const
+    {
+        std::lock_guard lock(motion_mutex_);
+        return move_j_calls_;
+    }
+
+    int moveLCalls() const
+    {
+        std::lock_guard lock(motion_mutex_);
+        return move_l_calls_;
+    }
+
+    int stopMotionCalls() const
+    {
+        std::lock_guard lock(motion_mutex_);
+        return stop_motion_calls_;
+    }
+
+    int torqueOffCalls() const
+    {
+        std::lock_guard lock(motion_mutex_);
+        return torque_off_calls_;
+    }
+
     device::Result startServoMode(const device::ServoOptions&) override
     {
         return device::Result::success();
@@ -217,6 +319,42 @@ public:
     bool next_success{true};
     std::string next_response_json;
     std::string last_request_json;
+
+private:
+    device::Result enterMotion(const char* operation, int& call_count)
+    {
+        std::unique_lock lock(motion_mutex_);
+        ++call_count;
+        if (!block_next_motion_) {
+            return device::Result::success();
+        }
+
+        block_next_motion_ = false;
+        blocking_motion_started_ = true;
+        blocking_motion_name_ = operation;
+        motion_started_cv_.notify_all();
+        motion_release_cv_.wait(
+            lock,
+            [this]() { return release_blocking_motion_; });
+        return device::Result::success();
+    }
+
+    mutable std::mutex motion_mutex_;
+    std::condition_variable motion_started_cv_;
+    std::condition_variable motion_release_cv_;
+    std::condition_variable stop_started_cv_;
+    std::condition_variable stop_release_cv_;
+    bool block_next_motion_{false};
+    bool blocking_motion_started_{false};
+    bool release_blocking_motion_{false};
+    bool block_next_stop_{false};
+    bool blocking_stop_started_{false};
+    bool release_blocking_stop_{false};
+    std::string blocking_motion_name_;
+    int move_j_calls_{0};
+    int move_l_calls_{0};
+    int stop_motion_calls_{0};
+    int torque_off_calls_{0};
 };
 
 class JsonCommandNonArmDevice final : public device::AbstractDevice {
@@ -243,6 +381,7 @@ class GrpcArmServiceTest : public ::testing::Test {
 protected:
     void SetUp() override
     {
+        control::ControlAuthorityManager::instance().clear();
         device::DeviceManager::destroyInstance();
         config::DeviceManagerConfig config;
         auto& manager = device::DeviceManager::getInstance(config);
@@ -263,6 +402,7 @@ protected:
         aubo_arm_.reset();
         left_arm_.reset();
         device::DeviceManager::destroyInstance();
+        control::ControlAuthorityManager::instance().clear();
     }
 
     grpc::Status execute(const std::string& device_id,
@@ -276,6 +416,60 @@ protected:
         return service_->ExecuteJsonCommand(&context, &request, &response);
     }
 
+    struct MoveOutcome {
+        grpc::Status status;
+        bool response_success{false};
+        std::string response_error;
+    };
+
+    MoveOutcome moveJ(const std::string& device_id)
+    {
+        api::MoveJ_Request request;
+        request.mutable_header()->set_device_id(device_id);
+        request.mutable_target()->add_position(0.1);
+        api::MoveJ_Response response;
+        grpc::ServerContext context;
+        auto status = service_->moveJ(&context, &request, &response);
+        return {
+            std::move(status),
+            response.header().success(),
+            response.header().error_message()};
+    }
+
+    MoveOutcome moveL(const std::string& device_id)
+    {
+        api::MoveL_Request request;
+        request.mutable_header()->set_device_id(device_id);
+        request.mutable_target()->set_x(0.1);
+        api::MoveL_Response response;
+        grpc::ServerContext context;
+        auto status = service_->moveL(&context, &request, &response);
+        return {
+            std::move(status),
+            response.header().success(),
+            response.header().error_message()};
+    }
+
+    grpc::Status stopMotion(
+        const std::string& device_id,
+        api::CommandHeader_Feedback& response)
+    {
+        api::CommandHeader_Request request;
+        request.set_device_id(device_id);
+        grpc::ServerContext context;
+        return service_->stopMotion(&context, &request, &response);
+    }
+
+    grpc::Status torqueOff(
+        const std::string& device_id,
+        api::CommandHeader_Feedback& response)
+    {
+        api::CommandHeader_Request request;
+        request.set_device_id(device_id);
+        grpc::ServerContext context;
+        return service_->torqueOff(&context, &request, &response);
+    }
+
     std::shared_ptr<JsonCommandRobotArm> left_arm_;
     std::shared_ptr<JsonCommandRobotArm> aubo_arm_;
     std::shared_ptr<JsonCommandNonArmDevice> non_arm_;
@@ -374,5 +568,125 @@ TEST_F(GrpcArmServiceTest,
     EXPECT_EQ(left_arm_->execute_calls, 0);
 }
 
+TEST_F(GrpcArmServiceTest,
+       StopMotionRevokesBlockedMoveJLeaseBeforeMoveLReturns)
+{
+    aubo_arm_->blockNextMotion();
+    auto blocked_move = std::async(
+        std::launch::async,
+        [this]() { return moveJ("aubo_arm"); });
+
+    const bool move_started = aubo_arm_->waitForBlockingMotion(
+        "moveJ", std::chrono::seconds(2));
+
+    MoveOutcome conflict;
+    MoveOutcome during_stop;
+    api::CommandHeader_Feedback torque_off_response;
+    grpc::Status torque_off_status;
+    api::CommandHeader_Feedback stop_response;
+    grpc::Status stop_status;
+    MoveOutcome resumed_move;
+    bool stop_started = false;
+    std::future<grpc::Status> blocked_stop;
+    if (move_started) {
+        conflict = moveL("aubo_arm");
+        aubo_arm_->blockNextStopMotion();
+        blocked_stop = std::async(
+            std::launch::async,
+            [this, &stop_response]() {
+                return stopMotion("aubo_arm", stop_response);
+            });
+        stop_started = aubo_arm_->waitForBlockingStop(
+            std::chrono::seconds(2));
+        if (stop_started) {
+            torque_off_status = torqueOff(
+                "aubo_arm", torque_off_response);
+            during_stop = moveL("aubo_arm");
+        }
+        aubo_arm_->releaseBlockingStop();
+        stop_status = blocked_stop.get();
+        resumed_move = moveL("aubo_arm");
+    }
+
+    // Keep the original RPC active until after the replacement MoveL has
+    // attempted to acquire control. This models a driver whose stopped motion
+    // takes time to unwind and guards the lease hand-off itself.
+    aubo_arm_->releaseBlockingMotion();
+    const auto original_move = blocked_move.get();
+
+    ASSERT_TRUE(move_started);
+    ASSERT_TRUE(stop_started);
+    EXPECT_EQ(conflict.status.error_code(),
+              grpc::StatusCode::FAILED_PRECONDITION);
+    EXPECT_EQ(conflict.response_error, conflict.status.error_message());
+    EXPECT_EQ(during_stop.status.error_code(),
+              grpc::StatusCode::FAILED_PRECONDITION);
+    EXPECT_EQ(during_stop.response_error,
+              during_stop.status.error_message());
+    EXPECT_TRUE(torque_off_status.ok())
+        << torque_off_status.error_message();
+    EXPECT_TRUE(torque_off_response.success())
+        << torque_off_response.error_message();
+    EXPECT_TRUE(stop_status.ok()) << stop_status.error_message();
+    EXPECT_TRUE(stop_response.success())
+        << stop_response.error_message();
+    EXPECT_TRUE(resumed_move.status.ok())
+        << resumed_move.status.error_message();
+    EXPECT_TRUE(resumed_move.response_success)
+        << resumed_move.response_error;
+    EXPECT_TRUE(original_move.status.ok())
+        << original_move.status.error_message();
+    EXPECT_TRUE(original_move.response_success)
+        << 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);
+}
+
+TEST_F(GrpcArmServiceTest,
+       StopMotionRevokesBlockedMoveLLeaseBeforeMoveJReturns)
+{
+    aubo_arm_->blockNextMotion();
+    auto blocked_move = std::async(
+        std::launch::async,
+        [this]() { return moveL("aubo_arm"); });
+
+    const bool move_started = aubo_arm_->waitForBlockingMotion(
+        "moveL", std::chrono::seconds(2));
+
+    MoveOutcome conflict;
+    api::CommandHeader_Feedback stop_response;
+    grpc::Status stop_status;
+    MoveOutcome resumed_move;
+    if (move_started) {
+        conflict = moveJ("aubo_arm");
+        stop_status = stopMotion("aubo_arm", stop_response);
+        resumed_move = moveJ("aubo_arm");
+    }
+
+    aubo_arm_->releaseBlockingMotion();
+    const auto original_move = blocked_move.get();
+
+    ASSERT_TRUE(move_started);
+    EXPECT_EQ(conflict.status.error_code(),
+              grpc::StatusCode::FAILED_PRECONDITION);
+    EXPECT_EQ(conflict.response_error, conflict.status.error_message());
+    EXPECT_TRUE(stop_status.ok()) << stop_status.error_message();
+    EXPECT_TRUE(stop_response.success())
+        << stop_response.error_message();
+    EXPECT_TRUE(resumed_move.status.ok())
+        << resumed_move.status.error_message();
+    EXPECT_TRUE(resumed_move.response_success)
+        << resumed_move.response_error;
+    EXPECT_TRUE(original_move.status.ok())
+        << original_move.status.error_message();
+    EXPECT_TRUE(original_move.response_success)
+        << original_move.response_error;
+    EXPECT_EQ(aubo_arm_->moveJCalls(), 1);
+    EXPECT_EQ(aubo_arm_->moveLCalls(), 1);
+    EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1);
+}
+
 } // namespace
 } // namespace cmvr::service

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

@@ -2,8 +2,11 @@
 
 #include <array>
 #include <chrono>
+#include <condition_variable>
 #include <cstdint>
+#include <future>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <utility>
 #include <vector>
@@ -12,6 +15,7 @@
 #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"
 
 namespace cmvr::service {
@@ -36,11 +40,58 @@ public:
     {
         return health_;
     }
+    bool stop() override
+    {
+        std::unique_lock lock(stop_mutex_);
+        ++stop_calls_;
+        if (block_next_stop_) {
+            block_next_stop_ = false;
+            stop_started_ = true;
+            stop_started_cv_.notify_all();
+            stop_release_cv_.wait(
+                lock,
+                [this]() { return release_stop_; });
+        }
+        return true;
+    }
+    int stopCalls() const
+    {
+        std::lock_guard lock(stop_mutex_);
+        return stop_calls_;
+    }
+    void blockNextStop()
+    {
+        std::lock_guard lock(stop_mutex_);
+        block_next_stop_ = true;
+        stop_started_ = false;
+        release_stop_ = false;
+    }
+    bool waitForStop(const std::chrono::milliseconds timeout)
+    {
+        std::unique_lock lock(stop_mutex_);
+        return stop_started_cv_.wait_for(
+            lock, timeout, [this]() { return stop_started_; });
+    }
+    void releaseStop()
+    {
+        {
+            std::lock_guard lock(stop_mutex_);
+            release_stop_ = true;
+        }
+        stop_release_cv_.notify_all();
+    }
 
 private:
     device::DeviceKind kind_;
     std::string type_name_;
     device::DeviceHealthSnapshot health_;
+    mutable std::mutex stop_mutex_;
+    std::condition_variable stop_started_cv_;
+    std::condition_variable stop_release_cv_;
+    int stop_calls_{0};
+    bool block_next_stop_{false};
+    bool stop_started_{false};
+    bool release_stop_{false};
 };
 
 std::uint64_t currentUnixTimeMs()
@@ -66,6 +117,7 @@ class GrpcSystemServiceTest : public ::testing::Test {
 protected:
     void SetUp() override
     {
+        control::ControlAuthorityManager::instance().clear();
         device::DeviceManager::destroyInstance();
     }
 
@@ -74,6 +126,7 @@ protected:
         service_.reset();
         owned_devices_.clear();
         device::DeviceManager::destroyInstance();
+        control::ControlAuthorityManager::instance().clear();
     }
 
     api::GetDeviceListCommand_Feedback getDeviceList()
@@ -287,5 +340,74 @@ TEST_F(GrpcSystemServiceTest, MapsEveryKnownDeviceKind)
     }
 }
 
+TEST_F(GrpcSystemServiceTest,
+       StopAllStopsRegisteredDevicesAndRevokesOnlyArmLease)
+{
+    config::DeviceManagerConfig config;
+    auto& manager = device::DeviceManager::getInstance(config);
+    auto arm = std::make_shared<SnapshotDevice>(
+        "leased_arm", device::DeviceKind::Arm, "TestArm");
+    auto camera = std::make_shared<SnapshotDevice>(
+        "leased_camera", device::DeviceKind::Camera, "TestCamera");
+    auto already_stopping_arm = std::make_shared<SnapshotDevice>(
+        "already_stopping_arm", device::DeviceKind::Arm, "TestArm");
+    registerDevice(manager, arm);
+    registerDevice(manager, camera);
+    registerDevice(manager, already_stopping_arm);
+
+    auto& authority = control::ControlAuthorityManager::instance();
+    const auto arm_lease = authority.tryAcquire(
+        arm->id(), "arm-session", std::chrono::seconds(30));
+    const auto camera_lease = authority.tryAcquire(
+        camera->id(), "camera-session", std::chrono::seconds(30));
+    const auto existing_stop_barrier = authority.preemptAcquire(
+        already_stopping_arm->id(),
+        "existing-stop",
+        std::chrono::seconds(30));
+    ASSERT_TRUE(arm_lease.acquired) << arm_lease.detail;
+    ASSERT_TRUE(camera_lease.acquired) << camera_lease.detail;
+    ASSERT_TRUE(existing_stop_barrier.acquired)
+        << existing_stop_barrier.detail;
+    ASSERT_TRUE(authority.validate(arm_lease.token));
+    ASSERT_TRUE(authority.validate(camera_lease.token));
+
+    service_ = std::make_unique<gRPCSystemServiceImpl>();
+    arm->blockNextStop();
+    api::StopAllCommand_Request request;
+    api::StopAllCommand_Feedback response;
+    auto stop_all = std::async(
+        std::launch::async,
+        [this, &request, &response]() {
+            grpc::ServerContext context;
+            return service_->StopAll(&context, &request, &response);
+        });
+    const bool stop_started = arm->waitForStop(
+        std::chrono::seconds(2));
+    const auto move_during_stop = authority.tryAcquire(
+        arm->id(), "move-during-stop", std::chrono::seconds(30));
+    arm->releaseStop();
+    const auto status = stop_all.get();
+
+    ASSERT_TRUE(stop_started);
+    EXPECT_FALSE(move_during_stop.acquired);
+    ASSERT_TRUE(status.ok()) << status.error_message();
+    ASSERT_TRUE(response.header().success())
+        << response.header().error_message();
+    EXPECT_EQ(arm->stopCalls(), 1);
+    EXPECT_EQ(camera->stopCalls(), 1);
+    EXPECT_EQ(already_stopping_arm->stopCalls(), 1);
+    EXPECT_FALSE(authority.validate(arm_lease.token));
+    EXPECT_FALSE(authority.isLeased(arm->id()));
+    EXPECT_TRUE(authority.validate(camera_lease.token));
+    EXPECT_TRUE(authority.isLeased(camera->id()));
+    EXPECT_TRUE(authority.validate(existing_stop_barrier.token));
+    EXPECT_TRUE(authority.isLeased(already_stopping_arm->id()));
+    const auto move_after_stop = authority.tryAcquire(
+        arm->id(), "move-after-stop", std::chrono::seconds(30));
+    EXPECT_TRUE(move_after_stop.acquired) << move_after_stop.detail;
+    authority.release(move_after_stop.token);
+    authority.release(existing_stop_barrier.token);
+}
+
 } // namespace
 } // namespace cmvr::service