fix aubo hardware estop auto recovery

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_
+        if (software_emergency_stop_latched_) {
-            ? SafetyCondition::SoftwareEmergencyStop
+            latched_reason_ = SafetyCondition::SoftwareEmergencyStop;
-            : condition;
+        } else if (!preserve_hardware_estop_latch) {
+            latched_reason_ = condition;
+        }
     }
 
     std::optional<SafetyPermit> tryPermit() const

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