fix(aubo): prevent resume after hardware e-stop

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

@@ -2,6 +2,8 @@ add_library(aubo_arm SHARED
         aubo_arm.cpp
 )
 
+find_package(Threads REQUIRED)
+
 target_include_directories(aubo_arm PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 set(AUBO_SDK_ROOT ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/aubo_sdk/v0.27.1)
@@ -47,6 +49,7 @@ target_link_libraries(aubo_arm
         PRIVATE
         glog
         jsoncpp
+        Threads::Threads
 )
 
 add_library(cmvr_es::device::aubo_arm ALIAS aubo_arm)
@@ -80,6 +83,19 @@ if(BUILD_TESTING)
     )
     set_tests_properties(aubo_motion_state_test PROPERTIES TIMEOUT 10)
 
+    add_executable(aubo_safety_state_test
+            tests/aubo_safety_state_test.cpp
+    )
+    target_include_directories(aubo_safety_state_test
+            PRIVATE
+            ${CMAKE_SOURCE_DIR}/cmvr-es
+    )
+    add_test(
+            NAME aubo_safety_state_test
+            COMMAND aubo_safety_state_test
+    )
+    set_tests_properties(aubo_safety_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/README.md

@@ -103,6 +103,19 @@ cmake --install build
 
 ## 安全与语义边界
 
+- 后端使用独立 SDK RPC 会话持续读取控制器的 `SafetyModeType`、
+  `RobotModeType` 和硬件急停来源；首次有效样本前、监控断线或样本过期时，
+  所有 Move、Speed、Servo 和程序启动请求均按不安全状态拒绝；
+- 硬件急停、防护停机、Safety Fault/Violation 会锁存安全事件，并使当前运动
+  generation 失效。控制器重新报告 `Normal`/`ReducedMode` 不会自动解除锁存；
+- 锁存后会终止直接运动与程序、关闭 servo 模式并清理控制器轨迹。只有确认
+  `ExecId == -1`、普通队列和轨迹队列均为空、运行时已停止且机械臂稳定后，
+  显式 `torqueOn`/`clearFault`/`unlockProtectiveStop` 才可能恢复运动权限；
+- 恢复流程不会调用 `resume`、`arbitraryResume`、`startMove`，也不会重新提交
+  急停前的目标、速度、servo 指令或程序；
+- AUBO SDK 未在本地文档中保证急停期间 `clearPath` 的可用性，也未说明释放
+  急停开关后的控制器恢复时序。因此本实现保持 fail-closed 并在释放后再次清队列，
+  但“释放开关后零位移”的最终保证仍需真机验证及控制器侧安全配置配合；
 - 只访问控制柜 Standard 数字 IO，不访问工具端 IO、可配置 IO 或安全 IO；
 - `set_do` 不修改输出 runstate；
 - 只有 `StandardOutputRunState::None` 的通道允许写入，否则返回
@@ -116,9 +129,12 @@ cmake --install build
 ## 测试
 
 ```bash
-cmake --build build --target aubo_arm_json_command_test -j4
+cmake --build build --target \
+  aubo_safety_state_test \
+  aubo_motion_state_test \
+  aubo_arm_json_command_test -j4
 ctest --test-dir build \
-  -R '^aubo_arm_json_command_test$' \
+  -R 'aubo_(safety_state|motion_state|arm_json_command)_test' \
   --output-on-failure
 ```
 

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

@@ -2,6 +2,7 @@
 #define CMVR_ES_AUBO_ARM_H
 
 #include <atomic>
+#include <cstdint>
 #include <memory>
 #include <mutex>
 #include <optional>
@@ -30,19 +31,19 @@ public:
     JointGroupState getJointState() const override;
     CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
     RobotMode getRobotMode() const override;
-    SafetyMode getSafetyMode() const override { return SafetyMode::Normal; }
+    SafetyMode getSafetyMode() const override;
-    ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; }
+    ControlMode getControlMode() const override;
 
     Result torqueOn() override;
     Result torqueOff() override;
     Result calibrateZeroQ(const std::string& joint_name) override;
     Result emergencyStop() override;
-    Result protectiveStop() override { return emergencyStop(); }
+    Result protectiveStop() override;
     Result setSpeedScaling(double scaling) override;
     double getSpeedScaling() const override { return speed_scaling_; }
-    bool isProtectiveStopped() const override { return false; }
+    bool isProtectiveStopped() const override;
-    bool isEmergencyStopped() const override { return emergency_stopped_; }
+    bool isEmergencyStopped() const override;
-    bool isFault() const override { return false; }
+    bool isFault() const override;
 
     Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override;
     Result speedJ(const JointVelocityCommand& velocity, double acceleration, double duration) override;
@@ -66,8 +67,8 @@ public:
     Result powerOff() override { return torqueOff(); }
     Result brakeRelease() override { return torqueOn(); }
     Result shutdown() override;
-    Result clearFault() override { return Result::success(); }
+    Result clearFault() override;
-    Result unlockProtectiveStop() override { return Result::success(); }
+    Result unlockProtectiveStop() override;
     Result loadProgram(const std::string& program_name) override;
     Result playProgram() override;
     Result pauseProgram() override;
@@ -92,6 +93,12 @@ private:
     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 ensureMotionReady_(const std::string& context,
+                              std::uint64_t& safety_epoch) const;
+    Result completeSafetyRecovery_(const std::string& context,
+                                   std::uint64_t expected_safety_epoch);
+    Result unlockProtectiveStop_(
+        std::optional<std::uint64_t> expected_safety_epoch);
     Result stopMotion_(MotionStopKind kind, double acceleration);
 
     struct SdkState;
@@ -109,7 +116,7 @@ private:
     std::atomic<bool> connected_{false};
     std::atomic<bool> busy_{false};
     std::atomic<bool> servo_mode_{false};
-    bool emergency_stopped_{false};
+    std::atomic<bool> emergency_stopped_{false};
     mutable std::mutex mutex_;
 
     std::unique_ptr<SdkState> sdk_;

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

@@ -66,6 +66,12 @@ struct StopRequest {
     }
 };
 
+struct SafetyCancelResult {
+    MotionKind kind{MotionKind::None};
+    MotionToken active_token;
+    bool tracked_motion{false};
+};
+
 // 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
@@ -183,6 +189,31 @@ public:
             tracked_motion};
     }
 
+    SafetyCancelResult cancelActiveForSafety()
+    {
+        std::lock_guard lock(mutex_);
+        const MotionToken active = owner_active_
+            ? active_token_
+            : MotionToken{};
+        if (active.valid()) {
+            cancelled_generation_ = std::max(
+                cancelled_generation_, active.generation);
+        }
+
+        const MotionKind kind = active.valid()
+            ? active.kind
+            : last_kind_;
+        if (kind != MotionKind::None) {
+            last_kind_ = kind;
+        }
+        // This block is intentionally independent of stop_in_progress_. The
+        // monitor may observe the safety event while a software Stop owns the
+        // stop transaction; either way no new motion may enter.
+        blocked_ = true;
+        owner_finished_cv_.notify_all();
+        return {kind, active, active.valid() || kind != MotionKind::None};
+    }
+
     bool cancelled(const MotionToken& token) const
     {
         std::lock_guard lock(mutex_);

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

@@ -0,0 +1,184 @@
+#ifndef CMVR_ES_AUBO_SAFETY_STATE_H
+#define CMVR_ES_AUBO_SAFETY_STATE_H
+
+#include <cstdint>
+#include <mutex>
+#include <optional>
+
+namespace cmvr::device::aubo_internal {
+
+// This is deliberately richer than RobotArm::SafetyMode. Recovery and
+// Violation have no lossless public mapping, but both must remain fail-closed.
+enum class SafetyCondition {
+    Unknown,
+    Normal,
+    Reduced,
+    Recovery,
+    Violation,
+    ProtectiveStop,
+    SafeguardStop,
+    SystemEmergencyStop,
+    RobotEmergencyStop,
+    Fault,
+};
+
+inline bool isMotionSafe(const SafetyCondition condition) noexcept
+{
+    return condition == SafetyCondition::Normal ||
+           condition == SafetyCondition::Reduced;
+}
+
+inline SafetyCondition effectiveSafetyCondition(
+    const SafetyCondition reported_condition,
+    const int robot_emergency_stop_source) noexcept
+{
+    if (robot_emergency_stop_source < 0) {
+        return SafetyCondition::Unknown;
+    }
+    if (robot_emergency_stop_source != 0) {
+        return SafetyCondition::RobotEmergencyStop;
+    }
+    return reported_condition;
+}
+
+inline bool needsProtectiveUnlock(
+    const SafetyCondition condition) noexcept
+{
+    return condition == SafetyCondition::ProtectiveStop ||
+           condition == SafetyCondition::Violation;
+}
+
+inline bool needsInterfaceBoardRestart(
+    const SafetyCondition condition) noexcept
+{
+    return condition == SafetyCondition::SystemEmergencyStop ||
+           condition == SafetyCondition::RobotEmergencyStop ||
+           condition == SafetyCondition::Fault;
+}
+
+struct SafetyPermit {
+    std::uint64_t epoch{0};
+
+    bool valid() const noexcept { return epoch != 0; }
+};
+
+struct RecoveryToken {
+    std::uint64_t epoch{0};
+
+    bool valid() const noexcept { return epoch != 0; }
+};
+
+struct SafetySnapshot {
+    SafetyCondition observed{SafetyCondition::Unknown};
+    SafetyCondition latched_reason{SafetyCondition::Unknown};
+    std::uint64_t epoch{0};
+    bool latched{false};
+    bool recovery_in_progress{false};
+};
+
+// Hardware safety is an event, not a level. Once an unsafe state has been
+// observed, returning to Normal only changes the observed level. A separate,
+// explicit recovery must prove that the old controller operation has been
+// cancelled before new motion permits can be issued.
+class SafetyState final {
+public:
+    SafetyState() = default;
+
+    void observe(const SafetyCondition condition)
+    {
+        std::lock_guard lock(mutex_);
+        const bool changed = observed_ != condition;
+        observed_ = condition;
+        if (isMotionSafe(condition)) {
+            return;
+        }
+
+        if (!latched_ || recovery_in_progress_ || changed) {
+            ++epoch_;
+        }
+        latched_ = true;
+        recovery_in_progress_ = false;
+        latched_reason_ = condition;
+    }
+
+    std::optional<SafetyPermit> tryPermit() const
+    {
+        std::lock_guard lock(mutex_);
+        if (latched_ || !isMotionSafe(observed_)) {
+            return std::nullopt;
+        }
+        return SafetyPermit{epoch_};
+    }
+
+    bool validate(const SafetyPermit permit) const
+    {
+        std::lock_guard lock(mutex_);
+        return permit.valid() && permit.epoch == epoch_ && !latched_ &&
+               isMotionSafe(observed_);
+    }
+
+    std::optional<RecoveryToken> beginRecovery(
+        const std::uint64_t expected_epoch)
+    {
+        std::lock_guard lock(mutex_);
+        if (expected_epoch == 0 || expected_epoch != epoch_ || !latched_ ||
+            recovery_in_progress_ ||
+            !isMotionSafe(observed_)) {
+            return std::nullopt;
+        }
+        recovery_in_progress_ = true;
+        return RecoveryToken{epoch_};
+    }
+
+    bool completeRecovery(
+        const RecoveryToken token,
+        const bool robot_running,
+        const bool controller_idle,
+        const bool cancellation_confirmed)
+    {
+        std::lock_guard lock(mutex_);
+        if (!token.valid() || token.epoch != epoch_ || !latched_ ||
+            !recovery_in_progress_ || !isMotionSafe(observed_) ||
+            !robot_running || !controller_idle ||
+            !cancellation_confirmed) {
+            return false;
+        }
+
+        latched_ = false;
+        recovery_in_progress_ = false;
+        latched_reason_ = SafetyCondition::Unknown;
+        ++epoch_;
+        return true;
+    }
+
+    void failRecovery(const RecoveryToken token)
+    {
+        std::lock_guard lock(mutex_);
+        if (token.valid() && token.epoch == epoch_) {
+            recovery_in_progress_ = false;
+        }
+    }
+
+    SafetySnapshot snapshot() const
+    {
+        std::lock_guard lock(mutex_);
+        return {
+            observed_,
+            latched_reason_,
+            epoch_,
+            latched_,
+            recovery_in_progress_};
+    }
+
+private:
+    mutable std::mutex mutex_;
+    SafetyCondition observed_{SafetyCondition::Unknown};
+    SafetyCondition latched_reason_{SafetyCondition::Unknown};
+    std::uint64_t epoch_{1};
+    bool latched_{false};
+    bool recovery_in_progress_{false};
+};
+
+} // namespace cmvr::device::aubo_internal
+
+#endif // CMVR_ES_AUBO_SAFETY_STATE_H

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

@@ -123,5 +123,34 @@ int main()
     CHECK_TRUE(recovered.started());
     state.finish(recovered.token, MotionFinishMode::Clear);
 
+    const auto safety_motion = state.begin(MotionKind::Linear);
+    CHECK_TRUE(safety_motion.started());
+    const auto safety_cancel = state.cancelActiveForSafety();
+    CHECK_TRUE(safety_cancel.kind == MotionKind::Linear);
+    CHECK_TRUE(safety_cancel.tracked_motion);
+    CHECK_TRUE(state.cancelled(safety_motion.token));
+    CHECK_TRUE(state.begin(MotionKind::Joint).status ==
+               MotionStartStatus::Blocked);
+    state.finish(safety_motion.token);
+    const auto safety_stop = state.beginStop();
+    CHECK_TRUE(safety_stop.started());
+    CHECK_TRUE(safety_stop.kind == MotionKind::Linear);
+    CHECK_TRUE(state.completeStop());
+
+    const auto retained_speed = state.begin(MotionKind::Joint);
+    CHECK_TRUE(retained_speed.started());
+    state.finish(retained_speed.token, MotionFinishMode::Retain);
+    const auto retained_cancel = state.cancelActiveForSafety();
+    CHECK_TRUE(retained_cancel.kind == MotionKind::Joint);
+    CHECK_TRUE(retained_cancel.tracked_motion);
+    CHECK_TRUE(!retained_cancel.active_token.valid());
+    CHECK_TRUE(state.begin(MotionKind::Linear).status ==
+               MotionStartStatus::Blocked);
+    const auto retained_stop = state.beginStop();
+    CHECK_TRUE(retained_stop.started());
+    CHECK_TRUE(retained_stop.kind == MotionKind::Joint);
+    CHECK_TRUE(retained_stop.tracked_motion);
+    CHECK_TRUE(state.completeStop());
+
     return 0;
 }

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

@@ -0,0 +1,88 @@
+#include "devices/arm/aubo_arm/aubo_safety_state.h"
+
+#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;
+
+    SafetyState state;
+    CHECK_TRUE(!state.tryPermit().has_value());
+
+    state.observe(SafetyCondition::Normal);
+    const auto initial_permit = state.tryPermit();
+    CHECK_TRUE(initial_permit.has_value());
+    CHECK_TRUE(state.validate(*initial_permit));
+
+    CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Normal, 1) ==
+               SafetyCondition::RobotEmergencyStop);
+    CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Normal, -1) ==
+               SafetyCondition::Unknown);
+    CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Reduced, 0) ==
+               SafetyCondition::Reduced);
+    CHECK_TRUE(needsProtectiveUnlock(SafetyCondition::ProtectiveStop));
+    CHECK_TRUE(needsProtectiveUnlock(SafetyCondition::Violation));
+    CHECK_TRUE(!needsProtectiveUnlock(SafetyCondition::SafeguardStop));
+    CHECK_TRUE(needsInterfaceBoardRestart(
+        SafetyCondition::RobotEmergencyStop));
+    CHECK_TRUE(needsInterfaceBoardRestart(
+        SafetyCondition::SystemEmergencyStop));
+    CHECK_TRUE(needsInterfaceBoardRestart(SafetyCondition::Fault));
+    CHECK_TRUE(!needsInterfaceBoardRestart(SafetyCondition::Recovery));
+
+    state.observe(SafetyCondition::RobotEmergencyStop);
+    CHECK_TRUE(!state.validate(*initial_permit));
+    CHECK_TRUE(state.snapshot().latched);
+    CHECK_TRUE(!state.beginRecovery(state.snapshot().epoch).has_value());
+
+    // Releasing the hardware switch must not unlock motion by itself.
+    state.observe(SafetyCondition::Normal);
+    CHECK_TRUE(state.snapshot().latched);
+    CHECK_TRUE(!state.tryPermit().has_value());
+
+    const auto recovery = state.beginRecovery(state.snapshot().epoch);
+    CHECK_TRUE(recovery.has_value());
+    CHECK_TRUE(!state.completeRecovery(*recovery, true, true, false));
+    state.failRecovery(*recovery);
+
+    const auto retry = state.beginRecovery(state.snapshot().epoch);
+    CHECK_TRUE(retry.has_value());
+    CHECK_TRUE(state.completeRecovery(*retry, true, true, true));
+    const auto recovered_permit = state.tryPermit();
+    CHECK_TRUE(recovered_permit.has_value());
+    CHECK_TRUE(state.validate(*recovered_permit));
+
+    // A new safety event invalidates an in-flight recovery token.
+    state.observe(SafetyCondition::ProtectiveStop);
+    state.observe(SafetyCondition::Reduced);
+    const auto stale_recovery = state.beginRecovery(
+        state.snapshot().epoch);
+    CHECK_TRUE(stale_recovery.has_value());
+    state.observe(SafetyCondition::SafeguardStop);
+    state.observe(SafetyCondition::Normal);
+    CHECK_TRUE(!state.completeRecovery(
+        *stale_recovery, true, true, true));
+    CHECK_TRUE(state.snapshot().latched);
+
+    // An old API call must not begin recovery for a newer safety event.
+    const auto stale_epoch = state.snapshot().epoch;
+    state.observe(SafetyCondition::RobotEmergencyStop);
+    state.observe(SafetyCondition::Normal);
+    CHECK_TRUE(!state.beginRecovery(stale_epoch).has_value());
+    CHECK_TRUE(!state.snapshot().recovery_in_progress);
+
+    return 0;
+}