From 8ac1bff0997067ca3ad3c99587aa3df0989e3a96 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Tue, 18 Aug 2026 15:03:17 +0800 Subject: [PATCH] add system operational state recovery --- .../src/device_safety_adapters.cpp | 127 +++++++- .../include/device_safety_endpoint.h | 11 + .../safety_manager/include/safety_manager.h | 8 +- .../safety_manager/src/safety_manager.cpp | 229 +++++++++++--- .../tests/safety_manager_test.cpp | 279 +++++++++++++++++- .../grpc/server/include/grpc_system_service.h | 1 + .../service/grpc/server/src/grpc_security.cpp | 3 + .../grpc/server/src/grpc_system_service.cpp | 160 ++++++++++ .../server/tests/grpc_system_service_test.cpp | 181 +++++++++++- ...evice_safety_control_plane_architecture.md | 24 +- protos/cmvr/api/safety_command.proto | 18 ++ protos/cmvr/api/system_service.proto | 1 + 12 files changed, 986 insertions(+), 56 deletions(-) diff --git a/cmvr-es/manager/device_manager/src/device_safety_adapters.cpp b/cmvr-es/manager/device_manager/src/device_safety_adapters.cpp index eb627d0f..9afdfa6c 100644 --- a/cmvr-es/manager/device_manager/src/device_safety_adapters.cpp +++ b/cmvr-es/manager/device_manager/src/device_safety_adapters.cpp @@ -55,6 +55,8 @@ using safety::TriState; using Probe = std::function; using Operation = std::function; +using RecoveryOperation = + std::function; TriState tri(const bool value) noexcept { @@ -66,6 +68,59 @@ bool agvStopResultAccepted(const AgvResult& result) noexcept return result.ok() || result.code == AgvErrorCode::UnsupportedCommand; } +bool armQuiescentForSafety(const ArmState& state) noexcept +{ + return state.protective_stopped || state.emergency_stopped || + (!state.moving && !state.program_running); +} + +SafetyReason armRecoveryReason(const ArmErrorCode code) noexcept +{ + switch (code) { + case ArmErrorCode::NotConnected: + case ArmErrorCode::ConnectionFailed: + return SafetyReason::DeviceDisconnected; + case ArmErrorCode::Timeout: + return SafetyReason::ParticipantTimeout; + case ArmErrorCode::RobotInEmergencyStop: + return SafetyReason::EmergencyStopActive; + case ArmErrorCode::RobotInProtectiveStop: + return SafetyReason::ProtectiveStopActive; + case ArmErrorCode::RobotInFault: + return SafetyReason::DeviceFault; + case ArmErrorCode::RobotNotReady: + case ArmErrorCode::RobotNotPowered: + return SafetyReason::DeviceNotReady; + case ArmErrorCode::UnsupportedCommand: + return SafetyReason::UnsupportedCommand; + case ArmErrorCode::OK: + return SafetyReason::None; + default: + return SafetyReason::DeviceFault; + } +} + +SafetyReason agvRecoveryReason(const AgvErrorCode code) noexcept +{ + switch (code) { + case AgvErrorCode::NotConnected: + case AgvErrorCode::ConnectionFailed: + return SafetyReason::DeviceDisconnected; + case AgvErrorCode::Timeout: + return SafetyReason::ParticipantTimeout; + case AgvErrorCode::EmergencyStopped: + return SafetyReason::EmergencyStopActive; + case AgvErrorCode::Fault: + return SafetyReason::DeviceFault; + case AgvErrorCode::UnsupportedCommand: + return SafetyReason::UnsupportedCommand; + case AgvErrorCode::OK: + return SafetyReason::None; + default: + return SafetyReason::DeviceFault; + } +} + SafetyBlocker blocker( const SafetyReason reason, const std::string& source) @@ -133,9 +188,13 @@ DeviceSafetyDescriptor defaultDescriptor(const AbstractDevice& device) class LegacyDeviceSafetyEndpoint final : public DeviceSafetyEndpoint { public: - LegacyDeviceSafetyEndpoint(DeviceSafetyDescriptor descriptor, Probe probe) + LegacyDeviceSafetyEndpoint( + DeviceSafetyDescriptor descriptor, + Probe probe, + RecoveryOperation operational_recovery) : descriptor_(std::move(descriptor)), probe_(std::move(probe)), + operational_recovery_(std::move(operational_recovery)), poll_interval_(std::clamp( descriptor_.maximum_snapshot_age / 2, std::chrono::milliseconds(50), @@ -272,6 +331,18 @@ public: return {true, SafetyReason::None, {}}; } + RecoveryCheckResult restoreOperationalState( + const RecoveryContext& context) override + { + if (!operational_recovery_) { + return { + false, + SafetyReason::UnsupportedCommand, + "device does not support operational recovery"}; + } + return operational_recovery_(context); + } + private: DeviceSafetySnapshot sample_() { @@ -334,6 +405,7 @@ private: DeviceSafetyDescriptor descriptor_; Probe probe_; + RecoveryOperation operational_recovery_; const std::chrono::milliseconds poll_interval_; std::atomic generation_{1}; std::atomic sequence_{0}; @@ -523,8 +595,9 @@ Probe probeFor(const std::shared_ptr& device) state.connected && state.powered_on && !state.fault && !state.emergency_stopped && !state.protective_stopped); - snapshot.quiescent = tri(!state.moving && !state.program_running); - snapshot.motion_active = tri(state.moving || state.program_running); + const bool quiescent = armQuiescentForSafety(state); + snapshot.quiescent = tri(quiescent); + snapshot.motion_active = tri(!quiescent); snapshot.actuator_enabled = tri(state.powered_on); snapshot.emergency_stop_active = tri(state.emergency_stopped); snapshot.protective_stop_active = tri(state.protective_stopped); @@ -571,7 +644,7 @@ Probe probeFor(const std::shared_ptr& device) snapshot.motion_active = tri(state.moving || active_navigation); snapshot.actuator_enabled = TriState::Unknown; snapshot.emergency_stop_active = tri(state.emergency_stopped); - snapshot.protective_stop_active = TriState::Unknown; + snapshot.protective_stop_active = TriState::False; snapshot.fault_active = tri(state.fault); snapshot.condition = !state.connected ? SafetyCondition::Unknown @@ -599,8 +672,6 @@ Probe probeFor(const std::shared_ptr& device) : TriState::Unknown; snapshot.motion_active = TriState::Unknown; snapshot.actuator_enabled = TriState::Unknown; - snapshot.emergency_stop_active = TriState::Unknown; - snapshot.protective_stop_active = TriState::Unknown; snapshot.fault_active = tri( state == AbstractDexHand::Status::FAULT); snapshot.condition = !connected @@ -715,8 +786,6 @@ Probe probeFor(const std::shared_ptr& device) snapshot.operational_ready = tri(!state.error); snapshot.quiescent = TriState::Unknown; snapshot.motion_active = TriState::Unknown; - snapshot.emergency_stop_active = TriState::Unknown; - snapshot.protective_stop_active = TriState::Unknown; snapshot.fault_active = tri(state.error); snapshot.condition = state.error ? SafetyCondition::Unsafe @@ -759,6 +828,42 @@ Probe probeFor(const std::shared_ptr& device) }; } +RecoveryOperation operationalRecoveryFor( + const std::shared_ptr& device) +{ + if (const auto arm = std::dynamic_pointer_cast(device)) { + return [arm](const RecoveryContext& context) { + if (SafetyClock::now() >= context.deadline) { + return RecoveryCheckResult{ + false, + SafetyReason::ParticipantTimeout, + "operational recovery deadline expired before clearFault"}; + } + const auto result = arm->clearFault(); + return RecoveryCheckResult{ + result.ok(), + armRecoveryReason(result.code), + result.ok() ? std::string{} : result.message}; + }; + } + if (const auto agv = std::dynamic_pointer_cast(device)) { + return [agv](const RecoveryContext& context) { + if (SafetyClock::now() >= context.deadline) { + return RecoveryCheckResult{ + false, + SafetyReason::ParticipantTimeout, + "operational recovery deadline expired before clearFault"}; + } + const auto result = agv->clearFault(); + return RecoveryCheckResult{ + result.ok(), + agvRecoveryReason(result.code), + result.ok() ? std::string{} : result.message}; + }; + } + return {}; +} + std::shared_ptr participantFor( const std::shared_ptr& device, const DeviceSafetyDescriptor& safety_descriptor, @@ -781,7 +886,7 @@ std::shared_ptr participantFor( }; verify = [arm] { const auto state = arm->getRobotState(); - const bool stopped = !state.moving && !state.program_running; + const bool stopped = armQuiescentForSafety(state); return ParticipantResult{ stopped, stopped ? SafetyReason::None @@ -998,7 +1103,9 @@ safety::DeviceSafetyRegistration makeDeviceSafetyRegistration( } else { registration.descriptor = defaultDescriptor(*device); registration.endpoint = std::make_shared( - registration.descriptor, probeFor(device)); + registration.descriptor, + probeFor(device), + operationalRecoveryFor(device)); } if (configured_maximum_age > std::chrono::milliseconds::zero()) { diff --git a/cmvr-es/manager/safety_manager/include/device_safety_endpoint.h b/cmvr-es/manager/safety_manager/include/device_safety_endpoint.h index 836ca6b8..c060b8b7 100644 --- a/cmvr-es/manager/safety_manager/include/device_safety_endpoint.h +++ b/cmvr-es/manager/safety_manager/include/device_safety_endpoint.h @@ -26,6 +26,17 @@ public: } virtual HardwareCheckResult validateBeforeDispatch( const AdmissionPermit& permit) = 0; + // Explicit operator-authorized recovery which may power or enable the + // device. RecoverSafetyState never calls this method; it is reserved for + // RestoreOperationalState transactions. + virtual RecoveryCheckResult restoreOperationalState( + const RecoveryContext&) + { + return { + false, + SafetyReason::UnsupportedCommand, + "device does not support operational recovery"}; + } virtual RecoveryCheckResult reconcileAdmissionState( const RecoveryContext& context) = 0; }; diff --git a/cmvr-es/manager/safety_manager/include/safety_manager.h b/cmvr-es/manager/safety_manager/include/safety_manager.h index 1ea4c982..20661939 100644 --- a/cmvr-es/manager/safety_manager/include/safety_manager.h +++ b/cmvr-es/manager/safety_manager/include/safety_manager.h @@ -118,11 +118,13 @@ struct RecoveryRequest { bool all_devices{false}; std::uint64_t expected_safety_epoch{0}; bool verify_only{true}; + bool restore_operational_state{false}; std::string reason; SafetyClock::time_point deadline{SafetyClock::time_point::max()}; - // Called only for a latch-clearing transaction, after hardware facts have - // been verified and before any software barrier is reconciled or released. - // A false result leaves admission latched. + // Authorizes the state-changing part of recovery. Operational restore + // calls this before invoking device recovery; software-only recovery calls + // it after hardware verification and before releasing admission barriers. + // A false result leaves admission latched and prevents device recovery. std::function authorize_clear; }; diff --git a/cmvr-es/manager/safety_manager/src/safety_manager.cpp b/cmvr-es/manager/safety_manager/src/safety_manager.cpp index bbb31eeb..10d3c42e 100644 --- a/cmvr-es/manager/safety_manager/src/safety_manager.cpp +++ b/cmvr-es/manager/safety_manager/src/safety_manager.cpp @@ -1759,7 +1759,10 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request) RecoveryResult invalid; invalid.recovery_id = request.recovery_id; if (request.recovery_id.empty() || request.reason.empty() || - (!request.all_devices && request.device_ids.empty())) { + (!request.all_devices && request.device_ids.empty()) || + (request.restore_operational_state && + (request.verify_only || !request.all_devices || + !request.authorize_clear))) { invalid.result = RecoveryResultCode::Failed; invalid.targets.push_back({ "system", @@ -1767,7 +1770,9 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request) request.reason.empty() ? SafetyReason::RecoveryReasonRequired : SafetyReason::InvalidArgument, - "recovery_id, reason, and an explicit non-empty scope are required"}); + request.restore_operational_state + ? "operational restore requires all_devices, latch clearing, and explicit authorization" + : "recovery_id, reason, and an explicit non-empty scope are required"}); return invalid; } std::unordered_set unique_ids; @@ -1787,7 +1792,9 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request) std::sort(fingerprint_ids.begin(), fingerprint_ids.end()); std::ostringstream fingerprint; fingerprint << request.expected_safety_epoch << ':' - << request.verify_only << ':' << request.all_devices << ':' + << request.verify_only << ':' + << request.restore_operational_state << ':' + << request.all_devices << ':' << request.reason; for (const auto& id : fingerprint_ids) { fingerprint << ':' << id; @@ -1912,7 +1919,9 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request) request.recovery_id, request.verify_only ? "recovery verification started" - : "software latch recovery started"); + : request.restore_operational_state + ? "operational state restoration started" + : "software latch recovery started"); } } @@ -1972,6 +1981,101 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request) result.current_safety_epoch, deadline, request.verify_only}; + bool clear_authorization_checked = false; + bool clear_authorized = true; + if (request.restore_operational_state && request.authorize_clear) { + clear_authorization_checked = true; + try { + clear_authorized = request.authorize_clear(); + } catch (...) { + clear_authorized = false; + } + } + if (request.restore_operational_state && !clear_authorized && + targets.empty()) { + all_verified = false; + result.targets.push_back({ + "system", + false, + SafetyReason::RecoveryAuditFailed, + "persistent recovery audit did not authorize operational restore"}); + } + + const auto validate_snapshot = []( + const SafetySnapshotView& view, + const SafetyPolicyFamily policy, + const bool require_operational_ready) { + if (!view.fresh) { + return RecoveryCheckResult{ + false, + SafetyReason::SafetyStateStale, + "refreshed safety sample is stale"}; + } + if (view.snapshot.condition == SafetyCondition::Unknown) { + return RecoveryCheckResult{ + false, + SafetyReason::SafetyStateMissing, + "hardware safety condition remains unknown"}; + } + if (view.snapshot.connected != TriState::True) { + return RecoveryCheckResult{ + false, + SafetyReason::DeviceDisconnected, + "device connection is not confirmed"}; + } + if (policy == SafetyPolicyFamily::Control) { + if (view.snapshot.emergency_stop_active != TriState::False) { + return RecoveryCheckResult{ + false, + SafetyReason::EmergencyStopActive, + "emergency stop is active or unknown"}; + } + if (view.snapshot.protective_stop_active != TriState::False) { + return RecoveryCheckResult{ + false, + SafetyReason::ProtectiveStopActive, + "protective stop is active or unknown"}; + } + if (view.snapshot.fault_active != TriState::False) { + return RecoveryCheckResult{ + false, + SafetyReason::DeviceFault, + "device fault remains active or unknown"}; + } + } else if (view.snapshot.fault_active == TriState::True) { + return RecoveryCheckResult{ + false, + SafetyReason::DeviceFault, + "device fault remains active"}; + } + if (view.snapshot.condition == SafetyCondition::Unsafe) { + return RecoveryCheckResult{ + false, + SafetyReason::HardwareUnsafe, + "hardware safety condition remains unsafe"}; + } + if (policy == SafetyPolicyFamily::Control && + view.snapshot.quiescent != TriState::True) { + return RecoveryCheckResult{ + false, + SafetyReason::DeviceStillMoving, + "control device has not confirmed a quiescent state"}; + } + if (require_operational_ready && + (view.snapshot.condition != SafetyCondition::Nominal || + view.snapshot.operational_ready != TriState::True)) { + return RecoveryCheckResult{ + false, + SafetyReason::DeviceNotReady, + "device did not confirm a nominal operational state"}; + } + return RecoveryCheckResult{true, SafetyReason::None, {}}; + }; + + if (request.restore_operational_state) { + std::lock_guard lock(impl_->mutex); + impl_->active_operation_phase = "restore_devices"; + } for (const auto& target : targets) { SafetyTargetResult target_result; target_result.target_id = target.id; @@ -1990,37 +2094,81 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request) target_result.reason = SafetyReason::SafetyStateStale; target_result.detail = "active safety refresh did not publish a newer sample"; - } else if (!view.fresh) { - target_result.reason = SafetyReason::SafetyStateStale; - target_result.detail = "refreshed safety sample is stale"; - } else if (view.snapshot.condition == SafetyCondition::Unknown) { - target_result.reason = SafetyReason::SafetyStateMissing; - target_result.detail = "hardware safety condition remains unknown"; - } else if (view.snapshot.condition == SafetyCondition::Unsafe) { - target_result.reason = SafetyReason::HardwareUnsafe; - target_result.detail = "hardware safety condition remains unsafe"; - } else if (view.snapshot.connected != TriState::True) { - target_result.reason = SafetyReason::DeviceDisconnected; - target_result.detail = "device connection is not confirmed"; - } else if (view.snapshot.emergency_stop_active != TriState::False) { - target_result.reason = SafetyReason::EmergencyStopActive; - target_result.detail = - "emergency stop must be released and observed as false"; - } else if (view.snapshot.protective_stop_active != TriState::False) { - target_result.reason = SafetyReason::ProtectiveStopActive; - target_result.detail = - "protective stop must be resolved by the typed hardware flow"; - } else if (view.snapshot.fault_active != TriState::False) { - target_result.reason = SafetyReason::DeviceFault; - target_result.detail = "device fault remains active or unknown"; - } else if (target.policy == SafetyPolicyFamily::Control && - view.snapshot.quiescent != TriState::True) { - target_result.reason = SafetyReason::DeviceStillMoving; - target_result.detail = - "control device has not confirmed a quiescent state"; } else { - target_result.success = true; - target_result.reason = SafetyReason::None; + RecoveryCheckResult checked; + if (!request.restore_operational_state) { + checked = validate_snapshot(view, target.policy, false); + } else if (!clear_authorized) { + checked = { + false, + SafetyReason::RecoveryAuditFailed, + "persistent recovery audit did not authorize operational restore"}; + } else if (!view.fresh) { + checked = { + false, + SafetyReason::SafetyStateStale, + "refreshed safety sample is stale"}; + } else if (view.snapshot.condition == SafetyCondition::Unknown) { + checked = { + false, + SafetyReason::SafetyStateMissing, + "hardware safety condition remains unknown"}; + } else if (view.snapshot.connected != TriState::True) { + checked = { + false, + SafetyReason::DeviceDisconnected, + "device connection is not confirmed"}; + } else if (target.policy == SafetyPolicyFamily::Control && + view.snapshot.emergency_stop_active != + TriState::False) { + checked = { + false, + SafetyReason::EmergencyStopActive, + "emergency stop must be released before operational recovery"}; + } else if (target.policy == SafetyPolicyFamily::Control && + view.snapshot.quiescent != TriState::True) { + checked = { + false, + SafetyReason::DeviceStillMoving, + "control device has not confirmed a quiescent state"}; + } else { + checked = validate_snapshot(view, target.policy, true); + if (!checked.reconciled) { + try { + checked = target.endpoint->restoreOperationalState( + recovery_context); + } catch (const std::exception& error) { + checked = { + false, SafetyReason::InternalError, error.what()}; + } catch (...) { + checked = { + false, + SafetyReason::InternalError, + "device operational recovery threw an unknown exception"}; + } + if (checked.reconciled) { + const auto restored_sequence = + view.snapshot.sample_sequence; + target.endpoint->requestSafetyRefresh(); + if (!impl_->snapshots.waitForNewerSample( + target.id, + restored_sequence, + deadline, + view)) { + checked = { + false, + SafetyReason::SafetyStateStale, + "operational recovery did not publish a newer safety sample"}; + } else { + checked = validate_snapshot( + view, target.policy, true); + } + } + } + } + target_result.success = checked.reconciled; + target_result.reason = checked.reason; + target_result.detail = std::move(checked.detail); } if (!target_result.success) { @@ -2030,7 +2178,10 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request) } std::vector retained_candidate_ids; - if (!request.verify_only) { + const bool operational_hardware_verified = all_verified; + if (!request.verify_only && + (!request.restore_operational_state || + operational_hardware_verified)) { const bool has_device_clear_candidate = std::any_of( result.targets.begin(), result.targets.end(), [](const auto& target) { return target.success; }); @@ -2059,8 +2210,9 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request) const bool has_clear_candidate = has_device_clear_candidate || !retained_candidate_ids.empty(); - bool clear_authorized = true; - if (has_clear_candidate && request.authorize_clear) { + if (has_clear_candidate && request.authorize_clear && + !clear_authorization_checked) { + clear_authorization_checked = true; try { clear_authorized = request.authorize_clear(); } catch (...) { @@ -2270,7 +2422,8 @@ RecoveryResult SafetyManager::recover(const RecoveryRequest& request) if (device == impl_->devices.end()) { continue; } - if (target.success && !request.verify_only) { + if (target.success && !request.verify_only && + (!request.restore_operational_state || all_verified)) { device->second.quarantined = false; device->second.software_blockers.clear(); } diff --git a/cmvr-es/manager/safety_manager/tests/safety_manager_test.cpp b/cmvr-es/manager/safety_manager/tests/safety_manager_test.cpp index 1667e251..04fd32c2 100644 --- a/cmvr-es/manager/safety_manager/tests/safety_manager_test.cpp +++ b/cmvr-es/manager/safety_manager/tests/safety_manager_test.cpp @@ -10,10 +10,10 @@ namespace cmvr::safety { namespace { -DeviceSafetyDescriptor controlDescriptor() +DeviceSafetyDescriptor controlDescriptor(std::string device_id = "arm") { DeviceSafetyDescriptor descriptor; - descriptor.device_id = "arm"; + descriptor.device_id = std::move(device_id); descriptor.kind = device::DeviceKind::Arm; descriptor.default_policy = SafetyPolicyFamily::Control; descriptor.maximum_snapshot_age = std::chrono::seconds(1); @@ -75,6 +75,23 @@ public: return recovery_check; } + RecoveryCheckResult restoreOperationalState( + const RecoveryContext&) override + { + ++operational_restores; + if (operational_restore_check.reconciled && + restore_to_nominal_on_success) { + condition = SafetyCondition::Nominal; + connected = TriState::True; + ready = TriState::True; + quiescent = TriState::True; + emergency_stop = TriState::False; + protective_stop = TriState::False; + fault = TriState::False; + } + return operational_restore_check; + } + DeviceSafetyDescriptor descriptor_; SafetySnapshotPublisher publisher_; SafetyCondition condition{SafetyCondition::Nominal}; @@ -86,11 +103,15 @@ public: TriState fault{TriState::False}; HardwareCheckResult final_check{true, SafetyReason::None, {}}; RecoveryCheckResult recovery_check{true, SafetyReason::None, {}}; + RecoveryCheckResult operational_restore_check{ + true, SafetyReason::None, {}}; std::uint64_t generation{1}; std::uint64_t sequence{0}; bool publish_on_refresh{true}; std::atomic hardware_checks{0}; std::atomic recoveries{0}; + std::atomic operational_restores{0}; + bool restore_to_nominal_on_success{true}; }; class FakeParticipant final : public SafetyParticipant { @@ -466,6 +487,260 @@ TEST(SafetyManagerTest, RecoveryCannotIgnoreEmergencyStop) EXPECT_EQ(endpoint->recoveries.load(), 0); } +TEST(SafetyManagerTest, + OperationalRestoreClearsRecoverableHardwareAndSoftwareState) +{ + SafetyManager coordinator; + auto endpoint = std::make_shared(controlDescriptor()); + endpoint->condition = SafetyCondition::Unsafe; + endpoint->ready = TriState::False; + endpoint->protective_stop = TriState::True; + endpoint->quiescent = TriState::True; + ASSERT_TRUE(coordinator.registerDevice( + {controlDescriptor(), endpoint, {}})); + coordinator.markStartupComplete(); + coordinator.quarantineDevice("arm", SafetyReason::OutcomeUnknown); + const auto before = coordinator.snapshot(); + + RecoveryRequest request; + request.recovery_id = "restore-operational"; + request.all_devices = true; + request.expected_safety_epoch = before.safety_epoch; + request.verify_only = false; + request.restore_operational_state = true; + request.reason = "operator confirmed the work cell is clear"; + request.deadline = SafetyClock::now() + std::chrono::seconds(1); + request.authorize_clear = [] { return true; }; + + const auto restored = coordinator.recover(request); + + EXPECT_EQ(restored.result, RecoveryResultCode::Recovered); + EXPECT_EQ(restored.system_state, SystemAdmissionState::Open); + ASSERT_EQ(restored.targets.size(), 1U); + EXPECT_TRUE(restored.targets.front().success) + << restored.targets.front().detail; + EXPECT_EQ(endpoint->operational_restores.load(), 1); + EXPECT_EQ(endpoint->recoveries.load(), 1); + EXPECT_EQ( + coordinator.snapshot().devices.front().admission_state, + DeviceAdmissionState::Open); +} + +TEST(SafetyManagerTest, OperationalRestoreFailsClosedOnActiveEmergencyStop) +{ + SafetyManager coordinator; + auto endpoint = std::make_shared(controlDescriptor()); + endpoint->condition = SafetyCondition::Unsafe; + endpoint->ready = TriState::False; + endpoint->emergency_stop = TriState::True; + endpoint->quiescent = TriState::True; + endpoint->operational_restore_check = { + false, + SafetyReason::EmergencyStopActive, + "hardware emergency-stop input is still active"}; + ASSERT_TRUE(coordinator.registerDevice( + {controlDescriptor(), endpoint, {}})); + coordinator.markStartupComplete(); + coordinator.quarantineDevice("arm", SafetyReason::OutcomeUnknown); + const auto before = coordinator.snapshot(); + + RecoveryRequest request; + request.recovery_id = "restore-active-estop"; + request.all_devices = true; + request.expected_safety_epoch = before.safety_epoch; + request.verify_only = false; + request.restore_operational_state = true; + request.reason = "operator requested recovery"; + request.deadline = SafetyClock::now() + std::chrono::seconds(1); + request.authorize_clear = [] { return true; }; + + const auto restored = coordinator.recover(request); + + EXPECT_EQ(restored.result, RecoveryResultCode::BlockerRemains); + EXPECT_EQ(restored.system_state, SystemAdmissionState::Latched); + ASSERT_EQ(restored.targets.size(), 1U); + EXPECT_FALSE(restored.targets.front().success); + EXPECT_EQ( + restored.targets.front().reason, + SafetyReason::EmergencyStopActive); + EXPECT_EQ(endpoint->operational_restores.load(), 0); + EXPECT_EQ(endpoint->recoveries.load(), 0); +} + +TEST(SafetyManagerTest, OperationalRestoreDoesNotRecoverAnActiveDevice) +{ + SafetyManager coordinator; + auto endpoint = std::make_shared(controlDescriptor()); + endpoint->condition = SafetyCondition::Unsafe; + endpoint->ready = TriState::False; + endpoint->quiescent = TriState::False; + endpoint->fault = TriState::True; + ASSERT_TRUE(coordinator.registerDevice( + {controlDescriptor(), endpoint, {}})); + coordinator.markStartupComplete(); + coordinator.quarantineDevice("arm", SafetyReason::OutcomeUnknown); + + RecoveryRequest request; + request.recovery_id = "restore-active-device"; + request.all_devices = true; + request.expected_safety_epoch = coordinator.snapshot().safety_epoch; + request.verify_only = false; + request.restore_operational_state = true; + request.reason = "operator requested recovery"; + request.deadline = SafetyClock::now() + std::chrono::seconds(1); + request.authorize_clear = [] { return true; }; + + const auto restored = coordinator.recover(request); + + EXPECT_EQ(restored.result, RecoveryResultCode::BlockerRemains); + ASSERT_EQ(restored.targets.size(), 1U); + EXPECT_EQ( + restored.targets.front().reason, + SafetyReason::DeviceStillMoving); + EXPECT_EQ(endpoint->operational_restores.load(), 0); + EXPECT_EQ(endpoint->recoveries.load(), 0); +} + +TEST(SafetyManagerTest, OperationalRestoreAuditFailurePreventsDeviceRecovery) +{ + SafetyManager coordinator; + auto endpoint = std::make_shared(controlDescriptor()); + endpoint->condition = SafetyCondition::Unsafe; + endpoint->ready = TriState::False; + endpoint->protective_stop = TriState::True; + ASSERT_TRUE(coordinator.registerDevice( + {controlDescriptor(), endpoint, {}})); + coordinator.markStartupComplete(); + coordinator.quarantineDevice("arm", SafetyReason::OutcomeUnknown); + + RecoveryRequest request; + request.recovery_id = "restore-audit-failure"; + request.all_devices = true; + request.expected_safety_epoch = coordinator.snapshot().safety_epoch; + request.verify_only = false; + request.restore_operational_state = true; + request.reason = "operator requested recovery"; + request.deadline = SafetyClock::now() + std::chrono::seconds(1); + request.authorize_clear = [] { return false; }; + + const auto restored = coordinator.recover(request); + + EXPECT_EQ(restored.result, RecoveryResultCode::BlockerRemains); + ASSERT_EQ(restored.targets.size(), 1U); + EXPECT_EQ( + restored.targets.front().reason, + SafetyReason::RecoveryAuditFailed); + EXPECT_EQ(endpoint->operational_restores.load(), 0); + EXPECT_EQ(endpoint->recoveries.load(), 0); +} + +TEST(SafetyManagerTest, OperationalRestoreRequiresExplicitAuthorization) +{ + SafetyManager coordinator; + auto endpoint = std::make_shared(controlDescriptor()); + ASSERT_TRUE(coordinator.registerDevice( + {controlDescriptor(), endpoint, {}})); + coordinator.markStartupComplete(); + + RecoveryRequest request; + request.recovery_id = "restore-without-authorization"; + request.all_devices = true; + request.expected_safety_epoch = coordinator.snapshot().safety_epoch; + request.verify_only = false; + request.restore_operational_state = true; + request.reason = "operator requested recovery"; + + const auto restored = coordinator.recover(request); + + EXPECT_EQ(restored.result, RecoveryResultCode::Failed); + ASSERT_EQ(restored.targets.size(), 1U); + EXPECT_EQ( + restored.targets.front().reason, + SafetyReason::InvalidArgument); + EXPECT_EQ(endpoint->operational_restores.load(), 0); +} + +TEST(SafetyManagerTest, + OperationalRestoreAuditFailureWithNoDevicesRemainsLatched) +{ + SafetyManager coordinator; + coordinator.markStartupComplete(); + + RecoveryRequest request; + request.recovery_id = "empty-restore-audit-failure"; + request.all_devices = true; + request.expected_safety_epoch = coordinator.snapshot().safety_epoch; + request.verify_only = false; + request.restore_operational_state = true; + request.reason = "operator requested recovery"; + request.deadline = SafetyClock::now() + std::chrono::seconds(1); + request.authorize_clear = [] { return false; }; + + const auto restored = coordinator.recover(request); + + EXPECT_EQ(restored.result, RecoveryResultCode::BlockerRemains); + EXPECT_EQ(restored.system_state, SystemAdmissionState::Latched); + ASSERT_EQ(restored.targets.size(), 1U); + EXPECT_EQ(restored.targets.front().target_id, "system"); + EXPECT_EQ( + restored.targets.front().reason, + SafetyReason::RecoveryAuditFailed); +} + +TEST(SafetyManagerTest, + OperationalRestoreDoesNotPartiallyReleaseSoftwareAdmission) +{ + SafetyManager coordinator; + auto first = std::make_shared( + controlDescriptor("arm-first")); + first->condition = SafetyCondition::Unsafe; + first->ready = TriState::False; + first->protective_stop = TriState::True; + auto second = std::make_shared( + controlDescriptor("arm-second")); + second->condition = SafetyCondition::Unsafe; + second->ready = TriState::False; + second->fault = TriState::True; + second->operational_restore_check = { + false, SafetyReason::DeviceFault, "fault reset failed"}; + ASSERT_TRUE(coordinator.registerDevice( + {controlDescriptor("arm-first"), first, {}})); + ASSERT_TRUE(coordinator.registerDevice( + {controlDescriptor("arm-second"), second, {}})); + coordinator.markStartupComplete(); + coordinator.quarantineDevice( + "arm-first", SafetyReason::OutcomeUnknown); + coordinator.quarantineDevice( + "arm-second", SafetyReason::OutcomeUnknown); + + RecoveryRequest request; + request.recovery_id = "restore-partial-failure"; + request.all_devices = true; + request.expected_safety_epoch = coordinator.snapshot().safety_epoch; + request.verify_only = false; + request.restore_operational_state = true; + request.reason = "operator requested recovery"; + request.deadline = SafetyClock::now() + std::chrono::seconds(1); + request.authorize_clear = [] { return true; }; + + const auto restored = coordinator.recover(request); + + EXPECT_EQ(restored.result, RecoveryResultCode::BlockerRemains); + EXPECT_EQ(restored.system_state, SystemAdmissionState::Latched); + EXPECT_EQ(first->operational_restores.load(), 1); + EXPECT_EQ(second->operational_restores.load(), 1); + EXPECT_EQ(first->recoveries.load(), 0); + EXPECT_EQ(second->recoveries.load(), 0); + const auto snapshot = coordinator.snapshot(); + ASSERT_EQ(snapshot.devices.size(), 2U); + for (const auto& device : snapshot.devices) { + EXPECT_EQ( + device.admission_state, + DeviceAdmissionState::Quarantined) + << device.descriptor.device_id; + } +} + TEST(SafetyManagerTest, RecoveryAuditFailureCannotReleaseLatch) { SafetyManager coordinator; diff --git a/cmvr-es/service/grpc/server/include/grpc_system_service.h b/cmvr-es/service/grpc/server/include/grpc_system_service.h index f0ae3deb..f6e8a3d5 100644 --- a/cmvr-es/service/grpc/server/include/grpc_system_service.h +++ b/cmvr-es/service/grpc/server/include/grpc_system_service.h @@ -50,6 +50,7 @@ namespace cmvr::service grpc::Status ExecuteActionQueue(grpc::ServerContext* context, const cmvr::api::ActionQueueCommand_Request* request, cmvr::api::ActionQueueCommand_Feedback* response) override; grpc::Status GetSafetyState(grpc::ServerContext* context, const cmvr::api::GetSafetyStateCommand_Request* request, cmvr::api::GetSafetyStateCommand_Feedback* response) override; grpc::Status RecoverSafetyState(grpc::ServerContext* context, const cmvr::api::RecoverSafetyStateCommand_Request* request, cmvr::api::RecoverSafetyStateCommand_Feedback* response) override; + grpc::Status RestoreOperationalState(grpc::ServerContext* context, const cmvr::api::RestoreOperationalStateCommand_Request* request, cmvr::api::RestoreOperationalStateCommand_Feedback* response) override; private: device::DeviceManager& dmgr_; const std::chrono::milliseconds stop_timeout_; diff --git a/cmvr-es/service/grpc/server/src/grpc_security.cpp b/cmvr-es/service/grpc/server/src/grpc_security.cpp index 78593af9..91271d8d 100644 --- a/cmvr-es/service/grpc/server/src/grpc_security.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_security.cpp @@ -419,6 +419,9 @@ const GrpcMethodPolicyRegistry& defaultGrpcMethodPolicyRegistry() add("SystemService", "RecoverSafetyState", GrpcAccessClass::Recover, CommandIntent::RecoverAdmission, SafetyPolicyFamily::Control, true, true); + add("SystemService", "RestoreOperationalState", + GrpcAccessClass::Recover, CommandIntent::RecoverAdmission, + SafetyPolicyFamily::Control, true, true); add_many("ArmService", {"getJointState", "getPose", "getPoseMatrix", diff --git a/cmvr-es/service/grpc/server/src/grpc_system_service.cpp b/cmvr-es/service/grpc/server/src/grpc_system_service.cpp index 211d2f37..a8c619a5 100644 --- a/cmvr-es/service/grpc/server/src/grpc_system_service.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_system_service.cpp @@ -1575,6 +1575,166 @@ grpc::Status gRPCSystemServiceImpl::RecoverSafetyState( return grpc::Status::OK; } +grpc::Status gRPCSystemServiceImpl::RestoreOperationalState( + grpc::ServerContext* context, + const cmvr::api::RestoreOperationalStateCommand_Request* request, + cmvr::api::RestoreOperationalStateCommand_Feedback* response) +{ + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.SystemService/RestoreOperationalState"); + if (!request || !response) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "RestoreOperationalState request and response are required"); + } + if (request->recovery_id().empty() || request->reason().empty()) { + const std::string detail = + "recovery_id and reason are required"; + setSafetyHeaderFailure( + response->mutable_header(), + request->reason().empty() + ? cmvr::safety::SafetyReason::RecoveryReasonRequired + : cmvr::safety::SafetyReason::InvalidArgument, + detail); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, detail); + } + if (!recovery_audit_sink_) { + const std::string detail = + "persistent recovery audit is not configured"; + setSafetyHeaderFailure( + response->mutable_header(), + cmvr::safety::SafetyReason::RecoveryAuditFailed, + detail); + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, + "RECOVERY_AUDIT_FAILED"); + } + + const auto accepted_snapshot = dmgr_.safetyManager().snapshot(); + RecoveryAuditRecord audit; + audit.occurred_at_unix_ms = unixTimeMs(); + audit.stage = "accepted"; + audit.correlation_id = cmvr_grpc_call_guard.context().correlation_id; + audit.principal_id = cmvr_grpc_call_guard.context().principal.id; + audit.peer = cmvr_grpc_call_guard.context().peer; + audit.recovery_id = request->recovery_id(); + audit.reason = request->reason(); + audit.mode = "restore_operational_state"; + audit.all_devices = true; + audit.expected_safety_epoch = accepted_snapshot.safety_epoch; + audit.result = "pending"; + std::string audit_error; + if (!recovery_audit_sink_->append(audit, &audit_error)) { + const auto detail = audit_error.empty() + ? std::string("persistent recovery audit write failed") + : audit_error; + setSafetyHeaderFailure( + response->mutable_header(), + cmvr::safety::SafetyReason::RecoveryAuditFailed, + detail); + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, + "RECOVERY_AUDIT_FAILED"); + } + + auto deadline = cmvr_grpc_call_guard.context().deadline; + const auto configured_deadline = + cmvr::safety::SafetyClock::now() + + dmgr_.safetyManager().config().recovery_timeout; + if (deadline == cmvr::safety::SafetyClock::time_point::max() || + configured_deadline < deadline) { + deadline = configured_deadline; + } + if (request->timeout_ms() != 0) { + deadline = std::min( + deadline, + cmvr::safety::SafetyClock::now() + + std::chrono::milliseconds(request->timeout_ms())); + } + + auto commit_audit = audit; + commit_audit.stage = "restore_commit"; + commit_audit.result = "authorized"; + const auto sink = recovery_audit_sink_; + + cmvr::safety::RecoveryRequest coordinator_request; + coordinator_request.recovery_id = request->recovery_id(); + coordinator_request.all_devices = true; + coordinator_request.expected_safety_epoch = + accepted_snapshot.safety_epoch; + coordinator_request.verify_only = false; + coordinator_request.restore_operational_state = true; + coordinator_request.reason = request->reason(); + coordinator_request.deadline = deadline; + coordinator_request.authorize_clear = + [sink, commit_audit = std::move(commit_audit)]() mutable { + commit_audit.occurred_at_unix_ms = unixTimeMs(); + std::string error; + const bool persisted = sink->append(commit_audit, &error); + if (!persisted) { + CMVR_LOG(ERROR) + << "[gRPCSystemServiceImpl] Operational restore audit " + "failed: " + << error; + } + return persisted; + }; + + const auto result = + dmgr_.safetyManager().recover(coordinator_request); + response->set_recovery_id(result.recovery_id); + response->set_result(toApiRecoveryResult(result.result)); + response->set_previous_safety_epoch(result.previous_safety_epoch); + response->set_current_safety_epoch(result.current_safety_epoch); + response->set_system_state( + toApiSystemAdmissionState(result.system_state)); + for (const auto& target : result.targets) { + populateSafetyTargetResult(target, *response->add_targets()); + } + + const bool success = recoveryCompletedAsRequested(result.result); + auto* header = response->mutable_header(); + header->set_success(success); + header->set_command_id(request->recovery_id()); + header->set_service_instance_id( + dmgr_.safetyManager().serviceInstanceId()); + header->set_safety_epoch(result.current_safety_epoch); + header->set_execution_state( + success ? cmvr::api::COMMAND_EXECUTION_STATE_COMPLETED + : cmvr::api::COMMAND_EXECUTION_STATE_FAILED); + if (success) { + header->set_reason_code(cmvr::api::COMMAND_REASON_CODE_NONE); + } else { + const auto failed = std::find_if( + result.targets.begin(), result.targets.end(), + [](const auto& target) { return !target.success; }); + if (failed != result.targets.end()) { + header->set_reason_code(toApiSafetyReason(failed->reason)); + header->set_error_message(failed->detail); + } else { + header->set_reason_code( + cmvr::api::COMMAND_REASON_CODE_INTERNAL_ERROR); + header->set_error_message( + "operational restore failed without a target result"); + } + } + setCurrentTimestamp(header->mutable_timestamp()); + + audit.occurred_at_unix_ms = unixTimeMs(); + audit.stage = "completed"; + audit.previous_safety_epoch = result.previous_safety_epoch; + audit.current_safety_epoch = result.current_safety_epoch; + audit.result = cmvr::safety::toString(result.result); + if (!recovery_audit_sink_->append(audit, &audit_error)) { + CMVR_LOG(ERROR) + << "[gRPCSystemServiceImpl] Operational restore completion audit " + "failed: " + << audit_error; + } + return grpc::Status::OK; +} + grpc::Status gRPCSystemServiceImpl::UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) { CMVR_GRPC_REQUIRE_REGISTERED_CALL( diff --git a/cmvr-es/service/grpc/server/tests/grpc_system_service_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_system_service_test.cpp index 3b3ba689..227355bd 100644 --- a/cmvr-es/service/grpc/server/tests/grpc_system_service_test.cpp +++ b/cmvr-es/service/grpc/server/tests/grpc_system_service_test.cpp @@ -646,7 +646,13 @@ public: return model; } std::size_t getDof() const override { return kDof; } - device::ArmState getRobotState() const override { return {}; } + device::ArmState getRobotState() const override + { + std::lock_guard lock(mutex_); + auto state = reported_state_; + state.moving = state.moving || active_motions_ != 0; + return state; + } device::JointGroupState getJointState() const override { return {}; } device::CartesianPose getTcpPose( device::FrameType = device::FrameType::Base) const override @@ -793,7 +799,19 @@ public: shutdown_calls_.fetch_add(1, std::memory_order_relaxed); return device::Result::success(); } - device::Result clearFault() override { return device::Result::success(); } + device::Result clearFault() override + { + std::lock_guard lock(mutex_); + ++clear_fault_calls_; + reported_state_.connected = true; + reported_state_.powered_on = true; + reported_state_.moving = false; + reported_state_.program_running = false; + reported_state_.fault = false; + reported_state_.emergency_stopped = false; + reported_state_.protective_stopped = false; + return device::Result::success(); + } device::Result unlockProtectiveStop() override { return device::Result::success(); @@ -949,6 +967,18 @@ public: return shutdown_calls_.load(std::memory_order_relaxed); } + int clearFaultCalls() const + { + std::lock_guard lock(mutex_); + return clear_fault_calls_; + } + + void setReportedState(const device::ArmState& state) + { + std::lock_guard lock(mutex_); + reported_state_ = state; + } + void blockHealthSnapshot() { std::lock_guard lock(health_mutex_); @@ -1096,6 +1126,8 @@ private: int health_snapshot_calls_{0}; bool block_health_snapshot_{false}; bool health_snapshot_entered_{false}; + device::ArmState reported_state_{}; + int clear_fault_calls_{0}; }; class ActionTestAgv final : public device::AbstractAGV { @@ -1342,6 +1374,18 @@ const api::SafetyOperationTargetResult* findSafetyTarget( return nullptr; } +const api::SafetyOperationTargetResult* findSafetyTarget( + const api::RestoreOperationalStateCommand_Feedback& response, + const std::string& id) +{ + for (const auto& target : response.targets()) { + if (target.target_id() == id) { + return ⌖ + } + } + return nullptr; +} + class GrpcSystemServiceTest : public ::testing::Test { protected: void SetUp() override @@ -1635,6 +1679,25 @@ TEST_F(GrpcSystemServiceTest, RecoveryIsDisabledByDefault) EXPECT_EQ(status.error_message(), "RECOVERY_RPC_DISABLED"); } +TEST_F(GrpcSystemServiceTest, OperationalRestoreIsDisabledByDefault) +{ + config::DeviceManagerConfig config; + (void)device::DeviceManager::getInstance(config); + service_ = std::make_unique(); + + api::RestoreOperationalStateCommand_Request request; + request.set_recovery_id("restore-disabled"); + request.set_reason("operator requested recovery"); + api::RestoreOperationalStateCommand_Feedback response; + grpc::ServerContext context; + + const auto status = service_->RestoreOperationalState( + &context, &request, &response); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_EQ(status.error_message(), "RECOVERY_RPC_DISABLED"); +} + TEST_F(GrpcSystemServiceTest, RecoveryRequiresDurableAuditBeforeCoordinator) { config::DeviceManagerConfig config; @@ -1682,6 +1745,64 @@ TEST_F(GrpcSystemServiceTest, RecoveryRequiresDurableAuditBeforeCoordinator) epoch_before_failure); } +TEST_F(GrpcSystemServiceTest, + RestoreOperationalStateRecoversArmAndClearsAdmission) +{ + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + action_trace_ = std::make_shared(); + action_arm_ = std::make_shared( + "restore-arm", action_trace_); + device::ArmState protected_state; + protected_state.connected = true; + protected_state.powered_on = true; + protected_state.moving = true; + protected_state.program_running = true; + protected_state.protective_stopped = true; + action_arm_->setReportedState(protected_state); + manager.registerDevice(action_arm_); + manager.safetyManager().quarantineDevice( + action_arm_->id(), safety::SafetyReason::OutcomeUnknown); + + auto audit = std::make_shared(); + service_ = std::make_unique( + std::chrono::seconds(1), makeAllowAllRecoveryGateway(), audit); + + api::RestoreOperationalStateCommand_Request request; + request.set_recovery_id("restore-whole-system"); + request.set_reason("operator confirmed the work cell is clear"); + request.set_timeout_ms(1000); + api::RestoreOperationalStateCommand_Feedback response; + grpc::ServerContext context; + + const auto status = service_->RestoreOperationalState( + &context, &request, &response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ( + response.result(), api::SAFETY_OPERATION_RESULT_RECOVERED); + EXPECT_EQ( + response.system_state(), api::SYSTEM_ADMISSION_STATE_OPEN); + EXPECT_EQ(action_arm_->clearFaultCalls(), 1); + const auto* arm_target = findSafetyTarget(response, action_arm_->id()); + ASSERT_NE(arm_target, nullptr); + EXPECT_EQ( + arm_target->result(), api::SAFETY_OPERATION_RESULT_SUCCEEDED); + ASSERT_EQ(audit->records.size(), 3U); + EXPECT_EQ(audit->records[0].stage, "accepted"); + EXPECT_EQ(audit->records[1].stage, "restore_commit"); + EXPECT_EQ(audit->records[2].stage, "completed"); + + const auto snapshot = manager.safetyManager().snapshot(); + EXPECT_EQ(snapshot.system_state, safety::SystemAdmissionState::Open); + ASSERT_EQ(snapshot.devices.size(), 1U); + EXPECT_EQ( + snapshot.devices.front().admission_state, + safety::DeviceAdmissionState::Open); +} + TEST_F(GrpcSystemServiceTest, MapsEveryKnownDeviceKind) { struct ExpectedMapping { @@ -2956,6 +3077,62 @@ TEST_F(GrpcSystemServiceTest, EXPECT_FALSE(lease.acquired); } +TEST_F(GrpcSystemServiceTest, + StopAllAcceptsProtectedArmAsQuiescent) +{ + initializeActionDevices(); + device::ArmState protected_state; + protected_state.connected = true; + protected_state.powered_on = true; + protected_state.moving = true; + protected_state.program_running = true; + protected_state.protective_stopped = true; + action_arm_->setReportedState(protected_state); + + api::StopAllCommand_Request request; + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll( + &context, &request, &response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_TRUE(response.header().success()) + << response.header().error_message(); + const auto* arm_target = findSafetyTarget(response, action_arm_->id()); + ASSERT_NE(arm_target, nullptr); + EXPECT_EQ( + arm_target->result(), api::SAFETY_OPERATION_RESULT_SUCCEEDED); +} + +TEST_F(GrpcSystemServiceTest, + StopAllDoesNotTreatGenericArmFaultAsQuiescent) +{ + initializeActionDevices(); + device::ArmState fault_state; + fault_state.connected = true; + fault_state.powered_on = true; + fault_state.moving = true; + fault_state.program_running = true; + fault_state.fault = true; + action_arm_->setReportedState(fault_state); + + api::StopAllCommand_Request request; + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll( + &context, &request, &response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_FALSE(response.header().success()); + const auto* arm_target = findSafetyTarget(response, action_arm_->id()); + ASSERT_NE(arm_target, nullptr); + EXPECT_EQ( + arm_target->result(), api::SAFETY_OPERATION_RESULT_FAILED); + EXPECT_EQ( + arm_target->reason_code(), + api::COMMAND_REASON_CODE_DEVICE_STILL_MOVING); +} + TEST_F(GrpcSystemServiceTest, StopAllStopsActiveMediaWithoutStoppingDeviceLifecycles) { diff --git a/docs/device_safety_control_plane_architecture.md b/docs/device_safety_control_plane_architecture.md index d6675cd9..061c0800 100644 --- a/docs/device_safety_control_plane_architecture.md +++ b/docs/device_safety_control_plane_architecture.md @@ -535,6 +535,27 @@ struct AdmissionPermit { 不能把它登记成 ResetFault； - ResetFault 成功只触发安全快照刷新，不直接清除 central quarantine。 +### 8.2.1 整机运行态恢复 + +`SystemService.RestoreOperationalState` 是面向上层平台的整机恢复事务，与 +`RecoverSafetyState` 的软件准入恢复语义分离： + +- 请求必须携带唯一 `recovery_id`、非空 `reason`，并通过与 Recover 相同的 + `RecoveryExposure` 和持久审计策略； +- 事务固定覆盖全部已注册设备，并与 StopAll、其他恢复事务通过 process operation mutex + 串行；事务期间系统处于 Recovering，拒绝新的普通运动命令； +- 硬件恢复前必须主动取得新鲜快照，确认设备已连接；Control 设备还必须确认物理急停已释放 + 且已经静止。条件未知、急停仍有效、设备仍运动或审计授权失败时，不调用设备恢复能力； +- 对尚未达到 Nominal/operational-ready 的支持设备调用 + `DeviceSafetyEndpoint::restoreOperationalState()`。Legacy Arm 和 AGV adapter 分别调用设备 + `clearFault()`；该操作允许按设备实现执行上电、使能和故障复位，但不得恢复旧轨迹、导航、 + teleop session、control lease 或 ActionQueue； +- 每个硬件恢复动作完成后必须再次主动刷新，确认连接、新鲜度、急停、保护停、fault、静止和 + operational-ready。任一设备失败时系统保持 Latched，并返回逐设备失败原因；中央 quarantine、 + retained barrier 和全局 gate 不做部分释放； +- 只有全部必需设备验证成功后，才协调释放软件 latch 和 retained barrier，将系统切回 Open。 + 调用成功只表示整机已使能、空闲并可接受新的 gRPC 命令，不会续跑中断前的任务。 + ### 8.3 流式控制 Teleoperation、Motor cyclic stream 和未来连续控制不为每个 setpoint 写 unary ledger： @@ -1092,7 +1113,8 @@ public: `CommandIntent` 和最低 role。未知方法在 Authenticated Profile 中 fail-closed；Compatibility Profile 可以只为已有 RPC 保留当前行为，但仍必须产生 `unclassified_method` 告警并在阶段 2 前清零。 -`RecoverSafetyState` 额外使用独立的 `RecoveryExposure`： +`RecoverSafetyState` 和 `RestoreOperationalState` 额外使用独立的 +`RecoveryExposure`： | RecoveryExposure | 行为 | | --- | --- | diff --git a/protos/cmvr/api/safety_command.proto b/protos/cmvr/api/safety_command.proto index b227f842..4e5b774c 100644 --- a/protos/cmvr/api/safety_command.proto +++ b/protos/cmvr/api/safety_command.proto @@ -184,3 +184,21 @@ message RecoverSafetyStateCommand { string recovery_id = 7; } } + +message RestoreOperationalStateCommand { + message Request { + string recovery_id = 1; + string reason = 2; + uint32 timeout_ms = 3; + } + + message Feedback { + CommandHeader.Feedback header = 1; + SafetyOperationResult result = 2; + uint64 previous_safety_epoch = 3; + uint64 current_safety_epoch = 4; + SystemAdmissionState system_state = 5; + repeated SafetyOperationTargetResult targets = 6; + string recovery_id = 7; + } +} diff --git a/protos/cmvr/api/system_service.proto b/protos/cmvr/api/system_service.proto index 44267733..ac961ac2 100644 --- a/protos/cmvr/api/system_service.proto +++ b/protos/cmvr/api/system_service.proto @@ -19,4 +19,5 @@ service SystemService { rpc GetSafetyState(GetSafetyStateCommand.Request) returns (GetSafetyStateCommand.Feedback) {} rpc RecoverSafetyState(RecoverSafetyStateCommand.Request) returns (RecoverSafetyStateCommand.Feedback) {} + rpc RestoreOperationalState(RestoreOperationalStateCommand.Request) returns (RestoreOperationalStateCommand.Feedback) {} }