From 9c9b412d91719704633fa0e2001cb93412b6ce3b Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Wed, 19 Aug 2026 10:50:06 +0800 Subject: [PATCH] fix aubo system emergency stop recovery --- cmvr-es/devices/arm/aubo_arm/README.md | 8 ++++--- cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp | 21 ++++++++++--------- .../devices/arm/aubo_arm/aubo_safety_state.h | 19 ++++++++++++++--- ...evice_safety_control_plane_architecture.md | 7 ++++--- 4 files changed, 36 insertions(+), 19 deletions(-) diff --git a/cmvr-es/devices/arm/aubo_arm/README.md b/cmvr-es/devices/arm/aubo_arm/README.md index 0d8c752a..fe7e0c5e 100644 --- a/cmvr-es/devices/arm/aubo_arm/README.md +++ b/cmvr-es/devices/arm/aubo_arm/README.md @@ -106,9 +106,11 @@ cmake --install build `RobotModeType` 和硬件急停来源；首次有效样本前、监控断线或样本过期时， 所有 Move、Speed、Servo 和程序启动请求均按不安全状态拒绝； - 硬件急停会立即使当前运动 generation 失效，并在急停输入有效期间保持锁存。 - 检测到硬件急停输入消失且控制器重新报告 `Normal`/`ReducedMode` 后，后端应 - 自动执行 `poweron()` 和 `startup()`，恢复到 `Running` 后再完成安全确认并开放新的 - gRPC 控制指令；防护停机和 Safety Fault/Violation 仍保持显式恢复语义； + AUBO SDK 将示教器/控制柜急停报告为 `RobotEmergencyStop`，将控制器系统急停 + （外部系统急停输入）报告为 `SystemEmergencyStop`；两者在本后端都属于硬件急停。 + 检测到任一硬件急停输入消失且控制器重新报告 `Normal`/`ReducedMode` 后，后端自动 + 执行 `poweron()` 和 `startup()`，恢复到 `Running` 后再完成安全确认并开放新的 gRPC + 控制指令；防护停机和 Safety Fault/Violation 仍保持显式恢复语义； - `emergencyStop()` 使用独立的 `SoftwareEmergencyStop` 锁存。即使软件急停在真实 硬件急停有效期间触发，后续硬件采样也不能覆盖该锁存，释放硬件急停开关不会 自动清除软件急停；它只能通过显式安全恢复流程解除； diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index 78eca49a..3c5b52d0 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -470,7 +470,7 @@ void publishSafetySample( monitor->servo_mode_select.store(servo_mode_select); monitor->last_sample_ns.store(monotonicNowNs()); - if (emergency_stop_source != 0) { + if (aubo_internal::isHardwareEmergencyStop(condition)) { const bool first_sample_for_event = !monitor->hardware_emergency_stop_latched.exchange(true); if (first_sample_for_event) { @@ -875,8 +875,7 @@ bool hardwareEmergencyStopRecoveryCurrent( monitor->emergency_stop_source.load() == 0 && snapshot.latched && snapshot.recovery_in_progress && snapshot.epoch == token.epoch && !snapshot.software_emergency_stop_latched && - snapshot.latched_reason == - aubo_internal::SafetyCondition::RobotEmergencyStop && + aubo_internal::isHardwareEmergencyStop(snapshot.latched_reason) && aubo_internal::isMotionSafe(snapshot.observed); } @@ -3899,13 +3898,16 @@ Result AuboArm::ensureMotionReady_( " rejected: hardware safety state is unavailable or stale"); } - if (monitor->emergency_stop_source.load() != 0) { + const auto sampled_condition = + aubo_internal::effectiveSafetyCondition( + safetyConditionFromSdk(static_cast( + monitor->safety_mode.load())), + monitor->emergency_stop_source.load()); + if (aubo_internal::isHardwareEmergencyStop(sampled_condition)) { const auto previous = monitor->safety_state->snapshot(); - monitor->safety_state->observe( - aubo_internal::SafetyCondition::RobotEmergencyStop); + monitor->safety_state->observe(sampled_condition); if (!previous.latched || - previous.observed != - aubo_internal::SafetyCondition::RobotEmergencyStop) { + previous.observed != sampled_condition) { cancelForSafetyTransition(monitor); } } @@ -3935,8 +3937,7 @@ Result AuboArm::ensureMotionReady_( } std::string recovery_instruction = "; clear the hardware condition and perform explicit recovery"; - if (condition == - aubo_internal::SafetyCondition::RobotEmergencyStop && + if (aubo_internal::isHardwareEmergencyStop(condition) && monitor->auto_power_on_after_hardware_estop_release && !monitor->automatic_recovery_suppressed.load()) { recovery_instruction = diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h b/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h index c99a0c60..065453cc 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h @@ -29,10 +29,23 @@ inline bool isMotionSafe(const SafetyCondition condition) noexcept condition == SafetyCondition::Reduced; } +inline bool isHardwareEmergencyStop( + const SafetyCondition condition) noexcept +{ + return condition == SafetyCondition::SystemEmergencyStop || + condition == SafetyCondition::RobotEmergencyStop; +} + inline SafetyCondition effectiveSafetyCondition( const SafetyCondition reported_condition, const int robot_emergency_stop_source) noexcept { + // The source bitmask describes robot-side inputs such as the control box + // and teach pendant. Keep the SDK's SystemEmergencyStop classification for + // the separate external system-emergency input. + if (reported_condition == SafetyCondition::SystemEmergencyStop) { + return SafetyCondition::SystemEmergencyStop; + } if (robot_emergency_stop_source < 0) { return SafetyCondition::Unknown; } @@ -89,7 +102,7 @@ inline bool shouldAutoRecoverHardwareEmergencyStop( hardware_emergency_stop_was_observed && snapshot.latched && !snapshot.recovery_in_progress && !snapshot.software_emergency_stop_latched && - snapshot.latched_reason == SafetyCondition::RobotEmergencyStop && + isHardwareEmergencyStop(snapshot.latched_reason) && isMotionSafe(snapshot.observed) && current_emergency_stop_source == 0; } @@ -116,7 +129,7 @@ public: const bool preserve_hardware_estop_latch = condition == SafetyCondition::Unknown && latched_ && - latched_reason_ == SafetyCondition::RobotEmergencyStop && + isHardwareEmergencyStop(latched_reason_) && !software_emergency_stop_latched_; if (!latched_ || recovery_in_progress_ || (changed && !preserve_hardware_estop_latch)) { @@ -199,7 +212,7 @@ public: if (!token.valid() || token.epoch != epoch_ || !latched_ || !recovery_in_progress_ || software_emergency_stop_latched_ || - latched_reason_ != SafetyCondition::RobotEmergencyStop || + !isHardwareEmergencyStop(latched_reason_) || !isMotionSafe(observed_) || !robot_running || !controller_idle || !cancellation_confirmed) { return false; diff --git a/docs/device_safety_control_plane_architecture.md b/docs/device_safety_control_plane_architecture.md index 061c0800..4e4ac300 100644 --- a/docs/device_safety_control_plane_architecture.md +++ b/docs/device_safety_control_plane_architecture.md @@ -28,9 +28,10 @@ 认证或 TLS 模式会启动失败，不会静默回退。匿名部署的恢复默认关闭，只有显式配置 `RECOVERY_LOCAL_ONLY`、服务端确认实际 peer 为 loopback/Unix socket 且持久审计可写时才可开放。 -AUBO 另有一条设备内硬件语义：真实硬件急停曾有效、随后输入消失且控制器重新报告 -`Normal/ReducedMode` 时，驱动会自动上电到 `Idle`、清理旧队列、执行 `startup()`，并在 -`Running` 下再次确认 quiescent 后解除该硬件锁存，使新的 gRPC 指令可以重新准入； +AUBO 另有一条设备内硬件语义：示教器/控制柜 `RobotEmergencyStop` 或外部系统输入 +`SystemEmergencyStop` 曾有效、随后输入消失且控制器重新报告 `Normal/ReducedMode` 时， +驱动会自动上电到 `Idle`、清理旧队列、执行 `startup()`，并在 `Running` 下再次确认 +quiescent 后解除该硬件锁存，使新的 gRPC 指令可以重新准入； 软件 `emergencyStop()` 使用独立 `SoftwareEmergencyStop` 锁存，即使它与硬件急停重叠也绝不被 硬件输入释放自动清除。自动流程不 resume、不重放旧目标；显式 Stop/PowerOff 会取消本轮自动上电。