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 b11fb94d..c99a0c60 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h @@ -114,7 +114,12 @@ public: return; } - if (!latched_ || recovery_in_progress_ || changed) { + const bool preserve_hardware_estop_latch = + condition == SafetyCondition::Unknown && latched_ && + latched_reason_ == SafetyCondition::RobotEmergencyStop && + !software_emergency_stop_latched_; + if (!latched_ || recovery_in_progress_ || + (changed && !preserve_hardware_estop_latch)) { ++epoch_; } latched_ = true; @@ -122,9 +127,11 @@ public: // A physical E-stop sample can continue arriving after a software // E-stop request. Keep the software stop independently latched so a // later physical-input release can never clear it automatically. - latched_reason_ = software_emergency_stop_latched_ - ? SafetyCondition::SoftwareEmergencyStop - : condition; + if (software_emergency_stop_latched_) { + latched_reason_ = SafetyCondition::SoftwareEmergencyStop; + } else if (!preserve_hardware_estop_latch) { + latched_reason_ = condition; + } } std::optional tryPermit() const diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_safety_state_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_safety_state_test.cpp index 716d2b00..1d350990 100644 --- a/cmvr-es/devices/arm/aubo_arm/tests/aubo_safety_state_test.cpp +++ b/cmvr-es/devices/arm/aubo_arm/tests/aubo_safety_state_test.cpp @@ -81,6 +81,26 @@ int main() CHECK_TRUE(recovered_permit.has_value()); CHECK_TRUE(state.validate(*recovered_permit)); + // Some AUBO cabinets drop the SDK connection while the physical E-stop is + // pressed. Losing the monitor sample must not erase the hardware E-stop + // latch; otherwise releasing the switch back to Normal would never trigger + // automatic power-on. + state.observe(SafetyCondition::RobotEmergencyStop); + const auto estop_epoch = state.snapshot().epoch; + state.observe(SafetyCondition::Unknown); + CHECK_TRUE(state.snapshot().latched); + CHECK_TRUE(state.snapshot().latched_reason == + SafetyCondition::RobotEmergencyStop); + CHECK_TRUE(state.snapshot().epoch == estop_epoch); + state.observe(SafetyCondition::Normal); + CHECK_TRUE(shouldAutoRecoverHardwareEmergencyStop( + state.snapshot(), true, 0, true, false)); + const auto disconnected_recovery = state.beginRecovery( + state.snapshot().epoch); + CHECK_TRUE(disconnected_recovery.has_value()); + CHECK_TRUE(state.completeHardwareEmergencyStopRecovery( + *disconnected_recovery, true, true, true)); + // Releasing a real E-stop must never clear a software-triggered stop that // was latched while the hardware input was active. state.observe(SafetyCondition::RobotEmergencyStop);