From f4be2ffaaa4fb8990399bab71e566f1f870943dd Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Mon, 17 Aug 2026 08:34:44 +0800 Subject: [PATCH] feat(safety): unify device admission and recovery Add the DeviceManager-owned safety coordinator, shared sensor/control policies, command ledger, service guards, generalized StopAll, and RecoverSafetyState. Preserve device-side hardware checks and AUBO hardware E-stop release reconciliation while keeping software E-stop independently latched. --- cmvr-es/CMakeLists.txt | 1 + cmvr-es/config/manager/device_manager.pb.txt | 12 + .../grpc_server_task/grpc_server_task.pb.txt | 9 + cmvr-es/devices/arm/aubo_arm/README.md | 20 +- cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp | 79 +- .../devices/arm/aubo_arm/aubo_safety_state.h | 54 +- .../aubo_arm/tests/aubo_safety_state_test.cpp | 44 +- cmvr-es/manager/device_manager/CMakeLists.txt | 2 + .../device_manager/include/device_manager.h | 18 + .../include/device_safety_adapters.h | 18 + .../device_manager/src/device_manager.cpp | 302 +- .../src/device_safety_adapters.cpp | 1022 +++++++ .../tests/device_manager_lifecycle_test.cpp | 29 + .../tests/device_manager_snapshot_test.cpp | 39 +- .../manager/media_source_hub/CMakeLists.txt | 28 + .../include/device_media_source_adapter.h | 8 + .../src/device_media_source_adapter.cpp | 39 + .../device_media_source_adapter_test.cpp | 113 + cmvr-es/manager/safety/CMakeLists.txt | 65 + .../manager/safety/include/command_ledger.h | 130 + .../safety/include/device_safety_endpoint.h | 39 + .../safety/include/safety_coordinator.h | 225 ++ .../safety/include/safety_participant.h | 88 + .../manager/safety/include/safety_reason.h | 48 + .../safety/include/safety_snapshot_store.h | 54 + cmvr-es/manager/safety/include/safety_types.h | 235 ++ cmvr-es/manager/safety/src/command_ledger.cpp | 276 ++ .../manager/safety/src/safety_coordinator.cpp | 2424 +++++++++++++++++ cmvr-es/manager/safety/src/safety_reason.cpp | 92 + .../safety/src/safety_snapshot_store.cpp | 305 +++ .../safety/tests/command_ledger_test.cpp | 122 + .../safety/tests/safety_coordinator_test.cpp | 507 ++++ .../tests/safety_snapshot_store_test.cpp | 111 + cmvr-es/service/CMakeLists.txt | 55 + .../action/include/action_queue_executor.h | 4 +- .../action/src/action_queue_executor.cpp | 162 +- .../camera_operational_activity_registry.h | 10 +- .../include/camera_ptz_activity_registry.h | 7 +- .../service/grpc/include/grpc_agv_service.h | 7 + .../service/grpc/include/grpc_arm_service.h | 7 + .../grpc/include/grpc_arm_teleop_service.h | 16 +- .../grpc/include/grpc_camera_service.h | 8 +- .../grpc/include/grpc_command_transaction.h | 211 ++ .../grpc/include/grpc_dexhand_service.h | 6 + .../service/grpc/include/grpc_head_service.h | 6 + .../service/grpc/include/grpc_hlc_service.h | 9 + .../grpc/include/grpc_microphone_service.h | 5 + .../service/grpc/include/grpc_motor_service.h | 33 +- .../grpc/include/grpc_recovery_audit.h | 38 + .../grpc/include/grpc_safety_participants.h | 43 + .../service/grpc/include/grpc_safety_proto.h | 37 + cmvr-es/service/grpc/include/grpc_security.h | 268 ++ .../grpc/include/grpc_speaker_service.h | 6 + .../grpc/include/grpc_system_service.h | 20 +- .../grpc/include/media_activity_coordinator.h | 14 + .../grpc/include/motor_activity_coordinator.h | 10 + .../camera_operational_activity_registry.cpp | 16 +- .../grpc/src/camera_ptz_activity_registry.cpp | 21 +- cmvr-es/service/grpc/src/grpc_agv_service.cpp | 264 +- cmvr-es/service/grpc/src/grpc_arm_service.cpp | 205 +- .../grpc/src/grpc_arm_teleop_service.cpp | 66 +- .../service/grpc/src/grpc_camera_service.cpp | 190 +- .../grpc/src/grpc_command_transaction.cpp | 1269 +++++++++ .../service/grpc/src/grpc_dexhand_service.cpp | 119 +- .../service/grpc/src/grpc_head_service.cpp | 535 ++-- cmvr-es/service/grpc/src/grpc_hlc_service.cpp | 123 +- .../grpc/src/grpc_microphone_service.cpp | 140 +- .../service/grpc/src/grpc_motor_service.cpp | 283 +- .../service/grpc/src/grpc_recovery_audit.cpp | 164 ++ .../grpc/src/grpc_safety_participants.cpp | 1139 ++++++++ .../service/grpc/src/grpc_safety_proto.cpp | 397 +++ cmvr-es/service/grpc/src/grpc_security.cpp | 688 +++++ .../service/grpc/src/grpc_speaker_service.cpp | 181 +- .../service/grpc/src/grpc_system_service.cpp | 461 +++- .../grpc/src/media_activity_coordinator.cpp | 26 +- .../grpc/src/motor_activity_coordinator.cpp | 13 +- .../camera_ptz_activity_registry_test.cpp | 85 + .../grpc/tests/grpc_arm_service_test.cpp | 71 +- .../tests/grpc_arm_teleop_service_test.cpp | 145 +- .../tests/grpc_command_transaction_test.cpp | 454 +++ .../grpc/tests/grpc_motor_service_test.cpp | 20 +- .../service/grpc/tests/grpc_security_test.cpp | 294 ++ .../grpc/tests/grpc_system_service_test.cpp | 455 ++-- .../tests/media_activity_coordinator_test.cpp | 17 + .../tests/motor_activity_coordinator_test.cpp | 17 + .../quic_edge/src/quic_edge_service.cpp | 14 + .../include/grpc_server_task.h | 4 + .../grpc_server_task/src/grpc_server_task.cpp | 105 +- .../include/touch_screen_task.h | 22 +- .../src/touch_screen_admission_test.cpp | 98 +- .../src/touch_screen_task.cpp | 195 +- ...evice_safety_control_plane_architecture.md | 1821 +++++++++++++ protos/cmvr/api/common.proto | 63 + protos/cmvr/api/safety_command.proto | 186 ++ protos/cmvr/api/system_command.proto | 19 + protos/cmvr/api/system_service.proto | 4 + .../device_manager_config.proto | 24 + .../grpc_server_config.proto | 40 + 98 files changed, 16911 insertions(+), 1082 deletions(-) create mode 100644 cmvr-es/manager/device_manager/include/device_safety_adapters.h create mode 100644 cmvr-es/manager/device_manager/src/device_safety_adapters.cpp create mode 100644 cmvr-es/manager/media_source_hub/tests/device_media_source_adapter_test.cpp create mode 100644 cmvr-es/manager/safety/CMakeLists.txt create mode 100644 cmvr-es/manager/safety/include/command_ledger.h create mode 100644 cmvr-es/manager/safety/include/device_safety_endpoint.h create mode 100644 cmvr-es/manager/safety/include/safety_coordinator.h create mode 100644 cmvr-es/manager/safety/include/safety_participant.h create mode 100644 cmvr-es/manager/safety/include/safety_reason.h create mode 100644 cmvr-es/manager/safety/include/safety_snapshot_store.h create mode 100644 cmvr-es/manager/safety/include/safety_types.h create mode 100644 cmvr-es/manager/safety/src/command_ledger.cpp create mode 100644 cmvr-es/manager/safety/src/safety_coordinator.cpp create mode 100644 cmvr-es/manager/safety/src/safety_reason.cpp create mode 100644 cmvr-es/manager/safety/src/safety_snapshot_store.cpp create mode 100644 cmvr-es/manager/safety/tests/command_ledger_test.cpp create mode 100644 cmvr-es/manager/safety/tests/safety_coordinator_test.cpp create mode 100644 cmvr-es/manager/safety/tests/safety_snapshot_store_test.cpp create mode 100644 cmvr-es/service/grpc/include/grpc_command_transaction.h create mode 100644 cmvr-es/service/grpc/include/grpc_recovery_audit.h create mode 100644 cmvr-es/service/grpc/include/grpc_safety_participants.h create mode 100644 cmvr-es/service/grpc/include/grpc_safety_proto.h create mode 100644 cmvr-es/service/grpc/include/grpc_security.h create mode 100644 cmvr-es/service/grpc/src/grpc_command_transaction.cpp create mode 100644 cmvr-es/service/grpc/src/grpc_recovery_audit.cpp create mode 100644 cmvr-es/service/grpc/src/grpc_safety_participants.cpp create mode 100644 cmvr-es/service/grpc/src/grpc_safety_proto.cpp create mode 100644 cmvr-es/service/grpc/src/grpc_security.cpp create mode 100644 cmvr-es/service/grpc/tests/grpc_command_transaction_test.cpp create mode 100644 cmvr-es/service/grpc/tests/grpc_security_test.cpp create mode 100644 docs/device_safety_control_plane_architecture.md create mode 100644 protos/cmvr/api/safety_command.proto diff --git a/cmvr-es/CMakeLists.txt b/cmvr-es/CMakeLists.txt index e59b4eb0..f98ed533 100644 --- a/cmvr-es/CMakeLists.txt +++ b/cmvr-es/CMakeLists.txt @@ -7,6 +7,7 @@ add_subdirectory(algorithms) add_subdirectory(simulate) add_subdirectory(devices) add_subdirectory(manager/control_authority) +add_subdirectory(manager/safety) add_subdirectory(manager/device_manager) add_subdirectory(service/stop_all) add_subdirectory(manager/media_source_hub) diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index dcb2a5cc..21a78b33 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -4,6 +4,18 @@ device_manager { description: "cmvr edge system version 0.1" init_all_motors_when_no_active_joints: true + # The unified safety coordinator observes all decisions while the legacy + # gates remain authoritative during staged hardware migration. + safety { + mode: SHADOW + stop_all_timeout_ms: 15000 + recovery_timeout_ms: 10000 + command_ledger_result_capacity: 4096 + command_ledger_total_id_capacity: 262144 + event_history_capacity: 2048 + fail_startup_on_missing_control_capability: false + } + devices { id: "mujoco_world" type: DEVICE_TYPE_MUJOCO_WORLD diff --git a/cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt b/cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt index e351f03c..acbe6e6b 100644 --- a/cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt +++ b/cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt @@ -6,6 +6,15 @@ grpc_server { camera_stream_max_pending_frames: 2 camera_stream_max_frame_age_ms: 250 + # Current small-scope deployment intentionally keeps the existing clients + # certificate-free. Recovery remains unavailable over the network. + security { + transport_mode: INSECURE + authentication_mode: DISABLED + recovery_exposure: RECOVERY_DISABLED + allow_insecure_non_loopback: true + } + # The RobotArm adapter is implemented, but remains explicitly closed until # the device itself enables teleop group servo, real hashes are provisioned, # and group-write timing and independent stop behavior pass hardware review. diff --git a/cmvr-es/devices/arm/aubo_arm/README.md b/cmvr-es/devices/arm/aubo_arm/README.md index 05e6d903..55ef34f1 100644 --- a/cmvr-es/devices/arm/aubo_arm/README.md +++ b/cmvr-es/devices/arm/aubo_arm/README.md @@ -106,16 +106,22 @@ cmake --install build - 后端使用独立 SDK RPC 会话持续读取控制器的 `SafetyModeType`、 `RobotModeType` 和硬件急停来源；首次有效样本前、监控断线或样本过期时， 所有 Move、Speed、Servo 和程序启动请求均按不安全状态拒绝； -- 硬件急停、防护停机、Safety Fault/Violation 会锁存安全事件，并使当前运动 - generation 失效。控制器重新报告 `Normal`/`ReducedMode` 不会自动解除锁存； -- 锁存后会终止直接运动与程序、关闭 servo 模式并清理控制器轨迹。只有确认 - `ExecId == -1`、普通队列和轨迹队列均为空、运行时已停止且机械臂稳定后， - 显式 `torqueOn`/`clearFault`/`unlockProtectiveStop` 才可能恢复运动权限； +- 硬件急停会立即使当前运动 generation 失效，并在急停输入有效期间保持锁存。 + 检测到硬件急停输入消失且控制器重新报告 `Normal`/`ReducedMode` 后，后端应 + 自动执行安全恢复确认；防护停机和 Safety Fault/Violation 仍保持显式恢复语义； +- `emergencyStop()` 使用独立的 `SoftwareEmergencyStop` 锁存。即使软件急停在真实 + 硬件急停有效期间触发，后续硬件采样也不能覆盖该锁存，释放硬件急停开关不会 + 自动清除软件急停；它只能通过显式安全恢复流程解除； +- 锁存后会终止直接运动与程序、关闭 servo 模式并清理控制器轨迹。硬件急停 + 自动恢复只有在确认 `ExecId == -1`、普通队列和轨迹队列均为空、运行时已停止 + 且机械臂稳定后才能解除锁存；如果自动确认失败，则继续保持 fail-closed， + 并允许通过 `torqueOn`/`clearFault`/`unlockProtectiveStop` 显式重试恢复； - 恢复流程不会调用 `resume`、`arbitraryResume`、`startMove`，也不会重新提交 急停前的目标、速度、servo 指令或程序； - AUBO SDK 未在本地文档中保证急停期间 `clearPath` 的可用性，也未说明释放 - 急停开关后的控制器恢复时序。因此本实现保持 fail-closed 并在释放后再次清队列， - 但“释放开关后零位移”的最终保证仍需真机验证及控制器侧安全配置配合； + 急停开关后的控制器恢复时序。因此自动恢复必须在释放后再次清队列并完成上述 + 安全确认；无法确认时不得解除锁存。“释放开关后零位移”的最终保证仍需真机 + 验证及控制器侧安全配置配合； - 只访问控制柜 Standard 数字 IO，不访问工具端 IO、可配置 IO 或安全 IO； - `set_do` 不修改输出 runstate； - 只有 `StandardOutputRunState::None` 的通道允许写入，否则返回 diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index 63bbd9e6..d92619e6 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -174,6 +174,18 @@ public: return true; } + bool completeHardwareEmergencyStop( + const bool controller_idle, + const bool cancellation_confirmed) + { + if (!state_->completeHardwareEmergencyStopRecovery( + token_, controller_idle, cancellation_confirmed)) { + return false; + } + completed_ = true; + return true; + } + private: std::shared_ptr state_; aubo_internal::RecoveryToken token_; @@ -304,6 +316,8 @@ const char* safetyConditionName( return "SystemEmergencyStop"; case Condition::RobotEmergencyStop: return "RobotEmergencyStop"; + case Condition::SoftwareEmergencyStop: + return "SoftwareEmergencyStop"; case Condition::Fault: return "Fault"; case Condition::Unknown: @@ -328,6 +342,7 @@ SafetyMode publicSafetyMode( case Condition::SystemEmergencyStop: return SafetyMode::SystemEmergencyStop; case Condition::RobotEmergencyStop: + case Condition::SoftwareEmergencyStop: return SafetyMode::EmergencyStop; case Condition::Violation: case Condition::Fault: @@ -376,6 +391,7 @@ struct AuboSafetyMonitor final { std::atomic runtime_state{ static_cast(RuntimeState::Stopped)}; std::atomic emergency_stop_source{-1}; + std::atomic hardware_emergency_stop_latched{false}; std::atomic servo_mode_select{0}; std::atomic last_sample_ns{0}; std::atomic cancellation_confirmed{true}; @@ -449,6 +465,12 @@ void publishSafetySample( monitor->servo_mode_select.store(servo_mode_select); monitor->last_sample_ns.store(monotonicNowNs()); + if (emergency_stop_source != 0) { + monitor->hardware_emergency_stop_latched.store(true); + } else if (!current.latched) { + monitor->hardware_emergency_stop_latched.store(false); + } + if (previous.observed != condition || (!previous.latched && current.latched)) { if (current.latched) { @@ -866,7 +888,56 @@ void runSafetyMonitor( refreshSafetySample( rpc_client, monitor, robot_interface); - if (monitor->safety_state->snapshot().latched) { + auto safety = monitor->safety_state->snapshot(); + const bool hardware_estop_released = + aubo_internal:: + shouldAutoRecoverHardwareEmergencyStop( + safety, + monitor + ->hardware_emergency_stop_latched + .load(), + monitor->emergency_stop_source.load()); + if (hardware_estop_released) { + const auto token = + monitor->safety_state->beginRecovery( + safety.epoch); + if (token.has_value()) { + SafetyRecoveryGuard recovery{ + monitor->safety_state, *token}; + cancelForSafetyTransition(monitor); + const bool terminated = + enforceControllerTermination( + rpc_client, monitor); + refreshSafetySample( + rpc_client, monitor, robot_interface); + const bool controller_idle = + terminated && + monitor->emergency_stop_source.load() == + 0 && + aubo_internal::isMotionSafe( + monitor->safety_state->snapshot() + .observed) && + controllerStillQuiescent( + rpc_client, robot_interface); + if (recovery.completeHardwareEmergencyStop( + controller_idle, + monitor->cancellation_confirmed + .load())) { + monitor->hardware_emergency_stop_latched + .store(false); + CMVR_LOG(INFO) + << "[AuboArm] hardware emergency-stop release safely reconciled, id=" + << monitor->arm_id; + } else { + CMVR_LOG(WARNING) + << "[AuboArm] hardware emergency-stop release remains latched because quiescence could not be confirmed, id=" + << monitor->arm_id; + } + } + safety = monitor->safety_state->snapshot(); + } + + if (safety.latched) { if (monitor->cancellation_confirmed.load() && !controllerStillQuiescent( rpc_client, robot_interface)) { @@ -1987,7 +2058,7 @@ Result AuboArm::emergencyStop() std::lock_guard lock(mutex_); if (sdk_ && sdk_->safety_monitor) { sdk_->safety_monitor->safety_state->observe( - aubo_internal::SafetyCondition::RobotEmergencyStop); + aubo_internal::SafetyCondition::SoftwareEmergencyStop); cancelForSafetyTransition(sdk_->safety_monitor); } } @@ -3625,7 +3696,9 @@ Result AuboArm::ensureMotionReady_( if (condition == aubo_internal::SafetyCondition::RobotEmergencyStop || condition == - aubo_internal::SafetyCondition::SystemEmergencyStop) { + aubo_internal::SafetyCondition::SystemEmergencyStop || + condition == + aubo_internal::SafetyCondition::SoftwareEmergencyStop) { code = ArmErrorCode::RobotInEmergencyStop; } else if ( condition == aubo_internal::SafetyCondition::ProtectiveStop || 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 8c13b4d8..54b5e750 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h @@ -19,6 +19,7 @@ enum class SafetyCondition { SafeguardStop, SystemEmergencyStop, RobotEmergencyStop, + SoftwareEmergencyStop, Fault, }; @@ -74,8 +75,22 @@ struct SafetySnapshot { std::uint64_t epoch{0}; bool latched{false}; bool recovery_in_progress{false}; + bool software_emergency_stop_latched{false}; }; +inline bool shouldAutoRecoverHardwareEmergencyStop( + const SafetySnapshot& snapshot, + const bool hardware_emergency_stop_was_observed, + const int current_emergency_stop_source) noexcept +{ + return hardware_emergency_stop_was_observed && snapshot.latched && + !snapshot.recovery_in_progress && + !snapshot.software_emergency_stop_latched && + snapshot.latched_reason == SafetyCondition::RobotEmergencyStop && + isMotionSafe(snapshot.observed) && + current_emergency_stop_source == 0; +} + // 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 @@ -89,6 +104,9 @@ public: std::lock_guard lock(mutex_); const bool changed = observed_ != condition; observed_ = condition; + if (condition == SafetyCondition::SoftwareEmergencyStop) { + software_emergency_stop_latched_ = true; + } if (isMotionSafe(condition)) { return; } @@ -98,7 +116,12 @@ public: } latched_ = true; recovery_in_progress_ = false; - latched_reason_ = condition; + // 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; } std::optional tryPermit() const @@ -144,6 +167,31 @@ public: return false; } + latched_ = false; + recovery_in_progress_ = false; + latched_reason_ = SafetyCondition::Unknown; + software_emergency_stop_latched_ = false; + ++epoch_; + return true; + } + + // Hardware E-stop release may clear only this software latch. It does not + // power on, release brakes, resume runtime, or issue a motion command. + bool completeHardwareEmergencyStopRecovery( + const RecoveryToken token, + const bool controller_idle, + const bool cancellation_confirmed) + { + std::lock_guard lock(mutex_); + if (!token.valid() || token.epoch != epoch_ || !latched_ || + !recovery_in_progress_ || + software_emergency_stop_latched_ || + latched_reason_ != SafetyCondition::RobotEmergencyStop || + !isMotionSafe(observed_) || !controller_idle || + !cancellation_confirmed) { + return false; + } + latched_ = false; recovery_in_progress_ = false; latched_reason_ = SafetyCondition::Unknown; @@ -167,7 +215,8 @@ public: latched_reason_, epoch_, latched_, - recovery_in_progress_}; + recovery_in_progress_, + software_emergency_stop_latched_}; } private: @@ -177,6 +226,7 @@ private: std::uint64_t epoch_{1}; bool latched_{false}; bool recovery_in_progress_{false}; + bool software_emergency_stop_latched_{false}; }; } // namespace cmvr::device::aubo_internal 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 b38fc6c2..8ae17007 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 @@ -48,10 +48,15 @@ int main() CHECK_TRUE(state.snapshot().latched); CHECK_TRUE(!state.beginRecovery(state.snapshot().epoch).has_value()); - // Releasing the hardware switch must not unlock motion by itself. + // The observed level alone does not unlock motion. The monitor must first + // prove the old controller operation is fully quiescent. state.observe(SafetyCondition::Normal); CHECK_TRUE(state.snapshot().latched); CHECK_TRUE(!state.tryPermit().has_value()); + CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop( + state.snapshot(), false, 0)); + CHECK_TRUE(shouldAutoRecoverHardwareEmergencyStop( + state.snapshot(), true, 0)); const auto recovery = state.beginRecovery(state.snapshot().epoch); CHECK_TRUE(recovery.has_value()); @@ -60,21 +65,54 @@ int main() const auto retry = state.beginRecovery(state.snapshot().epoch); CHECK_TRUE(retry.has_value()); - CHECK_TRUE(state.completeRecovery(*retry, true, true, true)); + CHECK_TRUE(state.completeHardwareEmergencyStopRecovery( + *retry, true, true)); const auto recovered_permit = state.tryPermit(); CHECK_TRUE(recovered_permit.has_value()); CHECK_TRUE(state.validate(*recovered_permit)); + // 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); + state.observe(SafetyCondition::SoftwareEmergencyStop); + // The hardware monitor continues publishing the physical E-stop level + // until the switch is released. It must not overwrite the software latch. + state.observe(SafetyCondition::RobotEmergencyStop); + state.observe(SafetyCondition::Normal); + CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop( + state.snapshot(), true, 0)); + CHECK_TRUE(state.snapshot().software_emergency_stop_latched); + CHECK_TRUE(state.snapshot().latched_reason == + SafetyCondition::SoftwareEmergencyStop); + const auto software_recovery = state.beginRecovery( + state.snapshot().epoch); + CHECK_TRUE(software_recovery.has_value()); + CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery( + *software_recovery, true, true)); + state.failRecovery(*software_recovery); + const auto explicit_software_recovery = state.beginRecovery( + state.snapshot().epoch); + CHECK_TRUE(explicit_software_recovery.has_value()); + CHECK_TRUE(state.completeRecovery( + *explicit_software_recovery, true, true, true)); + CHECK_TRUE(!state.snapshot().software_emergency_stop_latched); + // 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()); + CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery( + *stale_recovery, true, true)); + state.failRecovery(*stale_recovery); + const auto explicit_recovery = state.beginRecovery( + state.snapshot().epoch); + CHECK_TRUE(explicit_recovery.has_value()); state.observe(SafetyCondition::SafeguardStop); state.observe(SafetyCondition::Normal); CHECK_TRUE(!state.completeRecovery( - *stale_recovery, true, true, true)); + *explicit_recovery, true, true, true)); CHECK_TRUE(state.snapshot().latched); // An old API call must not begin recovery for a newer safety event. diff --git a/cmvr-es/manager/device_manager/CMakeLists.txt b/cmvr-es/manager/device_manager/CMakeLists.txt index 180e9638..67029d2e 100644 --- a/cmvr-es/manager/device_manager/CMakeLists.txt +++ b/cmvr-es/manager/device_manager/CMakeLists.txt @@ -1,5 +1,6 @@ add_library(device_manager STATIC src/device_factory.cpp + src/device_safety_adapters.cpp src/device_manager.cpp ) @@ -7,6 +8,7 @@ target_include_directories(device_manager PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(device_manager PRIVATE cmvr_es::proto + cmvr_es::safety_coordinator cmvr_es::device::camera cmvr_es::device::agv cmvr_es::device::speaker diff --git a/cmvr-es/manager/device_manager/include/device_manager.h b/cmvr-es/manager/device_manager/include/device_manager.h index 259e4429..1a353457 100644 --- a/cmvr-es/manager/device_manager/include/device_manager.h +++ b/cmvr-es/manager/device_manager/include/device_manager.h @@ -15,6 +15,7 @@ #include "device_factory.h" #include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "manager/safety/include/safety_coordinator.h" namespace cmvr::device { @@ -47,6 +48,15 @@ namespace cmvr::device { std::vector inventorySnapshot() const; DeviceManagerSnapshot snapshot() const; + safety::SafetyCoordinator& safetyCoordinator() noexcept + { + return *safety_coordinator_; + } + const safety::SafetyCoordinator& safetyCoordinator() const noexcept + { + return *safety_coordinator_; + } + std::string version() const; std::string name() const; std::string description() const; @@ -64,6 +74,7 @@ namespace cmvr::device { std::unordered_map devices_; std::unordered_map device_statuses_; std::unique_ptr dev_factory_; + std::unique_ptr safety_coordinator_; bool initialized_{false}; explicit DeviceManager(const config::DeviceManagerConfig &cfg); @@ -76,6 +87,13 @@ namespace cmvr::device { void update_device_status_(const std::string& device_id, ManagedDeviceState state, const std::string& error_message = {}); + DeviceHealthSnapshot sample_device_health_( + const std::shared_ptr& device) const; + void update_device_health_(const std::string& device_id, + DeviceHealthSnapshot health); + bool register_device_safety_( + const std::shared_ptr& device, + const config::DeviceConfigEntry* config_entry = nullptr); void stop_devices_(bool update_status = true); }; } // cmvr diff --git a/cmvr-es/manager/device_manager/include/device_safety_adapters.h b/cmvr-es/manager/device_manager/include/device_safety_adapters.h new file mode 100644 index 00000000..346cd10d --- /dev/null +++ b/cmvr-es/manager/device_manager/include/device_safety_adapters.h @@ -0,0 +1,18 @@ +#pragma once + +#include +#include + +#include "devices/abstract_device.h" +#include "manager/safety/include/safety_participant.h" + +namespace cmvr::device { + +safety::DeviceSafetyRegistration makeDeviceSafetyRegistration( + const std::shared_ptr& device, + std::chrono::milliseconds configured_maximum_age = + std::chrono::milliseconds::zero(), + std::chrono::milliseconds configured_stop_timeout = + std::chrono::milliseconds::zero()); + +} // namespace cmvr::device diff --git a/cmvr-es/manager/device_manager/src/device_manager.cpp b/cmvr-es/manager/device_manager/src/device_manager.cpp index 6856d03e..c10ea258 100644 --- a/cmvr-es/manager/device_manager/src/device_manager.cpp +++ b/cmvr-es/manager/device_manager/src/device_manager.cpp @@ -4,6 +4,7 @@ // #include "../include/device_manager.h" +#include "../include/device_safety_adapters.h" #include #include @@ -35,6 +36,68 @@ namespace { using GroupJointSelection = std::unordered_map>; using MotorJointSelections = std::unordered_map; constexpr std::size_t kMaxDeviceErrorLength = 512; +constexpr auto kSafetyStartupValidationTimeout = std::chrono::seconds(2); + +cmvr::safety::SafetyCoordinatorConfig safetyConfigFrom( + const cmvr::config::DeviceManagerConfig& config) +{ + cmvr::safety::SafetyCoordinatorConfig result; + if (!config.has_safety()) { + result.enforcement_mode = cmvr::safety::EnforcementMode::Shadow; + return result; + } + + const auto& source = config.safety(); + switch (source.mode()) { + case cmvr::config::SafetyCoordinatorConfig::LEGACY: + result.enforcement_mode = cmvr::safety::EnforcementMode::Legacy; + break; + case cmvr::config::SafetyCoordinatorConfig::ENFORCE_SELECTED: + result.enforcement_mode = + cmvr::safety::EnforcementMode::EnforceSelected; + break; + case cmvr::config::SafetyCoordinatorConfig::ENFORCE_ALL: + result.enforcement_mode = cmvr::safety::EnforcementMode::EnforceAll; + break; + case cmvr::config::SafetyCoordinatorConfig::SHADOW: + case cmvr::config::SafetyCoordinatorConfig::ENFORCEMENT_MODE_UNSPECIFIED: + default: + result.enforcement_mode = cmvr::safety::EnforcementMode::Shadow; + break; + } + for (const auto& id : source.enforced_device_ids()) { + if (!id.empty()) { + result.enforced_device_ids.insert(id); + } + } + for (const auto& entry : config.devices()) { + if (entry.safety_enforce() && !entry.id().empty()) { + result.enforced_device_ids.insert(entry.id()); + } + } + if (source.stop_all_timeout_ms() != 0) { + result.stop_all_timeout = + std::chrono::milliseconds(source.stop_all_timeout_ms()); + } + if (source.recovery_timeout_ms() != 0) { + result.recovery_timeout = + std::chrono::milliseconds(source.recovery_timeout_ms()); + } + if (source.command_ledger_result_capacity() != 0) { + result.command_ledger.result_capacity = + source.command_ledger_result_capacity(); + } + if (source.command_ledger_total_id_capacity() != 0) { + result.command_ledger.total_id_capacity = + source.command_ledger_total_id_capacity(); + } + if (source.event_history_capacity() != 0) { + result.event_history_capacity = source.event_history_capacity(); + } + result.fail_startup_on_missing_control_capability = + source.fail_startup_on_missing_control_capability(); + return result; +} std::uint64_t unixTimeMs() noexcept { @@ -170,10 +233,12 @@ std::shared_ptr DeviceManager::instance_ = nullptr; std::mutex DeviceManager::init_mutex_; -DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) { - cfg_ = cfg; - - dev_factory_ = std::make_unique(); +DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) + : cfg_(cfg), + dev_factory_(std::make_unique()), + safety_coordinator_(std::make_unique( + safetyConfigFrom(cfg))) +{ initialize_device_statuses_(); logSection("Device Plan"); log_device_plan_(); @@ -248,6 +313,7 @@ bool DeviceManager::start(){ bool started = false; std::string error_message; try { + (void)safety_coordinator_->advanceDeviceGeneration(id); started = device->start(); if (!started) { error_message = "device start returned false: " + id; @@ -264,6 +330,8 @@ bool DeviceManager::start(){ << " threw an unknown exception"; } if (started) { + const auto health = sample_device_health_(device); + update_device_health_(id, health); update_device_status_(id, ManagedDeviceState::Running); CMVR_LOG(INFO) << "[DeviceManager]: Start device " << id << " Success"; } else { @@ -273,13 +341,29 @@ bool DeviceManager::start(){ all_started = false; } } + if (all_started) { + const auto coverage = safety_coordinator_->validateStartupCoverage( + safety::SafetyClock::now() + kSafetyStartupValidationTimeout); + if (!coverage.ready) { + all_started = false; + for (const auto& issue : coverage.issues) { + CMVR_LOG(ERROR) + << "[DeviceManager]: Safety startup coverage failed" + << ", target=" << issue.target_id + << ", reason=" << safety::toString(issue.reason) + << ", detail=" << issue.detail; + } + } + } if (!all_started) { - CMVR_LOG(ERROR) << "[DeviceManager]: At least one enabled device failed " - "to start; stopping all devices"; + CMVR_LOG(ERROR) << "[DeviceManager]: Device or safety startup failed; " + "stopping all devices"; // Rollback is a physical cleanup operation. Preserve the start // results in the status table so the failure is diagnosable; an // explicit stop() records Stopped/Error transitions. stop_devices_(false); + } else { + safety_coordinator_->markStartupComplete(); } return all_started; } @@ -338,12 +422,18 @@ void DeviceManager::stop_devices_(const bool update_status) { update_device_status_(id, ManagedDeviceState::Stopped); } CMVR_LOG(INFO) << "[DeviceManager]: Stop device " << id << " Success"; + safety_coordinator_->updateDeviceRuntimeState( + id, ManagedDeviceState::Stopped, + sample_device_health_(device)); } else { if (update_status) { update_device_status_( id, ManagedDeviceState::Error, error_message); } CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id << " Failed"; + safety_coordinator_->updateDeviceRuntimeState( + id, ManagedDeviceState::Error, + {DeviceHealthState::Fault, error_message}); } } } @@ -445,6 +535,16 @@ void DeviceManager::registerDevice(const std::string& device_id, devices_.emplace(record.id, std::move(record)); device_statuses_[device_id] = std::move(status); } + if (!register_device_safety_(device)) { + update_device_status_( + device_id, ManagedDeviceState::Error, + "failed to register device safety capability: " + device_id); + CMVR_LOG(ERROR) << "[DeviceManager]: Failed to register device safety " + "capability, id=" << device_id; + } else { + update_device_health_( + device_id, sample_device_health_(device)); + } CMVR_LOG(INFO) << "[DeviceManager]: Register device success" << ", id=" << device_id << ", type=" << device->typeName() @@ -509,48 +609,43 @@ void DeviceManager::update_device_status_( const ManagedDeviceState state, const std::string& error_message) { - std::unique_lock lock(devices_mutex_); - auto& status = device_statuses_[device_id]; - if (status.id.empty()) { - status.id = device_id; + DeviceHealthSnapshot health; + { + std::unique_lock lock(devices_mutex_); + auto& status = device_statuses_[device_id]; + if (status.id.empty()) { + status.id = device_id; + } + const auto device_it = devices_.find(device_id); + if (device_it != devices_.end()) { + status.kind = device_it->second.kind; + status.type_name = device_it->second.type_name; + } + status.enabled = true; + status.state = state; + status.abnormal = state == ManagedDeviceState::Error; + status.error_message = + state == ManagedDeviceState::Error + ? truncateDeviceError( + error_message.empty() + ? "device lifecycle operation failed: " + device_id + : error_message) + : std::string{}; + status.status_updated_at_unix_ms = unixTimeMs(); + health = status.health; } - const auto device_it = devices_.find(device_id); - if (device_it != devices_.end()) { - status.kind = device_it->second.kind; - status.type_name = device_it->second.type_name; - } - status.enabled = true; - status.state = state; - status.abnormal = state == ManagedDeviceState::Error; - status.error_message = - state == ManagedDeviceState::Error - ? truncateDeviceError( - error_message.empty() - ? "device lifecycle operation failed: " + device_id - : error_message) - : std::string{}; - status.status_updated_at_unix_ms = unixTimeMs(); + safety_coordinator_->updateDeviceRuntimeState(device_id, state, health); } DeviceManagerSnapshot DeviceManager::snapshot() const { - struct SnapshotSource { - ManagedDeviceSnapshot status; - std::shared_ptr device; - }; - - std::vector sources; + std::vector sources; { std::shared_lock lock(devices_mutex_); sources.reserve(device_statuses_.size()); for (const auto& [id, stored_status] : device_statuses_) { - SnapshotSource source; - source.status = stored_status; - const auto device_it = devices_.find(id); - if (device_it != devices_.end()) { - source.device = device_it->second.device; - } - sources.push_back(std::move(source)); + (void)id; + sources.push_back(stored_status); } } @@ -561,36 +656,22 @@ DeviceManagerSnapshot DeviceManager::snapshot() const result.devices.reserve(sources.size()); for (auto& source : sources) { - if (source.device) { - try { - source.status.health = source.device->healthSnapshot(); - } catch (const std::exception& error) { - source.status.health.state = DeviceHealthState::Fault; - source.status.health.error_message = error.what(); - } catch (...) { - source.status.health.state = DeviceHealthState::Fault; - source.status.health.error_message = - "device health snapshot threw an unknown exception"; - } - } - source.status.health.error_message = - truncateDeviceError(source.status.health.error_message); + source.health.error_message = + truncateDeviceError(source.health.error_message); const bool lifecycle_error = - source.status.state == ManagedDeviceState::Error; + source.state == ManagedDeviceState::Error; const bool health_error = - source.status.health.state == DeviceHealthState::Degraded || - source.status.health.state == DeviceHealthState::Fault; - source.status.abnormal = lifecycle_error || health_error; - if (source.status.error_message.empty()) { - source.status.error_message = - source.status.health.error_message; + source.health.state == DeviceHealthState::Degraded || + source.health.state == DeviceHealthState::Fault; + source.abnormal = lifecycle_error || health_error; + if (source.error_message.empty()) { + source.error_message = source.health.error_message; } - source.status.error_message = - truncateDeviceError(source.status.error_message); - if (source.status.status_updated_at_unix_ms == 0) { - source.status.status_updated_at_unix_ms = unixTimeMs(); + source.error_message = truncateDeviceError(source.error_message); + if (source.status_updated_at_unix_ms == 0) { + source.status_updated_at_unix_ms = unixTimeMs(); } - result.devices.push_back(std::move(source.status)); + result.devices.push_back(std::move(source)); } std::sort(result.devices.begin(), result.devices.end(), @@ -601,6 +682,78 @@ DeviceManagerSnapshot DeviceManager::snapshot() const return result; } +DeviceHealthSnapshot DeviceManager::sample_device_health_( + const std::shared_ptr& device) const +{ + if (!device) { + return { + DeviceHealthState::Fault, + "device health target is null"}; + } + try { + auto health = device->healthSnapshot(); + health.error_message = truncateDeviceError(health.error_message); + return health; + } catch (const std::exception& error) { + return {DeviceHealthState::Fault, truncateDeviceError(error.what())}; + } catch (...) { + return { + DeviceHealthState::Fault, + "device health snapshot threw an unknown exception"}; + } +} + +void DeviceManager::update_device_health_( + const std::string& device_id, + DeviceHealthSnapshot health) +{ + ManagedDeviceState lifecycle = ManagedDeviceState::Unknown; + { + std::unique_lock lock(devices_mutex_); + auto& status = device_statuses_[device_id]; + status.health = std::move(health); + lifecycle = status.state; + const bool health_error = + status.health.state == DeviceHealthState::Degraded || + status.health.state == DeviceHealthState::Fault; + status.abnormal = + status.state == ManagedDeviceState::Error || health_error; + if (status.state != ManagedDeviceState::Error) { + status.error_message = status.health.error_message; + } + status.status_updated_at_unix_ms = unixTimeMs(); + health = status.health; + } + safety_coordinator_->updateDeviceRuntimeState( + device_id, lifecycle, std::move(health)); +} + +bool DeviceManager::register_device_safety_( + const std::shared_ptr& device, + const config::DeviceConfigEntry* config_entry) +{ + auto maximum_age = std::chrono::milliseconds::zero(); + auto stop_timeout = std::chrono::milliseconds::zero(); + if (config_entry) { + if (config_entry->maximum_safety_snapshot_age_ms() != 0) { + maximum_age = std::chrono::milliseconds( + config_entry->maximum_safety_snapshot_age_ms()); + } + if (config_entry->safety_stop_timeout_ms() != 0) { + stop_timeout = std::chrono::milliseconds( + config_entry->safety_stop_timeout_ms()); + } + } + auto registration = makeDeviceSafetyRegistration( + device, maximum_age, stop_timeout); + if (registration.descriptor.device_id.empty()) { + return false; + } + const bool registered = + safety_coordinator_->registerDevice(std::move(registration)); + return registered; +} + std::string DeviceManager::version() const { return cfg_.version().empty() ? "1.0" : cfg_.version(); } @@ -914,6 +1067,8 @@ bool DeviceManager::init_devices_() { << ", type=" << record.type_name << ", kind=" << toString(record.kind) << ", config_file=" << entry.config_file(); + const auto registered_device = record.device; + std::string registered_id; { std::unique_lock lock(devices_mutex_); const auto id = record.id; @@ -941,6 +1096,23 @@ bool DeviceManager::init_devices_() { status.abnormal = false; status.error_message.clear(); status.status_updated_at_unix_ms = unixTimeMs(); + registered_id = id; + } + + if (!register_device_safety_(registered_device, &entry)) { + update_device_status_( + registered_id, ManagedDeviceState::Error, + "failed to register device safety capability: " + + registered_id); + CMVR_LOG(ERROR) + << "[DeviceManager]: Failed to register device safety " + "capability" + << ", id=" << registered_id + << ", kind=" << toString(registered_device->kind()); + all_initialized = false; + } else { + update_device_health_( + registered_id, sample_device_health_(registered_device)); } } return all_initialized; diff --git a/cmvr-es/manager/device_manager/src/device_safety_adapters.cpp b/cmvr-es/manager/device_manager/src/device_safety_adapters.cpp new file mode 100644 index 00000000..6486c2fa --- /dev/null +++ b/cmvr-es/manager/device_manager/src/device_safety_adapters.cpp @@ -0,0 +1,1022 @@ +#include "manager/device_manager/include/device_safety_adapters.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "devices/agv/abstract_agv.h" +#include "devices/arm/robot_arm.h" +#include "devices/battery/abstract_battery.h" +#include "devices/biohead/abstract_biohead.h" +#include "devices/camera/abstract_camera.h" +#include "devices/dexhand/abstract_dexhand.h" +#include "devices/gripper/abstract_gripper.h" +#include "devices/microphone/abstract_microphone.h" +#include "devices/motor/manager/include/motor_manager.h" +#include "devices/speaker/abstract_speaker.h" +#include "manager/control_authority/include/control_authority_manager.h" +#include "manager/safety/include/device_safety_endpoint.h" + +namespace cmvr::device { + +namespace { + +using safety::AdmissionPermit; +using safety::BarrierToken; +using safety::BlockerScope; +using safety::CommandIntent; +using safety::DeviceSafetyDescriptor; +using safety::DeviceSafetyEndpoint; +using safety::DeviceSafetySnapshot; +using safety::HardwareCheckResult; +using safety::ParticipantDescriptor; +using safety::ParticipantPhase; +using safety::ParticipantResult; +using safety::RecoveryCheckResult; +using safety::RecoveryContext; +using safety::RecoveryRequirement; +using safety::SafetyBlocker; +using safety::SafetyClock; +using safety::SafetyCondition; +using safety::SafetyOperationContext; +using safety::SafetyParticipant; +using safety::SafetyPolicyFamily; +using safety::SafetyReason; +using safety::SafetySnapshotPublisher; +using safety::TriState; + +using Probe = std::function; +using Operation = std::function; + +TriState tri(const bool value) noexcept +{ + return value ? TriState::True : TriState::False; +} + +bool agvStopResultAccepted(const AgvResult& result) noexcept +{ + return result.ok() || result.code == AgvErrorCode::UnsupportedCommand; +} + +SafetyBlocker blocker( + const SafetyReason reason, + const std::string& source) +{ + return { + reason, + BlockerScope::Device, + RecoveryRequirement::HardwareReleaseRequired, + source, + {}, + 0, + 0}; +} + +DeviceSafetySnapshot baseSnapshot( + const std::string& id, + const std::uint64_t generation, + const std::uint64_t sequence) +{ + DeviceSafetySnapshot snapshot; + snapshot.device_id = id; + snapshot.device_generation = generation; + snapshot.sample_sequence = sequence; + snapshot.observed_at = SafetyClock::now(); + return snapshot; +} + +DeviceSafetyDescriptor defaultDescriptor(const AbstractDevice& device) +{ + DeviceSafetyDescriptor descriptor; + descriptor.device_id = device.id(); + descriptor.kind = device.kind(); + descriptor.maximum_snapshot_age = std::chrono::milliseconds(1000); + switch (device.kind()) { + case DeviceKind::AGV: + case DeviceKind::Arm: + case DeviceKind::BioHead: + case DeviceKind::DexHand: + case DeviceKind::Gripper: + case DeviceKind::Motor: + case DeviceKind::MotorSystem: + case DeviceKind::MujocoWorld: + case DeviceKind::Robot: + descriptor.default_policy = SafetyPolicyFamily::Control; + descriptor.requires_safe_stop = true; + descriptor.maximum_snapshot_age = std::chrono::milliseconds(500); + break; + case DeviceKind::Camera: + case DeviceKind::Microphone: + case DeviceKind::Speaker: + descriptor.default_policy = SafetyPolicyFamily::Sensor; + descriptor.requires_safe_stop = true; + break; + case DeviceKind::Battery: + case DeviceKind::CanBus: + case DeviceKind::MujocoViewer: + case DeviceKind::Unknown: + descriptor.default_policy = SafetyPolicyFamily::Sensor; + descriptor.requires_safe_stop = false; + break; + } + descriptor.supports_active_refresh = true; + return descriptor; +} + +class LegacyDeviceSafetyEndpoint final : public DeviceSafetyEndpoint { +public: + LegacyDeviceSafetyEndpoint(DeviceSafetyDescriptor descriptor, Probe probe) + : descriptor_(std::move(descriptor)), + probe_(std::move(probe)), + poll_interval_(std::clamp( + descriptor_.maximum_snapshot_age / 2, + std::chrono::milliseconds(50), + std::chrono::milliseconds(500))), + worker_([this] { workerLoop_(); }) + { + } + + ~LegacyDeviceSafetyEndpoint() override + { + { + std::lock_guard lock(mutex_); + stopping_ = true; + refresh_requested_ = true; + } + refresh_.notify_all(); + if (worker_.joinable()) { + worker_.join(); + } + } + + DeviceSafetyDescriptor descriptor() const override + { + return descriptor_; + } + + void bindPublisher(SafetySnapshotPublisher publisher) override + { + std::lock_guard lock(mutex_); + publisher_ = std::move(publisher); + } + + void requestSafetyRefresh() noexcept override + { + try { + { + std::lock_guard lock(mutex_); + refresh_requested_ = true; + } + refresh_.notify_one(); + } catch (...) { + } + } + + void onDeviceGenerationChanged( + const std::uint64_t generation) noexcept override + { + generation_.store(generation, std::memory_order_release); + sequence_.store(0, std::memory_order_release); + requestSafetyRefresh(); + } + + HardwareCheckResult validateBeforeDispatch( + const AdmissionPermit& permit) override + { + if (permit.intent == CommandIntent::Observe || + permit.intent == CommandIntent::Stop || + permit.intent == CommandIntent::ResetFault) { + return {true, SafetyReason::None, {}}; + } + DeviceSafetySnapshot snapshot; + try { + snapshot = sample_(); + } catch (const std::exception& error) { + return {false, SafetyReason::InternalError, error.what()}; + } catch (...) { + return { + false, + SafetyReason::InternalError, + "legacy final hardware check threw an unknown exception"}; + } + if (snapshot.device_generation != permit.device_generation) { + return { + false, + SafetyReason::GenerationMismatch, + "device generation changed during final hardware check"}; + } + if (snapshot.condition == SafetyCondition::Unknown) { + return { + false, + SafetyReason::SafetyStateMissing, + "hardware safety state is unknown at dispatch"}; + } + if (snapshot.condition == SafetyCondition::Unsafe) { + return { + false, + SafetyReason::HardwareUnsafe, + "hardware safety state is unsafe at dispatch"}; + } + if (snapshot.connected != TriState::True) { + return { + false, + SafetyReason::DeviceDisconnected, + "device connection is not confirmed at dispatch"}; + } + if (descriptor_.default_policy == SafetyPolicyFamily::Control) { + if (snapshot.emergency_stop_active != TriState::False) { + return { + false, + SafetyReason::EmergencyStopActive, + "emergency stop is active or unknown at dispatch"}; + } + if (snapshot.protective_stop_active != TriState::False) { + return { + false, + SafetyReason::ProtectiveStopActive, + "protective stop is active or unknown at dispatch"}; + } + if (snapshot.fault_active != TriState::False) { + return { + false, + SafetyReason::DeviceFault, + "device fault is active or unknown at dispatch"}; + } + const bool safe_start_from_restricted = + permit.intent == CommandIntent::StartActivity && + snapshot.condition == SafetyCondition::Restricted; + if (snapshot.operational_ready != TriState::True && + !safe_start_from_restricted) { + return { + false, + SafetyReason::DeviceNotReady, + "device is not operationally ready at dispatch"}; + } + } + return {true, SafetyReason::None, {}}; + } + + RecoveryCheckResult reconcileAdmissionState( + const RecoveryContext&) override + { + // The legacy adapter has no operation which powers, enables, or moves + // hardware. A fresh snapshot was already verified by Coordinator. + return {true, SafetyReason::None, {}}; + } + +private: + DeviceSafetySnapshot sample_() + { + const auto generation = generation_.load(std::memory_order_acquire); + const auto sequence = + sequence_.fetch_add(1, std::memory_order_acq_rel) + 1U; + auto snapshot = probe_(generation, sequence); + snapshot.device_id = descriptor_.device_id; + snapshot.device_generation = generation; + snapshot.sample_sequence = sequence; + if (snapshot.observed_at == SafetyClock::time_point{}) { + snapshot.observed_at = SafetyClock::now(); + } + SafetySnapshotPublisher publisher; + { + std::lock_guard lock(mutex_); + publisher = publisher_; + } + if (publisher) { + (void)publisher(snapshot); + } + return snapshot; + } + + void workerLoop_() + { + std::unique_lock lock(mutex_); + while (!stopping_) { + refresh_.wait_for(lock, poll_interval_, [this] { + return stopping_ || refresh_requested_; + }); + if (stopping_) { + return; + } + refresh_requested_ = false; + lock.unlock(); + try { + (void)sample_(); + } catch (...) { + DeviceSafetySnapshot unknown = baseSnapshot( + descriptor_.device_id, + generation_.load(std::memory_order_acquire), + sequence_.fetch_add(1, std::memory_order_acq_rel) + 1U); + unknown.condition = SafetyCondition::Unknown; + unknown.blockers.push_back(blocker( + SafetyReason::SafetyStateMissing, + descriptor_.device_id)); + SafetySnapshotPublisher publisher; + { + std::lock_guard publisher_lock(mutex_); + publisher = publisher_; + } + if (publisher) { + (void)publisher(std::move(unknown)); + } + } + lock.lock(); + } + } + + DeviceSafetyDescriptor descriptor_; + Probe probe_; + const std::chrono::milliseconds poll_interval_; + std::atomic generation_{1}; + std::atomic sequence_{0}; + std::mutex mutex_; + std::condition_variable refresh_; + SafetySnapshotPublisher publisher_; + bool refresh_requested_{true}; + bool stopping_{false}; + std::thread worker_; +}; + +class CallbackSafetyParticipant final : public SafetyParticipant { +public: + CallbackSafetyParticipant( + ParticipantDescriptor descriptor, + bool control_resource, + Operation stop, + Operation verify) + : descriptor_(std::move(descriptor)), + control_resource_(control_resource), + stop_(std::move(stop)), + verify_(std::move(verify)) + { + } + + ParticipantDescriptor descriptor() const override + { + return descriptor_; + } + + BarrierToken beginBarrier( + const SafetyOperationContext& context) override + { + BarrierToken token; + token.participant_id = descriptor_.participant_id; + token.operation_id = context.operation_id; + token.safety_epoch = context.safety_epoch; + token.generation = + generation_.fetch_add(1, std::memory_order_relaxed) + 1U; + + if (!control_resource_) { + return token; + } + const auto acquired = control::ControlAuthorityManager::instance() + .preemptAcquire( + descriptor_.participant_id, + "safety:" + context.operation_id, + std::chrono::hours(24)); + if (!acquired.acquired) { + return {}; + } + try { + std::lock_guard lock(mutex_); + control_barriers_.emplace(token.generation, acquired.token); + } catch (...) { + control::ControlAuthorityManager::instance().release( + acquired.token); + throw; + } + return token; + } + + ParticipantResult requestQuiesce( + const BarrierToken&, + const SafetyOperationContext&) override + { + return stop_ ? stop_() : ParticipantResult{ + false, + SafetyReason::UnsupportedCommand, + "device has no safe-stop operation"}; + } + + ParticipantResult verifyQuiescent( + const BarrierToken& token, + const SafetyOperationContext& context) override + { + bool handler_drained = true; + if (control_resource_) { + control::ControlLeaseToken barrier; + { + std::lock_guard lock(mutex_); + const auto found = control_barriers_.find(token.generation); + if (found == control_barriers_.end()) { + return { + false, + SafetyReason::StopUnconfirmed, + "control barrier is no longer active"}; + } + barrier = found->second; + } + handler_drained = control::ControlAuthorityManager::instance() + .waitForPreemptedRelease( + barrier, + std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>( + std::max( + std::chrono::milliseconds::zero(), + std::chrono::duration_cast< + std::chrono::milliseconds>( + context.deadline - SafetyClock::now())))); + } + + // A preempted handler may have crossed its driver boundary after the + // first stop request. Repeat the typed stop after the dispatch fence + // before trusting the device's final quiescence report. + const auto final_stop = stop_ ? stop_() : ParticipantResult{ + false, + SafetyReason::UnsupportedCommand, + "device has no final safe-stop operation"}; + if (!handler_drained) { + std::string detail = + "preempted control handler did not leave its dispatch fence"; + if (!final_stop.success && !final_stop.detail.empty()) { + detail += "; final stop failed: " + final_stop.detail; + } + return { + false, + SafetyReason::ParticipantTimeout, + std::move(detail)}; + } + if (!final_stop.success) { + return final_stop; + } + return verify_ ? verify_() : ParticipantResult{ + false, + SafetyReason::StopUnconfirmed, + "device has no quiescence verification"}; + } + + RecoveryCheckResult recoverAdmission( + const BarrierToken&, + const RecoveryContext&) override + { + return {true, SafetyReason::None, {}}; + } + + ParticipantResult releaseBarrier( + const BarrierToken& token) noexcept override + { + if (!control_resource_) { + return {true, SafetyReason::None, {}}; + } + try { + control::ControlLeaseToken barrier; + { + std::lock_guard lock(mutex_); + const auto found = control_barriers_.find(token.generation); + if (found == control_barriers_.end()) { + return { + false, + SafetyReason::StopUnconfirmed, + "control barrier is no longer active"}; + } + barrier = found->second; + control_barriers_.erase(found); + } + control::ControlAuthorityManager::instance().release(barrier); + return {true, SafetyReason::None, {}}; + } catch (...) { + return { + false, + SafetyReason::InternalError, + "control barrier release threw an exception"}; + } + } + +private: + ParticipantDescriptor descriptor_; + bool control_resource_{false}; + Operation stop_; + Operation verify_; + std::atomic generation_{0}; + std::mutex mutex_; + std::unordered_map + control_barriers_; +}; + +Probe probeFor(const std::shared_ptr& device) +{ + if (const auto arm = std::dynamic_pointer_cast(device)) { + return [arm](const std::uint64_t generation, + const std::uint64_t sequence) { + auto snapshot = baseSnapshot(arm->id(), generation, sequence); + const auto state = arm->getRobotState(); + snapshot.connected = tri(state.connected); + snapshot.operational_ready = tri( + 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); + snapshot.actuator_enabled = tri(state.powered_on); + snapshot.emergency_stop_active = tri(state.emergency_stopped); + snapshot.protective_stop_active = tri(state.protective_stopped); + snapshot.fault_active = tri(state.fault); + snapshot.condition = !state.connected + ? SafetyCondition::Unknown + : state.fault || state.emergency_stopped || + state.protective_stopped + ? SafetyCondition::Unsafe + : state.powered_on + ? SafetyCondition::Nominal + : SafetyCondition::Restricted; + if (state.emergency_stopped) { + snapshot.blockers.push_back(blocker( + SafetyReason::EmergencyStopActive, arm->id())); + } + if (state.protective_stopped) { + snapshot.blockers.push_back(blocker( + SafetyReason::ProtectiveStopActive, arm->id())); + } + if (state.fault) { + snapshot.blockers.push_back(blocker( + SafetyReason::DeviceFault, arm->id())); + } + return snapshot; + }; + } + + if (const auto agv = std::dynamic_pointer_cast(device)) { + return [agv](const std::uint64_t generation, + const std::uint64_t sequence) { + auto snapshot = baseSnapshot(agv->id(), generation, sequence); + const auto state = agv->runtimeState(); + const auto navigation = agv->navigationStatus(); + const bool active_navigation = + navigation.state == AgvTaskState::Waiting || + navigation.state == AgvTaskState::Running || + navigation.state == AgvTaskState::Paused; + snapshot.connected = tri(state.connected); + snapshot.operational_ready = tri( + state.connected && state.localized && !state.fault && + !state.emergency_stopped); + snapshot.quiescent = tri(!state.moving && !active_navigation); + 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.fault_active = tri(state.fault); + snapshot.condition = !state.connected + ? SafetyCondition::Unknown + : state.fault || state.emergency_stopped + ? SafetyCondition::Unsafe + : SafetyCondition::Nominal; + return snapshot; + }; + } + + if (const auto hand = std::dynamic_pointer_cast(device)) { + return [hand](const std::uint64_t generation, + const std::uint64_t sequence) { + auto snapshot = baseSnapshot(hand->id(), generation, sequence); + const auto state = hand->state(); + const bool connected = + state == AbstractDexHand::Status::INITIALIZED || + state == AbstractDexHand::Status::STREAMING || + state == AbstractDexHand::Status::STOPPED; + snapshot.connected = tri(connected); + snapshot.operational_ready = tri(connected); + snapshot.quiescent = state == AbstractDexHand::Status::STOPPED || + state == AbstractDexHand::Status::INITIALIZED + ? TriState::True + : 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 + ? state == AbstractDexHand::Status::FAULT + ? SafetyCondition::Unsafe + : SafetyCondition::Unknown + : SafetyCondition::Nominal; + return snapshot; + }; + } + + if (const auto motors = std::dynamic_pointer_cast(device)) { + return [motors](const std::uint64_t generation, + const std::uint64_t sequence) { + auto snapshot = baseSnapshot(motors->id(), generation, sequence); + const auto& motor_map = motors->motorsMap(); + if (motor_map.empty()) { + snapshot.condition = SafetyCondition::Unknown; + snapshot.blockers.push_back(blocker( + SafetyReason::SafetyStateMissing, motors->id())); + return snapshot; + } + bool quiescent = true; + for (const auto& [joint, motor] : motor_map) { + (void)joint; + if (!motor) { + snapshot.condition = SafetyCondition::Unknown; + return snapshot; + } + quiescent = quiescent && std::abs(motor->getQd()) < 1e-3; + } + snapshot.connected = TriState::True; + snapshot.operational_ready = TriState::True; + snapshot.quiescent = tri(quiescent); + snapshot.motion_active = tri(!quiescent); + snapshot.condition = SafetyCondition::Nominal; + return snapshot; + }; + } + + if (const auto camera = std::dynamic_pointer_cast(device)) { + return [camera](const std::uint64_t generation, + const std::uint64_t sequence) { + auto snapshot = baseSnapshot(camera->id(), generation, sequence); + CameraState state{}; + camera->getState(state); + snapshot.connected = tri(state.is_initialized && state.is_opened); + snapshot.operational_ready = tri( + state.is_initialized && state.is_opened && !state.is_error); + snapshot.quiescent = tri( + !state.is_streaming && !state.is_recording); + snapshot.motion_active = TriState::Unknown; + snapshot.fault_active = tri(state.is_error); + snapshot.condition = state.is_error + ? SafetyCondition::Unsafe + : state.is_initialized + ? SafetyCondition::Nominal + : SafetyCondition::Unknown; + return snapshot; + }; + } + + if (const auto microphone = + std::dynamic_pointer_cast(device)) { + return [microphone](const std::uint64_t generation, + const std::uint64_t sequence) { + auto snapshot = baseSnapshot( + microphone->id(), generation, sequence); + MicrophoneState state{}; + microphone->getState(state); + snapshot.connected = tri(state.is_initialized); + snapshot.operational_ready = tri( + state.is_initialized && !state.is_error); + snapshot.quiescent = tri(!state.is_recording); + snapshot.fault_active = tri(state.is_error); + snapshot.condition = state.is_error + ? SafetyCondition::Unsafe + : state.is_initialized + ? SafetyCondition::Nominal + : SafetyCondition::Unknown; + return snapshot; + }; + } + + if (const auto speaker = + std::dynamic_pointer_cast(device)) { + return [speaker](const std::uint64_t generation, + const std::uint64_t sequence) { + auto snapshot = baseSnapshot(speaker->id(), generation, sequence); + SpeakerState state{}; + speaker->getState(state); + snapshot.connected = tri(state.is_initialized); + snapshot.operational_ready = tri(state.is_initialized); + snapshot.quiescent = tri( + !state.is_running && !state.is_decoding); + snapshot.condition = state.is_initialized + ? SafetyCondition::Nominal + : SafetyCondition::Unknown; + return snapshot; + }; + } + + if (const auto head = std::dynamic_pointer_cast(device)) { + return [head](const std::uint64_t generation, + const std::uint64_t sequence) { + auto snapshot = baseSnapshot(head->id(), generation, sequence); + RobotState state{}; + head->getState(state); + snapshot.connected = state.error + ? TriState::False + : TriState::True; + 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 + : SafetyCondition::Nominal; + return snapshot; + }; + } + + if (const auto battery = + std::dynamic_pointer_cast(device)) { + return [battery](const std::uint64_t generation, + const std::uint64_t sequence) { + auto snapshot = baseSnapshot(battery->id(), generation, sequence); + BatteryState state{}; + battery->getState(state); + const bool fault = state.health == Lifecycle::ERROR || + state.health == Lifecycle::ESTOP; + snapshot.connected = state.health == Lifecycle::INIT + ? TriState::Unknown + : TriState::True; + snapshot.operational_ready = tri(!fault); + snapshot.quiescent = TriState::True; + snapshot.fault_active = tri(fault); + snapshot.condition = fault + ? SafetyCondition::Unsafe + : state.health == Lifecycle::INIT + ? SafetyCondition::Unknown + : SafetyCondition::Nominal; + return snapshot; + }; + } + + return [id = device->id()](const std::uint64_t generation, + const std::uint64_t sequence) { + auto snapshot = baseSnapshot(id, generation, sequence); + snapshot.condition = SafetyCondition::Unknown; + snapshot.blockers.push_back(blocker( + SafetyReason::SafetyStateMissing, id)); + return snapshot; + }; +} + +std::shared_ptr participantFor( + const std::shared_ptr& device, + const DeviceSafetyDescriptor& safety_descriptor, + const std::chrono::milliseconds timeout) +{ + Operation stop; + Operation verify; + + if (const auto arm = std::dynamic_pointer_cast(device)) { + stop = [arm] { + auto result = arm->stopMotion(); + if (!result.ok()) { + result = arm->emergencyStop(); + } + return ParticipantResult{ + result.ok(), + result.ok() ? SafetyReason::None + : SafetyReason::StopUnconfirmed, + result.message}; + }; + verify = [arm] { + const auto state = arm->getRobotState(); + const bool stopped = !state.moving && !state.program_running; + return ParticipantResult{ + stopped, + stopped ? SafetyReason::None + : SafetyReason::DeviceStillMoving, + stopped ? std::string{} + : "RobotArm still reports active motion"}; + }; + } else if (const auto agv = + std::dynamic_pointer_cast(device)) { + stop = [agv] { + const auto cancel = agv->cancelNavigation(); + const auto velocity = agv->stopVelocityControl(); + const auto mapping = agv->stopMapping(); + const auto confirmed = agv->confirmMotionStopped(); + const bool stopped = + agvStopResultAccepted(cancel) && + agvStopResultAccepted(velocity) && + agvStopResultAccepted(mapping) && confirmed.ok(); + return ParticipantResult{ + stopped, + stopped ? SafetyReason::None + : confirmed.ok() ? SafetyReason::StopUnconfirmed + : SafetyReason::DeviceStillMoving, + stopped ? std::string{} + : confirmed.message.empty() + ? "AGV operational stop was not confirmed" + : confirmed.message}; + }; + verify = [agv] { + const auto confirmed = agv->confirmMotionStopped(); + return ParticipantResult{ + confirmed.ok(), + confirmed.ok() ? SafetyReason::None + : SafetyReason::DeviceStillMoving, + confirmed.message}; + }; + } else if (const auto hand = + std::dynamic_pointer_cast(device)) { + stop = [hand] { + const bool stopped = hand->stopOperationalActivity(); + return ParticipantResult{ + stopped, + stopped ? SafetyReason::None + : SafetyReason::StopUnconfirmed, + stopped ? std::string{} + : "DexHand did not confirm operational stop"}; + }; + verify = stop; + } else if (const auto motors = + std::dynamic_pointer_cast(device)) { + stop = [motors] { + bool stopped = true; + for (const auto& [joint, motor] : motors->motorsMap()) { + (void)joint; + stopped = motor && motor->quickStop() && stopped; + } + return ParticipantResult{ + stopped, + stopped ? SafetyReason::None + : SafetyReason::StopUnconfirmed, + stopped ? std::string{} + : "one or more motors did not acknowledge Quick Stop"}; + }; + verify = [motors] { + bool stopped = !motors->motorsMap().empty(); + for (const auto& [joint, motor] : motors->motorsMap()) { + (void)joint; + stopped = stopped && motor && std::abs(motor->getQd()) < 1e-3; + } + return ParticipantResult{ + stopped, + stopped ? SafetyReason::None + : SafetyReason::DeviceStillMoving, + stopped ? std::string{} + : "motor velocity has not converged to zero"}; + }; + } else if (const auto camera = + std::dynamic_pointer_cast(device)) { + stop = [camera] { + CameraState state{}; + camera->getState(state); + if (state.is_recording) { + camera->stopRecording(); + } + // Some backends use the default operational hook and report + // false even when no such activity exists. The observable final + // capture state is authoritative for this generic adapter. + (void)camera->stopOperationalActivity(); + camera->getState(state); + const bool stopped = !state.is_streaming && !state.is_recording; + return ParticipantResult{ + stopped, + stopped ? SafetyReason::None + : SafetyReason::StopUnconfirmed, + stopped ? std::string{} + : "camera activity did not stop"}; + }; + verify = [camera] { + CameraState state{}; + camera->getState(state); + const bool stopped = !state.is_streaming && !state.is_recording; + return ParticipantResult{ + stopped, + stopped ? SafetyReason::None + : SafetyReason::StopUnconfirmed, + stopped ? std::string{} + : "camera still reports active capture or recording"}; + }; + } else if (const auto microphone = + std::dynamic_pointer_cast(device)) { + stop = [microphone] { + microphone->stopStreaming(); + microphone->stopRecording(); + return ParticipantResult{true, SafetyReason::None, {}}; + }; + verify = [microphone] { + MicrophoneState state{}; + microphone->getState(state); + return ParticipantResult{ + !state.is_recording, + !state.is_recording ? SafetyReason::None + : SafetyReason::StopUnconfirmed, + state.is_recording + ? "microphone still reports active recording" + : std::string{}}; + }; + } else if (const auto speaker = + std::dynamic_pointer_cast(device)) { + stop = [speaker] { + const bool stopped = speaker->stopPlayback(); + return ParticipantResult{ + stopped, + stopped ? SafetyReason::None + : SafetyReason::StopUnconfirmed, + stopped ? std::string{} + : "speaker playback did not stop"}; + }; + verify = [speaker] { + SpeakerState state{}; + speaker->getState(state); + const bool stopped = !state.is_running && !state.is_decoding; + return ParticipantResult{ + stopped, + stopped ? SafetyReason::None + : SafetyReason::StopUnconfirmed, + stopped ? std::string{} + : "speaker still reports active playback"}; + }; + } else if (const auto head = + std::dynamic_pointer_cast(device)) { + stop = [head] { + const bool stopped = head->stopOperationalActivity(); + return ParticipantResult{ + stopped, + stopped ? SafetyReason::None + : SafetyReason::StopUnconfirmed, + stopped ? std::string{} + : "BioHead did not confirm operational stop"}; + }; + verify = stop; + } else if (const auto gripper = + std::dynamic_pointer_cast(device)) { + stop = [gripper] { + const bool stopped = gripper->stopOperationalActivity(); + return ParticipantResult{ + stopped, + stopped ? SafetyReason::None + : SafetyReason::StopUnconfirmed, + stopped ? std::string{} + : "gripper did not confirm operational stop"}; + }; + verify = stop; + } + + if (!stop || !verify) { + return {}; + } + ParticipantDescriptor descriptor; + descriptor.participant_id = device->id(); + descriptor.phase = safety_descriptor.default_policy == + SafetyPolicyFamily::Control + ? ParticipantPhase::Actuator + : ParticipantPhase::PeripheralActivity; + descriptor.required = true; + descriptor.timeout = timeout > std::chrono::milliseconds::zero() + ? timeout + : std::chrono::seconds(5); + return std::make_shared( + std::move(descriptor), + safety_descriptor.default_policy == SafetyPolicyFamily::Control, + std::move(stop), + std::move(verify)); +} + +} // namespace + +safety::DeviceSafetyRegistration makeDeviceSafetyRegistration( + const std::shared_ptr& device, + const std::chrono::milliseconds configured_maximum_age, + const std::chrono::milliseconds configured_stop_timeout) +{ + if (!device) { + return {}; + } + + safety::DeviceSafetyRegistration registration; + if (const auto endpoint_provider = + std::dynamic_pointer_cast( + device)) { + registration.endpoint = endpoint_provider->safetyEndpoint(); + } + if (registration.endpoint) { + registration.descriptor = registration.endpoint->descriptor(); + } else { + registration.descriptor = defaultDescriptor(*device); + registration.endpoint = std::make_shared( + registration.descriptor, probeFor(device)); + } + + if (configured_maximum_age > std::chrono::milliseconds::zero()) { + registration.descriptor.maximum_snapshot_age = std::min( + registration.descriptor.maximum_snapshot_age, + configured_maximum_age); + } + + if (const auto participant_provider = + std::dynamic_pointer_cast( + device)) { + registration.participant = participant_provider->safetyParticipant(); + } + if (!registration.participant) { + registration.participant = participantFor( + device, registration.descriptor, configured_stop_timeout); + } + return registration; +} + +} // namespace cmvr::device diff --git a/cmvr-es/manager/device_manager/tests/device_manager_lifecycle_test.cpp b/cmvr-es/manager/device_manager/tests/device_manager_lifecycle_test.cpp index a9009350..07f3a456 100644 --- a/cmvr-es/manager/device_manager/tests/device_manager_lifecycle_test.cpp +++ b/cmvr-es/manager/device_manager/tests/device_manager_lifecycle_test.cpp @@ -121,4 +121,33 @@ TEST_F(DeviceManagerLifecycleTest, EXPECT_EQ(device->stop_calls, 1); } +TEST_F(DeviceManagerLifecycleTest, + EnforceSelectedCannotStartWithMissingConfiguredTarget) +{ + cmvr::config::DeviceManagerConfig config; + auto* safety = config.mutable_safety(); + safety->set_mode( + cmvr::config::SafetyCoordinatorConfig::ENFORCE_SELECTED); + safety->add_enforced_device_ids("missing-arm"); + + auto& manager = cmvr::device::DeviceManager::getInstance(config); + ASSERT_TRUE(manager.initialized()); + EXPECT_FALSE(manager.start()); + EXPECT_EQ( + manager.safetyCoordinator().snapshot().system_state, + cmvr::safety::SystemAdmissionState::Starting); +} + +TEST_F(DeviceManagerLifecycleTest, + EnforceSelectedCannotSilentlyCoverNoDevices) +{ + cmvr::config::DeviceManagerConfig config; + config.mutable_safety()->set_mode( + cmvr::config::SafetyCoordinatorConfig::ENFORCE_SELECTED); + + auto& manager = cmvr::device::DeviceManager::getInstance(config); + ASSERT_TRUE(manager.initialized()); + EXPECT_FALSE(manager.start()); +} + } // namespace diff --git a/cmvr-es/manager/device_manager/tests/device_manager_snapshot_test.cpp b/cmvr-es/manager/device_manager/tests/device_manager_snapshot_test.cpp index e7c5f4e9..25f69f36 100644 --- a/cmvr-es/manager/device_manager/tests/device_manager_snapshot_test.cpp +++ b/cmvr-es/manager/device_manager/tests/device_manager_snapshot_test.cpp @@ -434,7 +434,7 @@ bool testConcurrentSnapshotAndRegistration() return true; } -bool testInventorySnapshotDoesNotWaitForDeviceHealth() +bool testManagerSnapshotsDoNotWaitForDeviceHealth() { DeviceManager::destroyInstance(); cmvr::config::DeviceManagerConfig config; @@ -443,15 +443,26 @@ bool testInventorySnapshotDoesNotWaitForDeviceHealth() std::make_shared("blocked_health_arm"); auto other_device = std::make_shared("a_camera", DeviceKind::Camera); - manager.registerDevice(blocking_device); manager.registerDevice(other_device); - auto health_future = std::async(std::launch::async, [&manager] { - return manager.snapshot(); - }); + auto registration_future = std::async( + std::launch::async, [&manager, blocking_device] { + manager.registerDevice(blocking_device); + }); if (!blocking_device->waitForHealthCall(std::chrono::seconds(2))) { blocking_device->releaseHealthCall(); - health_future.wait(); + registration_future.wait(); + return false; + } + + auto snapshot_future = std::async(std::launch::async, [&manager] { + return manager.snapshot(); + }); + if (snapshot_future.wait_for(std::chrono::milliseconds(250)) != + std::future_status::ready) { + blocking_device->releaseHealthCall(); + snapshot_future.wait(); + registration_future.wait(); return false; } @@ -462,12 +473,18 @@ bool testInventorySnapshotDoesNotWaitForDeviceHealth() std::future_status::ready) { blocking_device->releaseHealthCall(); inventory_future.wait(); - health_future.wait(); + registration_future.wait(); return false; } + const auto snapshot = snapshot_future.get(); const auto inventory = inventory_future.get(); - const bool inventory_valid = + const auto* blocked_status = + findDevice(snapshot, "blocked_health_arm"); + const bool snapshots_valid = + blocked_status != nullptr && + blocked_status->state == ManagedDeviceState::Registered && + blocked_status->health.state == DeviceHealthState::Unknown && inventory.size() == 2 && isSorted(inventory) && inventory[0].id == "a_camera" && inventory[0].kind == DeviceKind::Camera && @@ -478,8 +495,8 @@ bool testInventorySnapshotDoesNotWaitForDeviceHealth() blocking_device->health_calls.load() == 1; blocking_device->releaseHealthCall(); - health_future.get(); - return inventory_valid && blocking_device->health_calls.load() == 1; + registration_future.get(); + return snapshots_valid && blocking_device->health_calls.load() == 1; } } // namespace @@ -491,7 +508,7 @@ int main() testCategoryHealthAdapters() && testConfiguredAndDynamicSnapshots() && testConcurrentSnapshotAndRegistration() && - testInventorySnapshotDoesNotWaitForDeviceHealth(); + testManagerSnapshotsDoNotWaitForDeviceHealth(); DeviceManager::destroyInstance(); return success ? 0 : 1; } diff --git a/cmvr-es/manager/media_source_hub/CMakeLists.txt b/cmvr-es/manager/media_source_hub/CMakeLists.txt index b7fb4487..49b6473e 100644 --- a/cmvr-es/manager/media_source_hub/CMakeLists.txt +++ b/cmvr-es/manager/media_source_hub/CMakeLists.txt @@ -39,8 +39,36 @@ if(NOT CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR) cmvr_es::common cmvr_es::proto cmvr_es::logging + cmvr_es::safety_coordinator ) add_library(cmvr_es::device_media_source_adapter ALIAS device_media_source_adapter) + + if(BUILD_TESTING) + add_executable(device_media_source_adapter_test + tests/device_media_source_adapter_test.cpp + ) + target_compile_features(device_media_source_adapter_test PRIVATE cxx_std_17) + target_link_libraries(device_media_source_adapter_test + PRIVATE + cmvr_es::device_media_source_adapter + gtest + gtest_main + ) + add_test( + NAME device_media_source_adapter_test + COMMAND device_media_source_adapter_test + ) + set(_device_media_source_adapter_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}") + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _device_media_source_adapter_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(device_media_source_adapter_test PROPERTIES + ENVIRONMENT + "${_device_media_source_adapter_test_environment}" + ) + endif() endif() option(CMVR_MEDIA_SOURCE_HUB_BUILD_TESTS diff --git a/cmvr-es/manager/media_source_hub/include/device_media_source_adapter.h b/cmvr-es/manager/media_source_hub/include/device_media_source_adapter.h index 728e2d85..b23b0535 100644 --- a/cmvr-es/manager/media_source_hub/include/device_media_source_adapter.h +++ b/cmvr-es/manager/media_source_hub/include/device_media_source_adapter.h @@ -10,6 +10,7 @@ #include "devices/camera/abstract_camera.h" #include "devices/microphone/abstract_microphone.h" #include "manager/media_source_hub/include/media_source_hub.h" +#include "manager/safety/include/safety_coordinator.h" namespace cmvr::media { @@ -19,6 +20,13 @@ MediaSourceHub& globalMediaSourceHub(); std::string cameraColorTrackId(const std::string& device_id); std::string microphoneTrackId(const std::string& device_id); +// Acquires the Coordinator's Sensor/StartActivity lane and performs the +// device endpoint's final hardware check. Keep the returned guard alive until +// the operation which can start the physical media producer has returned. +safety::DispatchGuard beginMediaSourceStartDispatch( + safety::SafetyCoordinator& coordinator, + const std::string& device_id); + // Registration is idempotent for an already registered track. The adapter owns a // short-lived pump thread and one startStreaming()/stopStreaming() lease only while // at least one Hub subscription is active. It ensures start() succeeds but deliberately diff --git a/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp b/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp index 56f4cd02..70cbf688 100644 --- a/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp +++ b/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp @@ -20,6 +20,8 @@ namespace cmvr::media { namespace { +std::atomic media_start_sequence{0}; + std::string normalizedCodec(std::string codec) { codec.erase( std::remove_if(codec.begin(), codec.end(), [](const unsigned char c) { @@ -630,6 +632,43 @@ std::string microphoneTrackId(const std::string& device_id) { return device_id + "/audio/main"; } +safety::DispatchGuard beginMediaSourceStartDispatch( + safety::SafetyCoordinator& coordinator, + const std::string& device_id) +{ + safety::AdmissionRequest request; + request.command = { + "cmvr.internal.MediaSourceHub/StartSource", + safety::CommandIntent::StartActivity, + safety::SafetyPolicyFamily::Sensor, + true, + false}; + request.actor.principal_id = "internal:media-source-hub"; + request.actor.authenticated = true; + request.command_id = "media-source-start:" + device_id + ':' + + std::to_string( + media_start_sequence.fetch_add(1, std::memory_order_relaxed) + 1U); + request.device_id = device_id; + + auto admission = coordinator.admit(request); + if (!admission.permit.has_value()) { + CMVR_LOG(WARNING) + << "[DeviceMediaSourceAdapter] Media source admission rejected for " + << device_id << ": " << safety::toString(admission.decision.reason) + << " (" << admission.decision.detail << ')'; + return {}; + } + auto dispatch = coordinator.beginDispatch(*admission.permit); + if (!dispatch.acquired()) { + CMVR_LOG(WARNING) + << "[DeviceMediaSourceAdapter] Media source final check rejected for " + << device_id << ": " + << safety::toString(dispatch.hardwareCheck().reason) << " (" + << dispatch.hardwareCheck().detail << ')'; + } + return dispatch; +} + bool ensureCameraMediaSource( MediaSourceHub& hub, const std::shared_ptr& camera, diff --git a/cmvr-es/manager/media_source_hub/tests/device_media_source_adapter_test.cpp b/cmvr-es/manager/media_source_hub/tests/device_media_source_adapter_test.cpp new file mode 100644 index 00000000..8516204c --- /dev/null +++ b/cmvr-es/manager/media_source_hub/tests/device_media_source_adapter_test.cpp @@ -0,0 +1,113 @@ +#include "manager/media_source_hub/include/device_media_source_adapter.h" + +#include +#include +#include +#include + +#include + +#include "manager/safety/include/device_safety_endpoint.h" + +namespace cmvr::media { +namespace { + +class FakeSensorEndpoint final : public safety::DeviceSafetyEndpoint { +public: + explicit FakeSensorEndpoint(std::string device_id) + { + descriptor_.device_id = std::move(device_id); + descriptor_.kind = device::DeviceKind::Camera; + descriptor_.default_policy = safety::SafetyPolicyFamily::Sensor; + descriptor_.maximum_snapshot_age = std::chrono::seconds(1); + descriptor_.supports_active_refresh = true; + } + + safety::DeviceSafetyDescriptor descriptor() const override + { + return descriptor_; + } + + void bindPublisher(safety::SafetySnapshotPublisher publisher) override + { + publisher_ = std::move(publisher); + } + + void requestSafetyRefresh() noexcept override + { + if (!publisher_) { + return; + } + safety::DeviceSafetySnapshot snapshot; + snapshot.device_id = descriptor_.device_id; + snapshot.condition = safety::SafetyCondition::Nominal; + snapshot.device_generation = 1; + snapshot.sample_sequence = ++sequence_; + snapshot.observed_at = safety::SafetyClock::now(); + snapshot.connected = safety::TriState::True; + snapshot.operational_ready = safety::TriState::True; + snapshot.quiescent = safety::TriState::True; + snapshot.motion_active = safety::TriState::False; + snapshot.actuator_enabled = safety::TriState::False; + snapshot.emergency_stop_active = safety::TriState::False; + snapshot.protective_stop_active = safety::TriState::False; + snapshot.fault_active = safety::TriState::False; + (void)publisher_(std::move(snapshot)); + } + + safety::HardwareCheckResult validateBeforeDispatch( + const safety::AdmissionPermit& permit) override + { + ++hardware_checks; + last_intent = permit.intent; + return {true, safety::SafetyReason::None, {}}; + } + + safety::RecoveryCheckResult reconcileAdmissionState( + const safety::RecoveryContext&) override + { + return {true, safety::SafetyReason::None, {}}; + } + + std::atomic hardware_checks{0}; + safety::CommandIntent last_intent{safety::CommandIntent::Observe}; + +private: + safety::DeviceSafetyDescriptor descriptor_; + safety::SafetySnapshotPublisher publisher_; + std::atomic sequence_{0}; +}; + +TEST(DeviceMediaSourceAdapterTest, + SensorStartUsesFinalCheckAndQuarantineRejectsRestart) +{ + safety::SafetyCoordinatorConfig config; + config.enforcement_mode = safety::EnforcementMode::EnforceAll; + safety::SafetyCoordinator coordinator(config); + auto endpoint = std::make_shared("camera"); + ASSERT_TRUE(coordinator.registerDevice( + {endpoint->descriptor(), endpoint, {}})); + endpoint->requestSafetyRefresh(); + coordinator.updateDeviceRuntimeState( + "camera", + device::ManagedDeviceState::Running, + {device::DeviceHealthState::Healthy, {}}); + coordinator.markStartupComplete(); + + { + auto dispatch = beginMediaSourceStartDispatch(coordinator, "camera"); + ASSERT_TRUE(dispatch.acquired()); + EXPECT_EQ(endpoint->hardware_checks.load(), 1); + EXPECT_EQ(endpoint->last_intent, safety::CommandIntent::StartActivity); + } + + coordinator.quarantineDevice( + "camera", safety::SafetyReason::OutcomeUnknown, + "uncertain-media-start"); + auto rejected = beginMediaSourceStartDispatch(coordinator, "camera"); + EXPECT_FALSE(rejected.acquired()); + EXPECT_EQ(endpoint->hardware_checks.load(), 1); +} + +} // namespace +} // namespace cmvr::media diff --git a/cmvr-es/manager/safety/CMakeLists.txt b/cmvr-es/manager/safety/CMakeLists.txt new file mode 100644 index 00000000..e63a7d57 --- /dev/null +++ b/cmvr-es/manager/safety/CMakeLists.txt @@ -0,0 +1,65 @@ +add_library(safety_coordinator STATIC + src/command_ledger.cpp + src/safety_coordinator.cpp + src/safety_reason.cpp + src/safety_snapshot_store.cpp +) + +target_include_directories(safety_coordinator PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} + ${CMAKE_SOURCE_DIR}/cmvr-es +) + +target_link_libraries(safety_coordinator PUBLIC + cmvr_es::control_authority +) + +add_library(cmvr_es::safety_coordinator ALIAS safety_coordinator) +install(TARGETS safety_coordinator LIBRARY DESTINATION lib) + +if(BUILD_TESTING) + add_executable(safety_snapshot_store_test + tests/safety_snapshot_store_test.cpp + ) + target_link_libraries(safety_snapshot_store_test PRIVATE + cmvr_es::safety_coordinator + gtest + gtest_main + pthread + ) + add_test( + NAME safety_snapshot_store_test + COMMAND safety_snapshot_store_test + ) + set_tests_properties(safety_snapshot_store_test PROPERTIES TIMEOUT 10) + + add_executable(command_ledger_test + tests/command_ledger_test.cpp + ) + target_link_libraries(command_ledger_test PRIVATE + cmvr_es::safety_coordinator + gtest + gtest_main + pthread + ) + add_test( + NAME command_ledger_test + COMMAND command_ledger_test + ) + set_tests_properties(command_ledger_test PROPERTIES TIMEOUT 10) + + add_executable(safety_coordinator_test + tests/safety_coordinator_test.cpp + ) + target_link_libraries(safety_coordinator_test PRIVATE + cmvr_es::safety_coordinator + gtest + gtest_main + pthread + ) + add_test( + NAME safety_coordinator_test + COMMAND safety_coordinator_test + ) + set_tests_properties(safety_coordinator_test PROPERTIES TIMEOUT 15) +endif() diff --git a/cmvr-es/manager/safety/include/command_ledger.h b/cmvr-es/manager/safety/include/command_ledger.h new file mode 100644 index 00000000..deeeb37b --- /dev/null +++ b/cmvr-es/manager/safety/include/command_ledger.h @@ -0,0 +1,130 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "manager/safety/include/safety_types.h" + +namespace cmvr::safety { + +struct CommandKey { + std::string effective_principal_id; + std::string command_id; + + bool operator==(const CommandKey& other) const noexcept + { + return effective_principal_id == other.effective_principal_id && + command_id == other.command_id; + } +}; + +struct CommandOutcome { + CommandLifecycle lifecycle{CommandLifecycle::Failed}; + SafetyReason reason{SafetyReason::InternalError}; + std::string detail; + std::string serialized_response; + std::uint64_t safety_epoch{0}; + std::uint64_t device_generation{0}; + bool hardware_submission_possible{false}; +}; + +enum class CommandReservationStatus { + AcceptedNew, + JoinedInFlight, + CachedResult, + CommandIdConflict, + ResultEvicted, + LedgerExhausted, + Invalid, +}; + +class CommandLedger final { +private: + struct State; + +public: + struct Config { + std::size_t result_capacity{4096}; + std::size_t total_id_capacity{256U * 1024U}; + }; + + class Ticket final { + public: + Ticket() = default; + bool valid() const noexcept { return state_ != nullptr; } + + private: + friend class CommandLedger; + explicit Ticket(std::shared_ptr state) + : state_(std::move(state)) + { + } + + std::shared_ptr state_; + }; + + struct Reservation { + CommandReservationStatus status{CommandReservationStatus::Invalid}; + Ticket ticket; + std::optional cached_outcome; + }; + + CommandLedger(); + explicit CommandLedger(Config config); + + Reservation reserve(CommandKey key, std::string payload_hash); + bool setLifecycle(const Ticket& ticket, + CommandLifecycle lifecycle, + std::uint64_t safety_epoch = 0, + std::uint64_t device_generation = 0, + bool hardware_submission_possible = false); + bool complete(const Ticket& ticket, CommandOutcome outcome); + std::optional wait( + const Ticket& ticket, + SafetyClock::time_point deadline = SafetyClock::time_point::max()) const; + std::optional lookup( + const CommandKey& key, + const std::string& payload_hash) const; + + std::size_t acceptedIdCount() const; + std::size_t liveRecordCount() const; + std::size_t retiredIdCount() const; + +private: + struct KeyHash { + std::size_t operator()(const CommandKey& key) const noexcept; + }; + + struct State { + CommandKey key; + std::string payload_hash; + mutable std::mutex mutex; + mutable std::condition_variable condition; + CommandLifecycle lifecycle{CommandLifecycle::Reserved}; + std::optional outcome; + std::uint64_t safety_epoch{0}; + std::uint64_t device_generation{0}; + bool hardware_submission_possible{false}; + bool terminal{false}; + }; + + static bool validKey_(const CommandKey& key) noexcept; + void trimTerminalResultsLocked_(); + + const Config config_; + mutable std::mutex mutex_; + std::unordered_map, KeyHash> records_; + std::unordered_map retired_ids_; + std::vector terminal_order_; + std::size_t terminal_result_count_{0}; +}; + +const char* toString(CommandReservationStatus status) noexcept; + +} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety/include/device_safety_endpoint.h b/cmvr-es/manager/safety/include/device_safety_endpoint.h new file mode 100644 index 00000000..9d743683 --- /dev/null +++ b/cmvr-es/manager/safety/include/device_safety_endpoint.h @@ -0,0 +1,39 @@ +#pragma once + +#include +#include + +#include "manager/safety/include/safety_types.h" + +namespace cmvr::safety { + +using SafetySnapshotPublisher = + std::function; + +class DeviceSafetyEndpoint { +public: + virtual ~DeviceSafetyEndpoint() = default; + + virtual DeviceSafetyDescriptor descriptor() const = 0; + virtual void bindPublisher(SafetySnapshotPublisher publisher) = 0; + virtual void requestSafetyRefresh() noexcept = 0; + // Called after a backend/session restart. Implementations must publish + // subsequent samples with this generation or remain fail-closed. + virtual void onDeviceGenerationChanged( + std::uint64_t generation) noexcept + { + (void)generation; + } + virtual HardwareCheckResult validateBeforeDispatch( + const AdmissionPermit& permit) = 0; + virtual RecoveryCheckResult reconcileAdmissionState( + const RecoveryContext& context) = 0; +}; + +class DeviceSafetyEndpointProvider { +public: + virtual ~DeviceSafetyEndpointProvider() = default; + virtual std::shared_ptr safetyEndpoint() = 0; +}; + +} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety/include/safety_coordinator.h b/cmvr-es/manager/safety/include/safety_coordinator.h new file mode 100644 index 00000000..55179b0d --- /dev/null +++ b/cmvr-es/manager/safety/include/safety_coordinator.h @@ -0,0 +1,225 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "manager/safety/include/command_ledger.h" +#include "manager/safety/include/device_safety_endpoint.h" +#include "manager/safety/include/safety_participant.h" +#include "manager/safety/include/safety_snapshot_store.h" + +namespace cmvr::safety { + +struct SafetyCoordinatorConfig { + EnforcementMode enforcement_mode{EnforcementMode::Shadow}; + std::unordered_set enforced_device_ids; + std::chrono::milliseconds stop_all_timeout{15000}; + std::chrono::milliseconds recovery_timeout{10000}; + CommandLedger::Config command_ledger; + std::size_t event_history_capacity{2048}; + bool fail_startup_on_missing_control_capability{false}; +}; + +struct AdmissionResult { + AdmissionDecision decision; + std::optional permit; +}; + +struct StartupCoverageIssue { + std::string target_id; + SafetyReason reason{SafetyReason::None}; + std::string detail; +}; + +struct StartupCoverageResult { + bool ready{false}; + std::vector issues; +}; + +struct DeviceSafetyStateView { + DeviceSafetyDescriptor descriptor; + SafetySnapshotView safety; + device::ManagedDeviceState lifecycle{ + device::ManagedDeviceState::Unknown}; + device::DeviceHealthSnapshot health; + DeviceAdmissionState admission_state{DeviceAdmissionState::Observing}; + std::vector blockers; +}; + +struct ParticipantResultView { + bool recorded{false}; + bool success{false}; + SafetyReason reason{SafetyReason::None}; + std::string detail; +}; + +struct ParticipantSafetyStateView { + ParticipantDescriptor descriptor; + bool registered{false}; + bool barrier_active{false}; + bool barrier_retained{false}; + std::string operation_id; + std::uint64_t safety_epoch{0}; + ParticipantResultView last_request; + ParticipantResultView last_verify; + ParticipantResultView last_release; +}; + +struct SafetyCoordinatorSnapshot { + SystemAdmissionState system_state{SystemAdmissionState::Starting}; + std::uint64_t safety_epoch{0}; + std::string service_instance_id; + EnforcementMode enforcement_mode{EnforcementMode::Shadow}; + std::string active_operation_id; + std::string active_operation_phase; + std::vector devices; + std::vector participants; + std::vector recent_events; +}; + +struct SafetyTargetResult { + std::string target_id; + bool success{false}; + SafetyReason reason{SafetyReason::None}; + std::string detail; + DeviceAdmissionState before_state{DeviceAdmissionState::Observing}; + DeviceAdmissionState after_state{DeviceAdmissionState::Observing}; +}; + +struct StopAllResult { + bool success{false}; + std::string operation_id; + std::uint64_t previous_safety_epoch{0}; + std::uint64_t current_safety_epoch{0}; + SystemAdmissionState system_state{SystemAdmissionState::Starting}; + std::vector targets; +}; + +enum class RecoveryResultCode { + Recovered, + VerifiedButStillBlocked, + BlockerRemains, + EpochMismatch, + NothingToRecover, + TimedOut, + Failed, +}; + +struct RecoveryRequest { + std::string recovery_id; + std::vector device_ids; + bool all_devices{false}; + std::uint64_t expected_safety_epoch{0}; + bool verify_only{true}; + 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. + std::function authorize_clear; +}; + +struct RecoveryResult { + RecoveryResultCode result{RecoveryResultCode::Failed}; + std::string recovery_id; + std::uint64_t previous_safety_epoch{0}; + std::uint64_t current_safety_epoch{0}; + SystemAdmissionState system_state{SystemAdmissionState::Starting}; + std::vector targets; +}; + +class SafetyCoordinator; + +class DispatchGuard final { +public: + DispatchGuard() noexcept = default; + ~DispatchGuard() noexcept; + DispatchGuard(DispatchGuard&& other) noexcept; + DispatchGuard& operator=(DispatchGuard&& other) noexcept; + DispatchGuard(const DispatchGuard&) = delete; + DispatchGuard& operator=(const DispatchGuard&) = delete; + + bool acquired() const noexcept { return coordinator_ != nullptr; } + const HardwareCheckResult& hardwareCheck() const noexcept + { + return hardware_check_; + } + +private: + friend class SafetyCoordinator; + DispatchGuard(SafetyCoordinator* coordinator, + std::string device_id, + HardwareCheckResult hardware_check) noexcept; + void reset_() noexcept; + + SafetyCoordinator* coordinator_{nullptr}; + std::string device_id_; + HardwareCheckResult hardware_check_; +}; + +class SafetyCoordinator final { +public: + explicit SafetyCoordinator(SafetyCoordinatorConfig config = {}); + ~SafetyCoordinator(); + SafetyCoordinator(const SafetyCoordinator&) = delete; + SafetyCoordinator& operator=(const SafetyCoordinator&) = delete; + + bool registerDevice(DeviceSafetyRegistration registration); + bool unregisterDevice(const std::string& device_id); + bool registerParticipant(std::shared_ptr participant); + bool unregisterParticipant(const std::string& participant_id); + + void updateDeviceRuntimeState( + const std::string& device_id, + device::ManagedDeviceState lifecycle, + device::DeviceHealthSnapshot health = {}); + bool publishSafetySnapshot(DeviceSafetySnapshot snapshot); + std::optional advanceDeviceGeneration( + const std::string& device_id); + StartupCoverageResult validateStartupCoverage( + SafetyClock::time_point deadline); + void markStartupComplete(); + void beginShutdown() noexcept; + + AdmissionDecision evaluate(const AdmissionRequest& request) const; + AdmissionResult admit(const AdmissionRequest& request); + // Lightweight session check. This validates the coordinator-owned epoch, + // generation, freshness, and admission state without calling the device + // endpoint or entering the hardware dispatch set. + HardwareCheckResult revalidatePermit( + const AdmissionPermit& permit) const; + DispatchGuard beginDispatch(const AdmissionPermit& permit); + void quarantineDevice(const std::string& device_id, + SafetyReason reason, + std::string operation_id = {}); + + StopAllResult stopAll( + std::string operation_id, + SafetyClock::time_point deadline = SafetyClock::time_point::max()); + RecoveryResult recover(const RecoveryRequest& request); + + SafetyCoordinatorSnapshot snapshot() const; + SafetySnapshotStore& snapshotStore() noexcept; + const SafetySnapshotStore& snapshotStore() const noexcept; + CommandLedger& commandLedger() noexcept; + const CommandLedger& commandLedger() const noexcept; + const std::string& serviceInstanceId() const noexcept; + const SafetyCoordinatorConfig& config() const noexcept; + +private: + friend class DispatchGuard; + struct Impl; + void endDispatch_(const std::string& device_id) noexcept; + std::unique_ptr impl_; +}; + +const char* toString(RecoveryResultCode value) noexcept; + +} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety/include/safety_participant.h b/cmvr-es/manager/safety/include/safety_participant.h new file mode 100644 index 00000000..69072164 --- /dev/null +++ b/cmvr-es/manager/safety/include/safety_participant.h @@ -0,0 +1,88 @@ +#pragma once + +#include +#include +#include +#include + +#include "manager/safety/include/safety_types.h" + +namespace cmvr::safety { + +class DeviceSafetyEndpoint; + +enum class ParticipantPhase { + Ingress, + Scheduler, + ControlSession, + Actuator, + PeripheralActivity, + Verification, +}; + +struct ParticipantDescriptor { + std::string participant_id; + ParticipantPhase phase{ParticipantPhase::Actuator}; + bool required{true}; + std::chrono::milliseconds timeout{5000}; +}; + +struct SafetyOperationContext { + std::string operation_id; + std::uint64_t safety_epoch{0}; + SafetyClock::time_point deadline{SafetyClock::time_point::max()}; +}; + +struct BarrierToken { + std::string participant_id; + std::string operation_id; + std::uint64_t safety_epoch{0}; + std::uint64_t generation{0}; + + bool valid() const noexcept + { + return !participant_id.empty() && !operation_id.empty() && + safety_epoch != 0 && generation != 0; + } +}; + +struct ParticipantResult { + bool success{false}; + SafetyReason reason{SafetyReason::StopUnconfirmed}; + std::string detail; +}; + +class SafetyParticipant { +public: + virtual ~SafetyParticipant() = default; + virtual ParticipantDescriptor descriptor() const = 0; + virtual BarrierToken beginBarrier( + const SafetyOperationContext& context) = 0; + virtual ParticipantResult requestQuiesce( + const BarrierToken& token, + const SafetyOperationContext& context) = 0; + virtual ParticipantResult verifyQuiescent( + const BarrierToken& token, + const SafetyOperationContext& context) = 0; + virtual RecoveryCheckResult recoverAdmission( + const BarrierToken& token, + const RecoveryContext& context) = 0; + // Commits the participant's admission reopening. A failed commit must + // leave that participant fail-closed and be retryable through recovery. + virtual ParticipantResult releaseBarrier( + const BarrierToken& token) noexcept = 0; +}; + +class SafetyParticipantProvider { +public: + virtual ~SafetyParticipantProvider() = default; + virtual std::shared_ptr safetyParticipant() = 0; +}; + +struct DeviceSafetyRegistration { + DeviceSafetyDescriptor descriptor; + std::shared_ptr endpoint; + std::shared_ptr participant; +}; + +} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety/include/safety_reason.h b/cmvr-es/manager/safety/include/safety_reason.h new file mode 100644 index 00000000..61b44206 --- /dev/null +++ b/cmvr-es/manager/safety/include/safety_reason.h @@ -0,0 +1,48 @@ +#pragma once + +#include + +namespace cmvr::safety { + +enum class SafetyReason { + None, + InvalidArgument, + Unauthenticated, + PermissionDenied, + RecoveryRpcDisabled, + DeviceNotFound, + DeviceUnavailable, + UnsupportedCommand, + SystemStarting, + SystemStopping, + SafetyLatched, + SafetyStateMissing, + SafetyStateStale, + HardwareUnsafe, + EmergencyStopActive, + ProtectiveStopActive, + DeviceDisconnected, + DeviceFault, + DeviceNotReady, + DeviceStillMoving, + ControlBusy, + GenerationMismatch, + CommandIdRequired, + CommandIdConflict, + ResultEvicted, + LedgerExhausted, + Backpressure, + DeadlineExceededBeforeDispatch, + OutcomeUnknown, + ParticipantTimeout, + StopUnconfirmed, + RecoveryEpochMismatch, + RecoveryReasonRequired, + RecoveryAuditFailed, + InternalError, +}; + +const char* toString(SafetyReason reason) noexcept; +bool retryWithSameCommandId(SafetyReason reason) noexcept; + +} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety/include/safety_snapshot_store.h b/cmvr-es/manager/safety/include/safety_snapshot_store.h new file mode 100644 index 00000000..b2871eaa --- /dev/null +++ b/cmvr-es/manager/safety/include/safety_snapshot_store.h @@ -0,0 +1,54 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "manager/safety/include/safety_types.h" + +namespace cmvr::safety { + +class SafetySnapshotStore final { +public: + bool registerDevice(const DeviceSafetyDescriptor& descriptor, + std::uint64_t initial_generation = 1); + bool unregisterDevice(const std::string& device_id); + bool publish(DeviceSafetySnapshot snapshot); + bool markUnknown(const std::string& device_id, + SafetyReason reason, + std::string source_id = {}); + std::optional bumpGeneration( + const std::string& device_id); + + SafetySnapshotView get( + const std::string& device_id, + SafetyClock::time_point now = SafetyClock::now()) const; + std::vector snapshot( + SafetyClock::time_point now = SafetyClock::now()) const; + + bool waitForNewerSample( + const std::string& device_id, + std::uint64_t previous_sequence, + SafetyClock::time_point deadline, + SafetySnapshotView& result) const; + +private: + struct Slot { + DeviceSafetyDescriptor descriptor; + DeviceSafetySnapshot snapshot; + bool has_sample{false}; + }; + + static SafetySnapshotView viewOf_( + const Slot& slot, + SafetyClock::time_point now); + + mutable std::shared_mutex mutex_; + mutable std::condition_variable_any changed_; + std::unordered_map slots_; +}; + +} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety/include/safety_types.h b/cmvr-es/manager/safety/include/safety_types.h new file mode 100644 index 00000000..66144b62 --- /dev/null +++ b/cmvr-es/manager/safety/include/safety_types.h @@ -0,0 +1,235 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "devices/device_types.h" +#include "manager/safety/include/safety_reason.h" + +namespace cmvr::safety { + +using SafetyClock = std::chrono::steady_clock; + +enum class TriState { + Unknown, + False, + True, +}; + +enum class SafetyCondition { + Nominal, + Restricted, + Unsafe, + Unknown, +}; + +enum class CommandIntent { + Observe, + StartActivity, + Configure, + Actuate, + Stop, + ResetFault, + RecoverAdmission, +}; + +enum class SafetyPolicyFamily { + Sensor, + Control, +}; + +enum class BlockerScope { + Device, + System, +}; + +enum class RecoveryRequirement { + RefreshOnly, + ClearSoftwareLatch, + HardwareReleaseRequired, + ManualInspectionRequired, +}; + +enum class SystemAdmissionState { + Starting, + Open, + Stopping, + Latched, + Recovering, + ShuttingDown, +}; + +enum class DeviceAdmissionState { + Observing, + Open, + Blocked, + Quarantined, + Recovering, + Removed, +}; + +enum class EnforcementMode { + Legacy, + Shadow, + EnforceSelected, + EnforceAll, +}; + +enum class CommandLifecycle { + Received, + Reserved, + RejectedBeforeDispatch, + Admitted, + Dispatching, + AcceptedByHardware, + Completed, + Failed, + CanceledBeforeDispatch, + OutcomeUnknown, +}; + +struct SafetyBlocker { + SafetyReason reason{SafetyReason::None}; + BlockerScope scope{BlockerScope::Device}; + RecoveryRequirement recovery_requirement{ + RecoveryRequirement::RefreshOnly}; + std::string source_id; + std::string operation_id; + std::uint64_t first_observed_at_unix_ms{0}; + std::uint64_t last_observed_at_unix_ms{0}; +}; + +struct DeviceSafetySnapshot { + std::string device_id; + SafetyCondition condition{SafetyCondition::Unknown}; + std::uint64_t device_generation{0}; + std::uint64_t sample_sequence{0}; + SafetyClock::time_point observed_at{}; + std::uint64_t observed_at_unix_ms{0}; + + TriState connected{TriState::Unknown}; + TriState operational_ready{TriState::Unknown}; + TriState quiescent{TriState::Unknown}; + TriState motion_active{TriState::Unknown}; + TriState actuator_enabled{TriState::Unknown}; + TriState emergency_stop_active{TriState::Unknown}; + TriState protective_stop_active{TriState::Unknown}; + TriState fault_active{TriState::Unknown}; + + std::vector blockers; +}; + +struct DeviceSafetyDescriptor { + std::string device_id; + device::DeviceKind kind{device::DeviceKind::Unknown}; + SafetyPolicyFamily default_policy{SafetyPolicyFamily::Sensor}; + std::chrono::milliseconds maximum_snapshot_age{1000}; + bool requires_safe_stop{false}; + bool supports_active_refresh{false}; + bool supports_non_enabling_fault_reset{false}; +}; + +struct SafetySnapshotView { + DeviceSafetyDescriptor descriptor; + DeviceSafetySnapshot snapshot; + bool registered{false}; + bool has_sample{false}; + bool fresh{false}; + std::chrono::milliseconds sample_age{ + std::chrono::milliseconds::max()}; +}; + +struct CommandActor { + std::string principal_id{"anonymous"}; + bool authenticated{false}; + std::vector roles; +}; + +struct CommandDescriptor { + std::string full_method_name; + CommandIntent intent{CommandIntent::Observe}; + SafetyPolicyFamily policy_family{SafetyPolicyFamily::Sensor}; + bool mutating{false}; + bool safety_lane{false}; +}; + +struct AdmissionRequest { + CommandDescriptor command; + CommandActor actor; + std::string command_id; + std::string device_id; + std::optional expected_device_generation; + std::uint64_t authority_generation{0}; + SafetyClock::time_point deadline{SafetyClock::time_point::max()}; +}; + +struct AdmissionDecision { + bool allowed{false}; + bool policy_allowed{false}; + bool enforced{false}; + SafetyReason reason{SafetyReason::None}; + std::string detail; + std::uint64_t safety_epoch{0}; + std::uint64_t device_generation{0}; +}; + +struct AdmissionPermit { + AdmissionPermit() = default; + AdmissionPermit(AdmissionPermit&&) noexcept = default; + AdmissionPermit& operator=(AdmissionPermit&&) noexcept = default; + AdmissionPermit(const AdmissionPermit&) = delete; + AdmissionPermit& operator=(const AdmissionPermit&) = delete; + + std::string command_id; + std::string device_id; + CommandIntent intent{CommandIntent::Observe}; + std::uint64_t safety_epoch{0}; + std::uint64_t device_generation{0}; + std::uint64_t authority_generation{0}; + SafetyClock::time_point deadline{SafetyClock::time_point::max()}; + bool policy_allowed{false}; + bool enforced{false}; +}; + +struct HardwareCheckResult { + bool safe{false}; + SafetyReason reason{SafetyReason::SafetyStateMissing}; + std::string detail; +}; + +struct RecoveryContext { + std::string recovery_id; + std::string reason; + std::uint64_t safety_epoch{0}; + SafetyClock::time_point deadline{SafetyClock::time_point::max()}; + bool verify_only{true}; +}; + +struct RecoveryCheckResult { + bool reconciled{false}; + SafetyReason reason{SafetyReason::None}; + std::string detail; +}; + +struct SafetyEvent { + std::uint64_t sequence{0}; + std::uint64_t safety_epoch{0}; + std::uint64_t occurred_at_unix_ms{0}; + std::string source_id; + std::string operation_id; + SafetyReason reason{SafetyReason::None}; + std::string detail; +}; + +const char* toString(TriState value) noexcept; +const char* toString(SafetyCondition value) noexcept; +const char* toString(CommandIntent value) noexcept; +const char* toString(SafetyPolicyFamily value) noexcept; +const char* toString(SystemAdmissionState value) noexcept; +const char* toString(DeviceAdmissionState value) noexcept; +const char* toString(EnforcementMode value) noexcept; + +} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety/src/command_ledger.cpp b/cmvr-es/manager/safety/src/command_ledger.cpp new file mode 100644 index 00000000..e4c6e790 --- /dev/null +++ b/cmvr-es/manager/safety/src/command_ledger.cpp @@ -0,0 +1,276 @@ +#include "manager/safety/include/command_ledger.h" + +#include +#include +#include +#include +#include + +namespace cmvr::safety { + +namespace { + +constexpr std::size_t kMaxPrincipalIdLength = 256; +constexpr std::size_t kMaxCommandIdLength = 128; + +bool validIdentifier(const std::string& value, const std::size_t maximum) +{ + if (value.empty() || value.size() > maximum) { + return false; + } + return std::all_of( + value.begin(), value.end(), [](const unsigned char character) { + return std::isalnum(character) || character == '-' || + character == '_' || character == '.' || + character == ':' || character == '/'; + }); +} + +} // namespace + +CommandLedger::CommandLedger() + : CommandLedger(Config{}) +{ +} + +CommandLedger::CommandLedger(Config config) + : config_(config) +{ + if (config_.total_id_capacity == 0 || + config_.result_capacity > config_.total_id_capacity) { + throw std::invalid_argument("invalid CommandLedger capacity"); + } +} + +std::size_t CommandLedger::KeyHash::operator()( + const CommandKey& key) const noexcept +{ + const auto first = std::hash{}(key.effective_principal_id); + const auto second = std::hash{}(key.command_id); + return first ^ (second + 0x9e3779b9U + (first << 6U) + (first >> 2U)); +} + +bool CommandLedger::validKey_(const CommandKey& key) noexcept +{ + return validIdentifier( + key.effective_principal_id, kMaxPrincipalIdLength) && + validIdentifier(key.command_id, kMaxCommandIdLength); +} + +CommandLedger::Reservation CommandLedger::reserve( + CommandKey key, + std::string payload_hash) +{ + if (!validKey_(key) || payload_hash.empty()) { + return {}; + } + + std::lock_guard lock(mutex_); + const auto live = records_.find(key); + if (live != records_.end()) { + const auto& state = live->second; + std::lock_guard state_lock(state->mutex); + if (state->payload_hash != payload_hash) { + return {CommandReservationStatus::CommandIdConflict, {}, {}}; + } + if (state->terminal && state->outcome.has_value()) { + return { + CommandReservationStatus::CachedResult, + Ticket(state), + state->outcome}; + } + return { + CommandReservationStatus::JoinedInFlight, + Ticket(state), + {}}; + } + + const auto retired = retired_ids_.find(key); + if (retired != retired_ids_.end()) { + return { + retired->second == payload_hash + ? CommandReservationStatus::ResultEvicted + : CommandReservationStatus::CommandIdConflict, + {}, + {}}; + } + + if (records_.size() + retired_ids_.size() >= + config_.total_id_capacity) { + return {CommandReservationStatus::LedgerExhausted, {}, {}}; + } + + auto state = std::make_shared(); + state->key = std::move(key); + state->payload_hash = std::move(payload_hash); + const auto inserted = records_.emplace(state->key, state); + if (!inserted.second) { + throw std::logic_error("CommandLedger duplicate insertion"); + } + return { + CommandReservationStatus::AcceptedNew, + Ticket(std::move(state)), + {}}; +} + +bool CommandLedger::setLifecycle( + const Ticket& ticket, + const CommandLifecycle lifecycle, + const std::uint64_t safety_epoch, + const std::uint64_t device_generation, + const bool hardware_submission_possible) +{ + if (!ticket.valid()) { + return false; + } + std::lock_guard ledger_lock(mutex_); + const auto found = records_.find(ticket.state_->key); + if (found == records_.end() || found->second != ticket.state_) { + return false; + } + std::lock_guard state_lock(ticket.state_->mutex); + if (ticket.state_->terminal) { + return false; + } + ticket.state_->lifecycle = lifecycle; + ticket.state_->safety_epoch = safety_epoch; + ticket.state_->device_generation = device_generation; + ticket.state_->hardware_submission_possible = + ticket.state_->hardware_submission_possible || + hardware_submission_possible; + return true; +} + +bool CommandLedger::complete(const Ticket& ticket, CommandOutcome outcome) +{ + if (!ticket.valid()) { + return false; + } + + std::lock_guard ledger_lock(mutex_); + const auto found = records_.find(ticket.state_->key); + if (found == records_.end() || found->second != ticket.state_) { + return false; + } + + { + std::lock_guard state_lock(ticket.state_->mutex); + if (ticket.state_->terminal) { + return false; + } + outcome.hardware_submission_possible = + outcome.hardware_submission_possible || + ticket.state_->hardware_submission_possible; + if (outcome.safety_epoch == 0) { + outcome.safety_epoch = ticket.state_->safety_epoch; + } + if (outcome.device_generation == 0) { + outcome.device_generation = ticket.state_->device_generation; + } + ticket.state_->lifecycle = outcome.lifecycle; + ticket.state_->outcome = std::move(outcome); + ticket.state_->terminal = true; + } + ticket.state_->condition.notify_all(); + terminal_order_.push_back(ticket.state_->key); + ++terminal_result_count_; + trimTerminalResultsLocked_(); + return true; +} + +void CommandLedger::trimTerminalResultsLocked_() +{ + std::size_t consumed = 0; + while (terminal_result_count_ > config_.result_capacity && + consumed < terminal_order_.size()) { + const auto key = terminal_order_[consumed++]; + const auto found = records_.find(key); + if (found == records_.end()) { + continue; + } + const auto& state = found->second; + std::lock_guard state_lock(state->mutex); + if (!state->terminal) { + continue; + } + retired_ids_.emplace(state->key, state->payload_hash); + records_.erase(found); + --terminal_result_count_; + } + if (consumed != 0) { + terminal_order_.erase( + terminal_order_.begin(), + terminal_order_.begin() + static_cast(consumed)); + } +} + +std::optional CommandLedger::wait( + const Ticket& ticket, + const SafetyClock::time_point deadline) const +{ + if (!ticket.valid()) { + return std::nullopt; + } + std::unique_lock lock(ticket.state_->mutex); + if (deadline == SafetyClock::time_point::max()) { + ticket.state_->condition.wait( + lock, [&ticket] { return ticket.state_->terminal; }); + } else if (!ticket.state_->condition.wait_until( + lock, deadline, + [&ticket] { return ticket.state_->terminal; })) { + return std::nullopt; + } + return ticket.state_->outcome; +} + +std::optional CommandLedger::lookup( + const CommandKey& key, + const std::string& payload_hash) const +{ + std::lock_guard lock(mutex_); + const auto found = records_.find(key); + if (found == records_.end()) { + return std::nullopt; + } + std::lock_guard state_lock(found->second->mutex); + if (found->second->payload_hash != payload_hash || + !found->second->terminal) { + return std::nullopt; + } + return found->second->outcome; +} + +std::size_t CommandLedger::acceptedIdCount() const +{ + std::lock_guard lock(mutex_); + return records_.size() + retired_ids_.size(); +} + +std::size_t CommandLedger::liveRecordCount() const +{ + std::lock_guard lock(mutex_); + return records_.size(); +} + +std::size_t CommandLedger::retiredIdCount() const +{ + std::lock_guard lock(mutex_); + return retired_ids_.size(); +} + +const char* toString(const CommandReservationStatus status) noexcept +{ + switch (status) { + case CommandReservationStatus::AcceptedNew: return "AcceptedNew"; + case CommandReservationStatus::JoinedInFlight: return "JoinedInFlight"; + case CommandReservationStatus::CachedResult: return "CachedResult"; + case CommandReservationStatus::CommandIdConflict: + return "CommandIdConflict"; + case CommandReservationStatus::ResultEvicted: return "ResultEvicted"; + case CommandReservationStatus::LedgerExhausted: return "LedgerExhausted"; + case CommandReservationStatus::Invalid: return "Invalid"; + } + return "Invalid"; +} + +} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety/src/safety_coordinator.cpp b/cmvr-es/manager/safety/src/safety_coordinator.cpp new file mode 100644 index 00000000..fb0c012e --- /dev/null +++ b/cmvr-es/manager/safety/src/safety_coordinator.cpp @@ -0,0 +1,2424 @@ +#include "manager/safety/include/safety_coordinator.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cmvr::safety { + +namespace { + +std::uint64_t unixTimeMs() noexcept +{ + const auto value = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + return value > 0 ? static_cast(value) : 1U; +} + +std::string makeInstanceId() +{ + static std::atomic sequence{0}; + std::ostringstream output; + output << "safety-" << std::hex + << std::chrono::duration_cast( + SafetyClock::now().time_since_epoch()).count() + << '-' + << sequence.fetch_add(1, std::memory_order_relaxed) + 1U; + return output.str(); +} + +std::string makeOperationId(const char* prefix) +{ + static std::atomic sequence{0}; + return std::string(prefix) + '-' + std::to_string( + sequence.fetch_add(1, std::memory_order_relaxed) + 1U); +} + +bool hasTrue(const TriState value) noexcept +{ + return value == TriState::True; +} + +bool isControlIntent(const CommandIntent intent) noexcept +{ + return intent == CommandIntent::Actuate || + intent == CommandIntent::Configure || + intent == CommandIntent::StartActivity; +} + +std::chrono::milliseconds boundedRemaining( + const SafetyClock::time_point deadline, + const std::chrono::milliseconds maximum) +{ + const auto now = SafetyClock::now(); + if (now >= deadline) { + return std::chrono::milliseconds::zero(); + } + return std::min( + maximum, + std::chrono::duration_cast(deadline - now)); +} + +SafetyBlocker makeBlocker( + const SafetyReason reason, + const BlockerScope scope, + const RecoveryRequirement recovery, + std::string source, + std::string operation = {}) +{ + const auto now = unixTimeMs(); + return { + reason, + scope, + recovery, + std::move(source), + std::move(operation), + now, + now}; +} + +class OperationExecutor final { +public: + explicit OperationExecutor(const std::size_t worker_count) + { + const auto count = std::max(1, worker_count); + workers_.reserve(count); + for (std::size_t index = 0; index < count; ++index) { + workers_.emplace_back([this] { workerLoop_(); }); + } + } + + ~OperationExecutor() + { + { + std::lock_guard lock(mutex_); + stopping_ = true; + } + available_.notify_all(); + for (auto& worker : workers_) { + if (worker.joinable()) { + worker.join(); + } + } + } + + std::future submit( + std::function operation) + { + auto task = std::make_shared>( + [operation = std::move(operation)]() mutable { + try { + return operation(); + } catch (const std::exception& error) { + return ParticipantResult{ + false, SafetyReason::InternalError, error.what()}; + } catch (...) { + return ParticipantResult{ + false, + SafetyReason::InternalError, + "participant threw an unknown exception"}; + } + }); + auto future = task->get_future(); + { + std::lock_guard lock(mutex_); + if (stopping_) { + throw std::runtime_error("safety operation executor stopped"); + } + jobs_.push([task] { (*task)(); }); + } + available_.notify_one(); + return future; + } + +private: + void workerLoop_() + { + while (true) { + std::function job; + { + std::unique_lock lock(mutex_); + available_.wait(lock, [this] { + return stopping_ || !jobs_.empty(); + }); + if (stopping_ && jobs_.empty()) { + return; + } + job = std::move(jobs_.front()); + jobs_.pop(); + } + job(); + } + } + + std::mutex mutex_; + std::condition_variable available_; + std::queue> jobs_; + std::vector workers_; + bool stopping_{false}; +}; + +int phaseRank(const ParticipantPhase phase) noexcept +{ + switch (phase) { + case ParticipantPhase::Ingress: return 0; + case ParticipantPhase::Scheduler: return 1; + case ParticipantPhase::ControlSession: return 2; + case ParticipantPhase::Actuator: return 3; + case ParticipantPhase::PeripheralActivity: return 4; + case ParticipantPhase::Verification: return 5; + } + return 5; +} + +} // namespace + +struct SafetyCoordinator::Impl { + struct PublisherBinding { + std::atomic active{true}; + SafetyCoordinator* coordinator{nullptr}; + }; + + struct DeviceSlot { + DeviceSafetyDescriptor descriptor; + std::shared_ptr endpoint; + std::shared_ptr participant; + std::shared_ptr publisher_binding; + device::ManagedDeviceState lifecycle{ + device::ManagedDeviceState::Unknown}; + device::DeviceHealthSnapshot health; + DeviceAdmissionState admission_state{ + DeviceAdmissionState::Observing}; + std::vector software_blockers; + std::size_t in_flight_dispatches{0}; + bool quarantined{false}; + }; + + struct RetainedBarrier { + std::shared_ptr participant; + BarrierToken token; + }; + + struct ParticipantRuntime { + ParticipantDescriptor descriptor; + bool registered{false}; + bool barrier_active{false}; + bool barrier_retained{false}; + std::string operation_id; + std::uint64_t safety_epoch{0}; + ParticipantResultView last_request; + ParticipantResultView last_verify; + ParticipantResultView last_release; + }; + + struct StopRound { + std::mutex mutex; + std::condition_variable completed; + std::atomic done{false}; + StopAllResult result; + }; + + struct RecoveryRound { + std::mutex mutex; + std::condition_variable completed; + std::string fingerprint; + std::atomic done{false}; + RecoveryResult result; + }; + + explicit Impl(SafetyCoordinatorConfig source) + : config(std::move(source)), + ledger(config.command_ledger), + service_instance_id(makeInstanceId()), + operation_executor(4) + { + if (config.stop_all_timeout <= std::chrono::milliseconds::zero() || + config.recovery_timeout <= std::chrono::milliseconds::zero() || + config.event_history_capacity == 0) { + throw std::invalid_argument("invalid SafetyCoordinatorConfig"); + } + } + + bool isEnforced(const std::string& device_id) const + { + switch (config.enforcement_mode) { + case EnforcementMode::Legacy: + case EnforcementMode::Shadow: + return false; + case EnforcementMode::EnforceSelected: + return config.enforced_device_ids.count(device_id) != 0; + case EnforcementMode::EnforceAll: + return true; + } + return false; + } + + void addEventLocked( + const SafetyReason reason, + std::string source, + std::string operation, + std::string detail) + { + SafetyEvent event; + event.sequence = ++next_event_sequence; + event.safety_epoch = safety_epoch; + event.occurred_at_unix_ms = unixTimeMs(); + event.source_id = std::move(source); + event.operation_id = std::move(operation); + event.reason = reason; + event.detail = std::move(detail); + events.push_back(std::move(event)); + while (events.size() > config.event_history_capacity) { + events.pop_front(); + } + } + + DeviceAdmissionState admissionStateFor( + const DeviceSlot& slot, + const SafetySnapshotView& view) const + { + if (slot.admission_state == DeviceAdmissionState::Removed) { + return DeviceAdmissionState::Removed; + } + if (slot.quarantined) { + return DeviceAdmissionState::Quarantined; + } + if (!view.has_sample) { + return DeviceAdmissionState::Observing; + } + if (!view.fresh || + view.snapshot.condition == SafetyCondition::Unknown || + view.snapshot.condition == SafetyCondition::Unsafe || + view.snapshot.connected == TriState::False || + view.snapshot.fault_active == TriState::True || + slot.lifecycle == device::ManagedDeviceState::Error) { + return DeviceAdmissionState::Blocked; + } + if (slot.descriptor.default_policy == SafetyPolicyFamily::Control && + (view.snapshot.connected != TriState::True || + view.snapshot.operational_ready != TriState::True || + view.snapshot.emergency_stop_active != TriState::False || + view.snapshot.protective_stop_active != TriState::False || + view.snapshot.fault_active != TriState::False)) { + return DeviceAdmissionState::Blocked; + } + return DeviceAdmissionState::Open; + } + + bool hasGlobalLatchLocked() const + { + if (!retained_barriers.empty()) { + return true; + } + for (const auto& [id, slot] : devices) { + (void)id; + if (slot.quarantined) { + return true; + } + const auto view = snapshots.get(slot.descriptor.device_id); + for (const auto& blocker : view.snapshot.blockers) { + if (blocker.scope == BlockerScope::System) { + return true; + } + } + } + return false; + } + + SafetyCoordinatorConfig config; + SafetySnapshotStore snapshots; + CommandLedger ledger; + const std::string service_instance_id; + + mutable std::mutex mutex; + std::condition_variable state_changed; + SystemAdmissionState system_state{SystemAdmissionState::Starting}; + std::uint64_t safety_epoch{1}; + std::unordered_map devices; + std::unordered_map> + participants; + std::unordered_map participant_runtime; + std::unordered_map retained_barriers; + std::deque events; + std::uint64_t next_event_sequence{0}; + std::string active_operation_id; + std::string active_operation_phase; + + std::mutex operation_execution_mutex; + OperationExecutor operation_executor; + std::mutex rounds_mutex; + std::shared_ptr active_stop_round; + std::unordered_map> + recovery_rounds; +}; + +DispatchGuard::DispatchGuard( + SafetyCoordinator* coordinator, + std::string device_id, + HardwareCheckResult hardware_check) noexcept + : coordinator_(coordinator), + device_id_(std::move(device_id)), + hardware_check_(std::move(hardware_check)) +{ +} + +DispatchGuard::~DispatchGuard() noexcept +{ + reset_(); +} + +DispatchGuard::DispatchGuard(DispatchGuard&& other) noexcept + : coordinator_(std::exchange(other.coordinator_, nullptr)), + device_id_(std::move(other.device_id_)), + hardware_check_(std::move(other.hardware_check_)) +{ +} + +DispatchGuard& DispatchGuard::operator=(DispatchGuard&& other) noexcept +{ + if (this != &other) { + reset_(); + coordinator_ = std::exchange(other.coordinator_, nullptr); + device_id_ = std::move(other.device_id_); + hardware_check_ = std::move(other.hardware_check_); + } + return *this; +} + +void DispatchGuard::reset_() noexcept +{ + if (coordinator_ == nullptr) { + return; + } + auto* coordinator = std::exchange(coordinator_, nullptr); + coordinator->endDispatch_(device_id_); +} + +SafetyCoordinator::SafetyCoordinator(SafetyCoordinatorConfig config) + : impl_(std::make_unique(std::move(config))) +{ +} + +SafetyCoordinator::~SafetyCoordinator() +{ + beginShutdown(); + std::vector> endpoints; + { + std::lock_guard lock(impl_->mutex); + endpoints.reserve(impl_->devices.size()); + for (auto& [id, slot] : impl_->devices) { + (void)id; + if (slot.publisher_binding) { + slot.publisher_binding->active.store( + false, std::memory_order_release); + } + if (slot.endpoint) { + endpoints.push_back(slot.endpoint); + } + } + } + for (const auto& endpoint : endpoints) { + endpoint->bindPublisher({}); + } +} + +bool SafetyCoordinator::registerDevice(DeviceSafetyRegistration registration) +{ + if (registration.descriptor.device_id.empty() || + registration.descriptor.maximum_snapshot_age <= + std::chrono::milliseconds::zero()) { + return false; + } + if (registration.endpoint) { + const auto endpoint_descriptor = registration.endpoint->descriptor(); + if (endpoint_descriptor.device_id != + registration.descriptor.device_id || + endpoint_descriptor.kind != registration.descriptor.kind) { + return false; + } + } + if (registration.descriptor.default_policy == + SafetyPolicyFamily::Control && + impl_->config.fail_startup_on_missing_control_capability && + (!registration.endpoint || + (registration.descriptor.requires_safe_stop && + !registration.participant))) { + return false; + } + if (!impl_->snapshots.registerDevice(registration.descriptor)) { + return false; + } + + auto binding = std::make_shared(); + binding->coordinator = this; + { + std::lock_guard lock(impl_->mutex); + if (impl_->devices.count(registration.descriptor.device_id) != 0) { + impl_->snapshots.unregisterDevice( + registration.descriptor.device_id); + return false; + } + Impl::DeviceSlot slot; + slot.descriptor = registration.descriptor; + slot.endpoint = registration.endpoint; + slot.participant = registration.participant; + slot.publisher_binding = binding; + if (!slot.endpoint && + slot.descriptor.default_policy == SafetyPolicyFamily::Control) { + slot.software_blockers.push_back(makeBlocker( + SafetyReason::SafetyStateMissing, + BlockerScope::Device, + RecoveryRequirement::RefreshOnly, + slot.descriptor.device_id)); + } + impl_->devices.emplace(slot.descriptor.device_id, std::move(slot)); + if (registration.participant) { + const auto descriptor = registration.participant->descriptor(); + if (descriptor.participant_id.empty() || + impl_->participants.count(descriptor.participant_id) != 0) { + impl_->devices.erase(registration.descriptor.device_id); + impl_->snapshots.unregisterDevice( + registration.descriptor.device_id); + return false; + } + impl_->participants.emplace( + descriptor.participant_id, registration.participant); + auto& runtime = + impl_->participant_runtime[descriptor.participant_id]; + runtime.descriptor = descriptor; + runtime.registered = true; + } + impl_->addEventLocked( + SafetyReason::SafetyStateMissing, + registration.descriptor.device_id, + {}, + "device safety capability registered; awaiting a fresh sample"); + } + + if (registration.endpoint) { + std::weak_ptr weak_binding = binding; + registration.endpoint->bindPublisher( + [weak_binding](DeviceSafetySnapshot snapshot) { + const auto active = weak_binding.lock(); + if (!active || + !active->active.load(std::memory_order_acquire) || + active->coordinator == nullptr) { + return false; + } + return active->coordinator->publishSafetySnapshot( + std::move(snapshot)); + }); + registration.endpoint->requestSafetyRefresh(); + } else { + impl_->snapshots.markUnknown( + registration.descriptor.device_id, + SafetyReason::SafetyStateMissing); + } + return true; +} + +bool SafetyCoordinator::unregisterDevice(const std::string& device_id) +{ + std::shared_ptr endpoint; + std::string participant_id; + { + std::lock_guard lock(impl_->mutex); + const auto found = impl_->devices.find(device_id); + if (found == impl_->devices.end()) { + return false; + } + if (found->second.in_flight_dispatches != 0) { + return false; + } + found->second.publisher_binding->active.store( + false, std::memory_order_release); + endpoint = found->second.endpoint; + if (found->second.participant) { + participant_id = + found->second.participant->descriptor().participant_id; + impl_->participants.erase(participant_id); + const auto runtime = + impl_->participant_runtime.find(participant_id); + if (runtime != impl_->participant_runtime.end()) { + runtime->second.registered = false; + if (!runtime->second.barrier_active && + impl_->retained_barriers.count(participant_id) == 0U) { + impl_->participant_runtime.erase(runtime); + } + } + } + found->second.admission_state = DeviceAdmissionState::Removed; + impl_->devices.erase(found); + impl_->addEventLocked( + SafetyReason::DeviceUnavailable, + device_id, + {}, + "device safety capability unregistered"); + } + if (endpoint) { + endpoint->bindPublisher({}); + } + impl_->snapshots.unregisterDevice(device_id); + return true; +} + +bool SafetyCoordinator::registerParticipant( + std::shared_ptr participant) +{ + if (!participant) { + return false; + } + const auto descriptor = participant->descriptor(); + if (descriptor.participant_id.empty()) { + return false; + } + std::lock_guard lock(impl_->mutex); + if (!impl_->participants.emplace( + descriptor.participant_id, std::move(participant)).second) { + return false; + } + auto& runtime = impl_->participant_runtime[descriptor.participant_id]; + runtime.descriptor = descriptor; + runtime.registered = true; + return true; +} + +bool SafetyCoordinator::unregisterParticipant( + const std::string& participant_id) +{ + std::lock_guard lock(impl_->mutex); + if (impl_->participants.erase(participant_id) == 0U) { + return false; + } + const auto runtime = impl_->participant_runtime.find(participant_id); + if (runtime != impl_->participant_runtime.end()) { + runtime->second.registered = false; + if (!runtime->second.barrier_active && + impl_->retained_barriers.count(participant_id) == 0U) { + impl_->participant_runtime.erase(runtime); + } + } + return true; +} + +void SafetyCoordinator::updateDeviceRuntimeState( + const std::string& device_id, + const device::ManagedDeviceState lifecycle, + device::DeviceHealthSnapshot health) +{ + const auto view = impl_->snapshots.get(device_id); + std::lock_guard lock(impl_->mutex); + const auto found = impl_->devices.find(device_id); + if (found == impl_->devices.end()) { + return; + } + found->second.lifecycle = lifecycle; + found->second.health = std::move(health); + found->second.admission_state = + impl_->admissionStateFor(found->second, view); + impl_->state_changed.notify_all(); +} + +bool SafetyCoordinator::publishSafetySnapshot(DeviceSafetySnapshot snapshot) +{ + const std::string device_id = snapshot.device_id; + if (!impl_->snapshots.publish(std::move(snapshot))) { + return false; + } + const auto view = impl_->snapshots.get(device_id); + std::lock_guard lock(impl_->mutex); + const auto found = impl_->devices.find(device_id); + if (found == impl_->devices.end()) { + return false; + } + const auto previous = found->second.admission_state; + found->second.admission_state = + impl_->admissionStateFor(found->second, view); + bool system_blocker = false; + for (const auto& blocker : view.snapshot.blockers) { + system_blocker = system_blocker || + blocker.scope == BlockerScope::System; + } + if (system_blocker && + impl_->system_state != SystemAdmissionState::Stopping && + impl_->system_state != SystemAdmissionState::Recovering && + impl_->system_state != SystemAdmissionState::ShuttingDown && + impl_->system_state != SystemAdmissionState::Latched) { + impl_->system_state = SystemAdmissionState::Latched; + ++impl_->safety_epoch; + impl_->addEventLocked( + SafetyReason::HardwareUnsafe, + device_id, + {}, + "system-scope hardware blocker latched admission"); + } else if (previous != found->second.admission_state) { + impl_->addEventLocked( + found->second.admission_state == DeviceAdmissionState::Open + ? SafetyReason::None + : SafetyReason::HardwareUnsafe, + device_id, + {}, + std::string("device admission state changed to ") + + toString(found->second.admission_state)); + } + impl_->state_changed.notify_all(); + return true; +} + +std::optional SafetyCoordinator::advanceDeviceGeneration( + const std::string& device_id) +{ + const auto generation = impl_->snapshots.bumpGeneration(device_id); + if (!generation.has_value()) { + return std::nullopt; + } + std::shared_ptr endpoint; + { + std::lock_guard lock(impl_->mutex); + const auto found = impl_->devices.find(device_id); + if (found == impl_->devices.end()) { + return std::nullopt; + } + found->second.admission_state = DeviceAdmissionState::Observing; + endpoint = found->second.endpoint; + impl_->addEventLocked( + SafetyReason::GenerationMismatch, + device_id, + {}, + "device backend generation advanced; awaiting a fresh sample"); + impl_->state_changed.notify_all(); + } + if (endpoint) { + endpoint->onDeviceGenerationChanged(*generation); + endpoint->requestSafetyRefresh(); + } + return generation; +} + +StartupCoverageResult SafetyCoordinator::validateStartupCoverage( + const SafetyClock::time_point deadline) +{ + struct Target { + DeviceSafetyDescriptor descriptor; + std::shared_ptr endpoint; + bool has_participant{false}; + }; + + StartupCoverageResult result; + std::vector targets; + std::unordered_set target_ids; + bool require_fresh_snapshot = false; + { + std::lock_guard lock(impl_->mutex); + const auto mode = impl_->config.enforcement_mode; + require_fresh_snapshot = + mode == EnforcementMode::EnforceSelected || + mode == EnforcementMode::EnforceAll; + + const auto add_target = [&](const Impl::DeviceSlot& slot) { + if (target_ids.insert(slot.descriptor.device_id).second) { + targets.push_back({ + slot.descriptor, + slot.endpoint, + static_cast(slot.participant)}); + } + }; + + if (mode == EnforcementMode::EnforceSelected) { + if (impl_->config.enforced_device_ids.empty()) { + result.issues.push_back({ + "system", + SafetyReason::InvalidArgument, + "ENFORCE_SELECTED requires at least one enforced_device_id"}); + } + for (const auto& id : impl_->config.enforced_device_ids) { + const auto found = impl_->devices.find(id); + if (found == impl_->devices.end()) { + result.issues.push_back({ + id, + SafetyReason::DeviceNotFound, + "enforced device is not registered"}); + continue; + } + add_target(found->second); + } + } + + if (mode == EnforcementMode::EnforceAll || + impl_->config.fail_startup_on_missing_control_capability) { + for (const auto& [id, slot] : impl_->devices) { + (void)id; + if (slot.descriptor.default_policy == + SafetyPolicyFamily::Control) { + add_target(slot); + } + } + } + } + + std::sort(targets.begin(), targets.end(), [](const auto& lhs, + const auto& rhs) { + return lhs.descriptor.device_id < rhs.descriptor.device_id; + }); + for (const auto& target : targets) { + if (!target.endpoint) { + result.issues.push_back({ + target.descriptor.device_id, + SafetyReason::SafetyStateMissing, + "enforced device has no final hardware safety endpoint"}); + } + if (target.descriptor.requires_safe_stop && + !target.has_participant) { + result.issues.push_back({ + target.descriptor.device_id, + SafetyReason::StopUnconfirmed, + "enforced device requires safe stop but has no participant"}); + } + if (require_fresh_snapshot && target.endpoint) { + target.endpoint->requestSafetyRefresh(); + } + } + + if (require_fresh_snapshot) { + for (;;) { + bool all_fresh = true; + for (const auto& target : targets) { + if (!target.endpoint) { + continue; + } + const auto view = impl_->snapshots.get( + target.descriptor.device_id); + if (!view.registered || !view.has_sample || !view.fresh) { + all_fresh = false; + break; + } + } + if (all_fresh || SafetyClock::now() >= deadline) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + + for (const auto& target : targets) { + if (!target.endpoint) { + continue; + } + const auto view = impl_->snapshots.get( + target.descriptor.device_id); + if (!view.registered || !view.has_sample) { + result.issues.push_back({ + target.descriptor.device_id, + SafetyReason::SafetyStateMissing, + "enforced device did not publish a startup safety snapshot"}); + } else if (!view.fresh) { + result.issues.push_back({ + target.descriptor.device_id, + SafetyReason::SafetyStateStale, + "enforced device startup safety snapshot is stale"}); + } + } + } + + std::sort(result.issues.begin(), result.issues.end(), + [](const auto& lhs, const auto& rhs) { + return lhs.target_id != rhs.target_id + ? lhs.target_id < rhs.target_id + : static_cast(lhs.reason) < + static_cast(rhs.reason); + }); + result.ready = result.issues.empty(); + return result; +} + +void SafetyCoordinator::markStartupComplete() +{ + std::lock_guard lock(impl_->mutex); + if (impl_->system_state != SystemAdmissionState::Starting) { + return; + } + impl_->system_state = impl_->hasGlobalLatchLocked() + ? SystemAdmissionState::Latched + : SystemAdmissionState::Open; + impl_->addEventLocked( + impl_->system_state == SystemAdmissionState::Open + ? SafetyReason::None + : SafetyReason::SafetyLatched, + "system", + {}, + "startup safety reconciliation completed"); + impl_->state_changed.notify_all(); +} + +void SafetyCoordinator::beginShutdown() noexcept +{ + if (!impl_) { + return; + } + try { + std::lock_guard lock(impl_->mutex); + if (impl_->system_state == SystemAdmissionState::ShuttingDown) { + return; + } + impl_->system_state = SystemAdmissionState::ShuttingDown; + ++impl_->safety_epoch; + impl_->addEventLocked( + SafetyReason::SystemStopping, + "system", + {}, + "safety coordinator is shutting down"); + impl_->state_changed.notify_all(); + } catch (...) { + } +} + +AdmissionDecision SafetyCoordinator::evaluate( + const AdmissionRequest& request) const +{ + AdmissionDecision decision; + SystemAdmissionState system_state; + DeviceAdmissionState device_state{DeviceAdmissionState::Observing}; + std::uint64_t epoch = 0; + bool enforced = false; + bool device_exists = false; + { + std::lock_guard lock(impl_->mutex); + system_state = impl_->system_state; + epoch = impl_->safety_epoch; + enforced = impl_->isEnforced(request.device_id); + const auto found = impl_->devices.find(request.device_id); + if (found != impl_->devices.end()) { + device_exists = true; + device_state = found->second.admission_state; + } + } + decision.safety_epoch = epoch; + decision.enforced = enforced; + + const auto reject = [&](const SafetyReason reason, std::string detail) { + decision.policy_allowed = false; + decision.allowed = !decision.enforced; + decision.reason = reason; + decision.detail = std::move(detail); + return decision; + }; + const auto allow = [&]() { + decision.policy_allowed = true; + decision.allowed = true; + decision.reason = SafetyReason::None; + return decision; + }; + + if (request.command.intent == CommandIntent::Observe) { + return allow(); + } + if (request.command.intent == CommandIntent::Stop || + request.command.intent == CommandIntent::ResetFault || + request.command.intent == CommandIntent::RecoverAdmission || + request.command.safety_lane) { + return allow(); + } + if (request.deadline <= SafetyClock::now()) { + return reject( + SafetyReason::DeadlineExceededBeforeDispatch, + "command deadline expired before admission"); + } + if (system_state != SystemAdmissionState::Open) { + return reject( + system_state == SystemAdmissionState::Starting + ? SafetyReason::SystemStarting + : system_state == SystemAdmissionState::Stopping || + system_state == SystemAdmissionState::ShuttingDown + ? SafetyReason::SystemStopping + : SafetyReason::SafetyLatched, + std::string("system admission is ") + toString(system_state)); + } + if (request.device_id.empty() || !device_exists) { + return reject( + SafetyReason::DeviceNotFound, + "device is not registered with the safety coordinator"); + } + if (device_state == DeviceAdmissionState::Quarantined) { + return reject( + SafetyReason::SafetyLatched, + "device is quarantined after an uncertain result"); + } + + const auto view = impl_->snapshots.get(request.device_id); + if (!view.has_sample) { + return reject( + SafetyReason::SafetyStateMissing, + "device has not published a safety sample"); + } + decision.device_generation = view.snapshot.device_generation; + if (!view.fresh) { + return reject( + SafetyReason::SafetyStateStale, + "device safety sample is stale"); + } + if (request.expected_device_generation.has_value() && + *request.expected_device_generation != + view.snapshot.device_generation) { + return reject( + SafetyReason::GenerationMismatch, + "expected device generation does not match the active backend"); + } + if (view.snapshot.condition == SafetyCondition::Unknown) { + return reject( + SafetyReason::SafetyStateMissing, + "hardware safety condition is unknown"); + } + if (view.snapshot.connected != TriState::True) { + return reject( + SafetyReason::DeviceDisconnected, + "device connection is not confirmed"); + } + if (hasTrue(view.snapshot.emergency_stop_active)) { + return reject( + SafetyReason::EmergencyStopActive, + "hardware emergency stop is active"); + } + if (hasTrue(view.snapshot.protective_stop_active)) { + return reject( + SafetyReason::ProtectiveStopActive, + "hardware protective stop is active"); + } + if (hasTrue(view.snapshot.fault_active)) { + return reject(SafetyReason::DeviceFault, "device fault is active"); + } + if (view.snapshot.condition == SafetyCondition::Unsafe) { + return reject(SafetyReason::HardwareUnsafe, "hardware is unsafe"); + } + if (request.command.policy_family == SafetyPolicyFamily::Sensor) { + return allow(); + } + if (view.snapshot.emergency_stop_active != TriState::False) { + return reject( + SafetyReason::EmergencyStopActive, + "control device emergency stop state is active or unknown"); + } + if (view.snapshot.protective_stop_active != TriState::False) { + return reject( + SafetyReason::ProtectiveStopActive, + "control device protective stop state is active or unknown"); + } + if (view.snapshot.fault_active != TriState::False) { + return reject( + SafetyReason::DeviceFault, + "control device fault state is active or unknown"); + } + if (request.command.intent == CommandIntent::Actuate && + view.snapshot.condition != SafetyCondition::Nominal) { + return reject( + SafetyReason::HardwareUnsafe, + "control actuation requires nominal hardware safety state"); + } + const bool safe_start_from_restricted = + request.command.intent == CommandIntent::StartActivity && + view.snapshot.condition == SafetyCondition::Restricted; + if (isControlIntent(request.command.intent) && + view.snapshot.operational_ready != TriState::True && + !safe_start_from_restricted) { + return reject( + SafetyReason::DeviceNotReady, + "control device is not confirmed operationally ready"); + } + return allow(); +} + +AdmissionResult SafetyCoordinator::admit(const AdmissionRequest& request) +{ + AdmissionResult result; + result.decision = evaluate(request); + if (!result.decision.allowed) { + return result; + } + AdmissionPermit permit; + permit.command_id = request.command_id; + permit.device_id = request.device_id; + permit.intent = request.command.intent; + permit.safety_epoch = result.decision.safety_epoch; + permit.device_generation = result.decision.device_generation; + permit.authority_generation = request.authority_generation; + permit.deadline = request.deadline; + permit.policy_allowed = result.decision.policy_allowed; + permit.enforced = result.decision.enforced; + result.permit.emplace(std::move(permit)); + return result; +} + +HardwareCheckResult SafetyCoordinator::revalidatePermit( + const AdmissionPermit& permit) const +{ + const auto rejected = [](const SafetyReason reason, std::string detail) { + return HardwareCheckResult{false, reason, std::move(detail)}; + }; + const bool safety_lane = + permit.intent == CommandIntent::Stop || + permit.intent == CommandIntent::ResetFault || + permit.intent == CommandIntent::RecoverAdmission; + + std::lock_guard lock(impl_->mutex); + const auto found = impl_->devices.find(permit.device_id); + if (found == impl_->devices.end()) { + return rejected( + SafetyReason::DeviceNotFound, + "device was removed after session admission"); + } + if (permit.deadline <= SafetyClock::now()) { + return rejected( + SafetyReason::DeadlineExceededBeforeDispatch, + "session deadline expired before dispatch"); + } + + const auto view = impl_->snapshots.get(permit.device_id); + const bool safe_start_from_restricted = + permit.intent == CommandIntent::StartActivity && view.fresh && + view.snapshot.condition == SafetyCondition::Restricted; + if (permit.enforced && !safety_lane && + (impl_->system_state != SystemAdmissionState::Open || + permit.safety_epoch != impl_->safety_epoch || + (found->second.admission_state != DeviceAdmissionState::Open && + !safe_start_from_restricted))) { + return rejected( + impl_->system_state == SystemAdmissionState::Stopping || + impl_->system_state == SystemAdmissionState::ShuttingDown + ? SafetyReason::SystemStopping + : SafetyReason::SafetyLatched, + "safety admission changed after session open"); + } + if (permit.enforced && !safety_lane && !view.fresh) { + return rejected( + SafetyReason::SafetyStateStale, + "device safety sample became stale during the session"); + } + if (permit.enforced && !safety_lane && + permit.device_generation != view.snapshot.device_generation) { + return rejected( + SafetyReason::GenerationMismatch, + "device generation changed during the session"); + } + return {true, SafetyReason::None, {}}; +} + +DispatchGuard SafetyCoordinator::beginDispatch( + const AdmissionPermit& permit) +{ + const auto rejected = [&permit]( + const SafetyReason reason, + std::string detail) { + return DispatchGuard( + nullptr, + permit.device_id, + HardwareCheckResult{false, reason, std::move(detail)}); + }; + const bool safety_lane = + permit.intent == CommandIntent::Stop || + permit.intent == CommandIntent::ResetFault || + permit.intent == CommandIntent::RecoverAdmission; + std::shared_ptr endpoint; + { + std::lock_guard lock(impl_->mutex); + const auto found = impl_->devices.find(permit.device_id); + if (found == impl_->devices.end()) { + return rejected( + SafetyReason::DeviceNotFound, + "device was removed before dispatch"); + } + if (permit.deadline <= SafetyClock::now()) { + return rejected( + SafetyReason::DeadlineExceededBeforeDispatch, + "command deadline expired before dispatch"); + } + const auto view = impl_->snapshots.get(permit.device_id); + const bool safe_start_from_restricted = + permit.intent == CommandIntent::StartActivity && view.fresh && + view.snapshot.condition == SafetyCondition::Restricted; + if (permit.enforced && !safety_lane && + (impl_->system_state != SystemAdmissionState::Open || + permit.safety_epoch != impl_->safety_epoch || + (found->second.admission_state != DeviceAdmissionState::Open && + !safe_start_from_restricted))) { + return rejected( + impl_->system_state == SystemAdmissionState::Stopping || + impl_->system_state == + SystemAdmissionState::ShuttingDown + ? SafetyReason::SystemStopping + : SafetyReason::SafetyLatched, + "safety admission changed before dispatch"); + } + if (permit.enforced && !safety_lane && !view.fresh) { + return rejected( + SafetyReason::SafetyStateStale, + "device safety sample became stale before dispatch"); + } + if (permit.enforced && !safety_lane && + permit.device_generation != view.snapshot.device_generation) { + return rejected( + SafetyReason::GenerationMismatch, + "device generation changed before dispatch"); + } + ++found->second.in_flight_dispatches; + endpoint = found->second.endpoint; + } + + HardwareCheckResult hardware_check{ + true, SafetyReason::None, {}}; + if (permit.enforced) { + if (!endpoint) { + hardware_check = { + false, + SafetyReason::SafetyStateMissing, + "device has no final hardware safety endpoint"}; + } else { + try { + hardware_check = endpoint->validateBeforeDispatch(permit); + } catch (const std::exception& error) { + hardware_check = { + false, SafetyReason::InternalError, error.what()}; + } catch (...) { + hardware_check = { + false, + SafetyReason::InternalError, + "final hardware check threw an unknown exception"}; + } + } + } + if (!hardware_check.safe) { + endDispatch_(permit.device_id); + return DispatchGuard( + nullptr, permit.device_id, std::move(hardware_check)); + } + return DispatchGuard(this, permit.device_id, std::move(hardware_check)); +} + +void SafetyCoordinator::endDispatch_(const std::string& device_id) noexcept +{ + try { + std::lock_guard lock(impl_->mutex); + const auto found = impl_->devices.find(device_id); + if (found != impl_->devices.end() && + found->second.in_flight_dispatches != 0) { + --found->second.in_flight_dispatches; + impl_->state_changed.notify_all(); + } + } catch (...) { + } +} + +void SafetyCoordinator::quarantineDevice( + const std::string& device_id, + const SafetyReason reason, + std::string operation_id) +{ + std::lock_guard lock(impl_->mutex); + const auto found = impl_->devices.find(device_id); + if (found == impl_->devices.end()) { + return; + } + found->second.quarantined = true; + found->second.admission_state = DeviceAdmissionState::Quarantined; + found->second.software_blockers.push_back(makeBlocker( + reason, + BlockerScope::Device, + RecoveryRequirement::ClearSoftwareLatch, + device_id, + operation_id)); + impl_->addEventLocked( + reason, + device_id, + std::move(operation_id), + "device quarantined"); + impl_->state_changed.notify_all(); +} + +StopAllResult SafetyCoordinator::stopAll( + std::string operation_id, + SafetyClock::time_point deadline) +{ + if (operation_id.empty()) { + operation_id = makeOperationId("stop"); + } + if (deadline == SafetyClock::time_point::max()) { + deadline = SafetyClock::now() + impl_->config.stop_all_timeout; + } + + std::shared_ptr round; + bool runner = false; + { + std::lock_guard lock(impl_->rounds_mutex); + if (impl_->active_stop_round && + !impl_->active_stop_round->done.load(std::memory_order_acquire)) { + round = impl_->active_stop_round; + } else { + round = std::make_shared(); + round->result.operation_id = std::move(operation_id); + impl_->active_stop_round = round; + runner = true; + } + } + + if (!runner) { + std::unique_lock lock(round->mutex); + if (!round->completed.wait_until( + lock, deadline, [&round] { + return round->done.load(std::memory_order_acquire); + })) { + StopAllResult timeout; + timeout.operation_id = round->result.operation_id; + timeout.success = false; + timeout.system_state = SystemAdmissionState::Stopping; + timeout.targets.push_back({ + "system", + false, + SafetyReason::ParticipantTimeout, + "caller timed out while joining the active StopAll round"}); + return timeout; + } + return round->result; + } + + StopAllResult result; + result.operation_id = round->result.operation_id; + std::unique_lock operation_lock(impl_->operation_execution_mutex); + + struct Work { + ParticipantDescriptor descriptor; + std::shared_ptr participant; + BarrierToken token; + std::future request_future; + std::future verify_future; + SafetyClock::time_point participant_deadline{ + SafetyClock::time_point::max()}; + bool request_completed{false}; + bool result_recorded{false}; + bool reused_retained_barrier{false}; + DeviceAdmissionState before{DeviceAdmissionState::Observing}; + }; + std::vector work; + { + std::lock_guard lock(impl_->mutex); + result.previous_safety_epoch = impl_->safety_epoch; + impl_->system_state = SystemAdmissionState::Stopping; + ++impl_->safety_epoch; + result.current_safety_epoch = impl_->safety_epoch; + impl_->active_operation_id = result.operation_id; + impl_->active_operation_phase = "barrier"; + impl_->addEventLocked( + SafetyReason::SystemStopping, + "system", + result.operation_id, + "StopAll entered the stopping state"); + work.reserve( + impl_->participants.size() + impl_->retained_barriers.size()); + for (const auto& [id, participant] : impl_->participants) { + Work item; + const auto retained = impl_->retained_barriers.find(id); + if (retained != impl_->retained_barriers.end()) { + item.participant = retained->second.participant; + item.token = retained->second.token; + item.reused_retained_barrier = true; + } else { + item.participant = participant; + } + item.descriptor = item.participant->descriptor(); + const auto device = impl_->devices.find(id); + if (device != impl_->devices.end()) { + item.before = device->second.admission_state; + } + work.push_back(std::move(item)); + } + for (const auto& [id, retained] : impl_->retained_barriers) { + if (impl_->participants.count(id) != 0) { + continue; + } + Work item; + item.participant = retained.participant; + item.descriptor = item.participant->descriptor(); + item.token = retained.token; + item.reused_retained_barrier = true; + const auto device = impl_->devices.find(id); + if (device != impl_->devices.end()) { + item.before = device->second.admission_state; + } + work.push_back(std::move(item)); + } + } + std::sort(work.begin(), work.end(), [](const auto& lhs, const auto& rhs) { + const auto left_phase = phaseRank(lhs.descriptor.phase); + const auto right_phase = phaseRank(rhs.descriptor.phase); + return left_phase != right_phase + ? left_phase < right_phase + : lhs.descriptor.participant_id < rhs.descriptor.participant_id; + }); + + SafetyOperationContext context{ + result.operation_id, + result.current_safety_epoch, + deadline}; + bool required_success = true; + for (auto& item : work) { + if (item.reused_retained_barrier) { + continue; + } + try { + item.token = item.participant->beginBarrier(context); + } catch (const std::exception& error) { + result.targets.push_back({ + item.descriptor.participant_id, + false, + SafetyReason::InternalError, + error.what(), + item.before, + item.before}); + item.result_recorded = true; + } catch (...) { + result.targets.push_back({ + item.descriptor.participant_id, + false, + SafetyReason::InternalError, + "beginBarrier threw an unknown exception", + item.before, + item.before}); + item.result_recorded = true; + } + if (!item.token.valid()) { + if (!item.result_recorded) { + result.targets.push_back({ + item.descriptor.participant_id, + false, + SafetyReason::StopUnconfirmed, + "participant did not establish its safety barrier", + item.before, + item.before}); + item.result_recorded = true; + } + if (item.descriptor.required) { + required_success = false; + } + continue; + } + { + std::lock_guard lock(impl_->mutex); + auto& runtime = impl_->participant_runtime[ + item.descriptor.participant_id]; + runtime.descriptor = item.descriptor; + runtime.registered = + impl_->participants.count( + item.descriptor.participant_id) != 0U; + runtime.barrier_active = true; + runtime.barrier_retained = item.reused_retained_barrier; + runtime.operation_id = item.token.operation_id; + runtime.safety_epoch = item.token.safety_epoch; + runtime.last_request = {}; + runtime.last_verify = {}; + runtime.last_release = {}; + } + } + + // Dispatch every physical stop request before waiting for a slow task, + // session, or driver. Phase ordering is applied to verification below. + for (int phase = 0; phase <= 5; ++phase) { + { + std::lock_guard lock(impl_->mutex); + impl_->active_operation_phase = + "request-phase-" + std::to_string(phase); + } + for (auto& item : work) { + if (phaseRank(item.descriptor.phase) != phase || + !item.token.valid()) { + continue; + } + const auto participant = item.participant; + const auto token = item.token; + item.participant_deadline = std::min( + deadline, SafetyClock::now() + item.descriptor.timeout); + item.request_future = impl_->operation_executor.submit( + [participant, token, context] { + return participant->requestQuiesce(token, context); + }); + } + } + + const auto record_result = [&](Work& item, + ParticipantResult participant_result) { + DeviceAdmissionState after = item.before; + { + std::lock_guard lock(impl_->mutex); + const auto device = impl_->devices.find( + item.descriptor.participant_id); + if (device != impl_->devices.end()) { + if (!participant_result.success && + item.descriptor.required) { + device->second.quarantined = true; + device->second.admission_state = + DeviceAdmissionState::Quarantined; + device->second.software_blockers.push_back(makeBlocker( + participant_result.reason, + BlockerScope::Device, + RecoveryRequirement::ClearSoftwareLatch, + item.descriptor.participant_id, + result.operation_id)); + } + after = device->second.admission_state; + } + } + result.targets.push_back({ + item.descriptor.participant_id, + participant_result.success, + participant_result.reason, + std::move(participant_result.detail), + item.before, + after}); + item.result_recorded = true; + if (!participant_result.success && item.descriptor.required) { + required_success = false; + } + }; + + const auto record_participant_result = [this]( + const std::string& participant_id, + const char* stage, + const ParticipantResult& participant_result) { + std::lock_guard lock(impl_->mutex); + const auto found = impl_->participant_runtime.find(participant_id); + if (found == impl_->participant_runtime.end()) { + return; + } + ParticipantResultView value{ + true, + participant_result.success, + participant_result.reason, + participant_result.detail}; + if (std::strcmp(stage, "request") == 0) { + found->second.last_request = std::move(value); + } else if (std::strcmp(stage, "verify") == 0) { + found->second.last_verify = std::move(value); + } else { + found->second.last_release = std::move(value); + } + }; + + for (int phase = 0; phase <= 5; ++phase) { + for (auto& item : work) { + if (phaseRank(item.descriptor.phase) != phase || + !item.token.valid() || !item.request_future.valid()) { + continue; + } + if (item.request_future.wait_until(item.participant_deadline) != + std::future_status::ready) { + const ParticipantResult timed_out{ + false, + SafetyReason::ParticipantTimeout, + "participant did not dispatch its stop request before its deadline"}; + record_participant_result( + item.descriptor.participant_id, "request", timed_out); + record_result(item, timed_out); + continue; + } + const auto requested = item.request_future.get(); + record_participant_result( + item.descriptor.participant_id, "request", requested); + item.request_completed = true; + if (!requested.success) { + // A completed initial failure still receives a final stop in + // verification. This preserves the retry-after-fence safety + // contract while retaining fail-closed timeout behavior. + item.request_completed = true; + } + } + } + + // Verification is phase ordered so scheduler/session fences are drained + // before actuator and peripheral participants perform their final stop. + for (int phase = 0; phase <= 5; ++phase) { + { + std::lock_guard lock(impl_->mutex); + impl_->active_operation_phase = + "verify-phase-" + std::to_string(phase); + } + for (auto& item : work) { + if (phaseRank(item.descriptor.phase) != phase || + !item.token.valid() || !item.request_completed || + item.result_recorded) { + continue; + } + const auto participant = item.participant; + const auto token = item.token; + item.verify_future = impl_->operation_executor.submit( + [participant, token, context] { + return participant->verifyQuiescent(token, context); + }); + } + for (auto& item : work) { + if (phaseRank(item.descriptor.phase) != phase || + !item.verify_future.valid() || item.result_recorded) { + continue; + } + if (item.verify_future.wait_until(item.participant_deadline) != + std::future_status::ready) { + const ParticipantResult timed_out{ + false, + SafetyReason::ParticipantTimeout, + "participant did not confirm quiescence before its deadline"}; + record_participant_result( + item.descriptor.participant_id, "verify", timed_out); + record_result(item, timed_out); + } else { + const auto verified = item.verify_future.get(); + record_participant_result( + item.descriptor.participant_id, "verify", verified); + record_result(item, verified); + } + } + } + + { + std::unique_lock lock(impl_->mutex); + impl_->active_operation_phase = "dispatch-fence"; + const auto no_in_flight = [&] { + return std::all_of( + impl_->devices.begin(), impl_->devices.end(), + [](const auto& item) { + return item.second.in_flight_dispatches == 0; + }); + }; + if (!impl_->state_changed.wait_until(lock, deadline, no_in_flight)) { + required_success = false; + result.targets.push_back({ + "dispatch-fence", + false, + SafetyReason::ParticipantTimeout, + "an admitted command was still inside its device submission boundary"}); + } + } + + const auto retain_barrier = [&](const Work& item) { + std::lock_guard lock(impl_->mutex); + impl_->retained_barriers[item.descriptor.participant_id] = { + item.participant, item.token}; + auto& runtime = impl_->participant_runtime[ + item.descriptor.participant_id]; + runtime.descriptor = item.descriptor; + runtime.barrier_active = true; + runtime.barrier_retained = true; + runtime.operation_id = item.token.operation_id; + runtime.safety_epoch = item.token.safety_epoch; + }; + const auto forget_retained_barrier = [&](const Work& item) { + std::lock_guard lock(impl_->mutex); + const auto found = impl_->retained_barriers.find( + item.descriptor.participant_id); + if (found != impl_->retained_barriers.end() && + found->second.token.operation_id == item.token.operation_id && + found->second.token.safety_epoch == item.token.safety_epoch && + found->second.token.generation == item.token.generation) { + impl_->retained_barriers.erase(found); + } + const auto runtime = impl_->participant_runtime.find( + item.descriptor.participant_id); + if (runtime != impl_->participant_runtime.end() && + runtime->second.operation_id == item.token.operation_id && + runtime->second.safety_epoch == item.token.safety_epoch) { + runtime->second.barrier_active = false; + runtime->second.barrier_retained = false; + if (!runtime->second.registered) { + impl_->participant_runtime.erase(runtime); + } + } + }; + const auto mark_release_failure = [&](Work& item, + ParticipantResult released) { + if (released.reason == SafetyReason::None) { + released.reason = SafetyReason::StopUnconfirmed; + } + const auto target = std::find_if( + result.targets.begin(), result.targets.end(), + [&item](const auto& candidate) { + return candidate.target_id == + item.descriptor.participant_id; + }); + if (target != result.targets.end()) { + target->success = false; + target->reason = released.reason; + target->detail = std::move(released.detail); + } else { + result.targets.push_back({ + item.descriptor.participant_id, + false, + released.reason, + std::move(released.detail), + item.before, + item.before}); + } + if (item.descriptor.required) { + required_success = false; + std::lock_guard lock(impl_->mutex); + const auto device = impl_->devices.find( + item.descriptor.participant_id); + if (device != impl_->devices.end()) { + device->second.quarantined = true; + device->second.admission_state = + DeviceAdmissionState::Quarantined; + device->second.software_blockers.push_back(makeBlocker( + released.reason, + BlockerScope::Device, + RecoveryRequirement::ClearSoftwareLatch, + item.descriptor.participant_id, + result.operation_id)); + } + } + }; + + // Commit admission reopening in reverse phase order. Ingress is therefore + // the final required barrier released; a failed subsystem commit keeps it + // closed and prevents a transient normal-command admission window. + for (auto current = work.rbegin(); current != work.rend(); ++current) { + auto& item = *current; + if (!item.token.valid()) { + continue; + } + const auto target = std::find_if( + result.targets.begin(), result.targets.end(), + [&item](const auto& candidate) { + return candidate.target_id == + item.descriptor.participant_id; + }); + const bool participant_verified = + target != result.targets.end() && target->success; + if (item.descriptor.required && + (!required_success || !participant_verified)) { + retain_barrier(item); + continue; + } + + const auto released = item.participant->releaseBarrier(item.token); + record_participant_result( + item.descriptor.participant_id, "release", released); + if (!released.success) { + mark_release_failure(item, released); + if (item.descriptor.required) { + retain_barrier(item); + } + } else { + forget_retained_barrier(item); + } + } + + { + std::lock_guard lock(impl_->mutex); + result.success = required_success; + impl_->system_state = required_success + ? SystemAdmissionState::Open + : SystemAdmissionState::Latched; + result.system_state = impl_->system_state; + impl_->active_operation_id.clear(); + impl_->active_operation_phase.clear(); + impl_->addEventLocked( + required_success + ? SafetyReason::None + : SafetyReason::StopUnconfirmed, + "system", + result.operation_id, + required_success + ? "StopAll confirmed all required participants quiescent" + : "StopAll left admission latched because a required participant was unconfirmed"); + impl_->state_changed.notify_all(); + } + + { + std::lock_guard lock(round->mutex); + round->result = result; + round->done.store(true, std::memory_order_release); + } + round->completed.notify_all(); + { + std::lock_guard lock(impl_->rounds_mutex); + if (impl_->active_stop_round == round) { + impl_->active_stop_round.reset(); + } + } + return result; +} + +RecoveryResult SafetyCoordinator::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())) { + invalid.result = RecoveryResultCode::Failed; + invalid.targets.push_back({ + "system", + false, + request.reason.empty() + ? SafetyReason::RecoveryReasonRequired + : SafetyReason::InvalidArgument, + "recovery_id, reason, and an explicit non-empty scope are required"}); + return invalid; + } + std::unordered_set unique_ids; + for (const auto& id : request.device_ids) { + if (id.empty() || !unique_ids.insert(id).second) { + invalid.result = RecoveryResultCode::Failed; + invalid.targets.push_back({ + id, + false, + SafetyReason::InvalidArgument, + "recovery device IDs must be non-empty and unique"}); + return invalid; + } + } + + std::vector fingerprint_ids = request.device_ids; + std::sort(fingerprint_ids.begin(), fingerprint_ids.end()); + std::ostringstream fingerprint; + fingerprint << request.expected_safety_epoch << ':' + << request.verify_only << ':' << request.all_devices << ':' + << request.reason; + for (const auto& id : fingerprint_ids) { + fingerprint << ':' << id; + } + + std::shared_ptr round; + bool runner = false; + { + std::lock_guard lock(impl_->rounds_mutex); + const auto found = impl_->recovery_rounds.find(request.recovery_id); + if (found != impl_->recovery_rounds.end()) { + round = found->second; + if (round->fingerprint != fingerprint.str()) { + invalid.result = RecoveryResultCode::Failed; + invalid.targets.push_back({ + "system", + false, + SafetyReason::CommandIdConflict, + "recovery_id is already associated with a different request"}); + return invalid; + } + } else { + round = std::make_shared(); + round->fingerprint = fingerprint.str(); + impl_->recovery_rounds.emplace(request.recovery_id, round); + runner = true; + } + } + + auto deadline = request.deadline; + if (deadline == SafetyClock::time_point::max()) { + deadline = SafetyClock::now() + impl_->config.recovery_timeout; + } + if (!runner) { + std::unique_lock lock(round->mutex); + if (!round->completed.wait_until( + lock, deadline, [&round] { + return round->done.load(std::memory_order_acquire); + })) { + invalid.result = RecoveryResultCode::TimedOut; + invalid.targets.push_back({ + "system", + false, + SafetyReason::ParticipantTimeout, + "caller timed out while joining the recovery transaction"}); + return invalid; + } + return round->result; + } + + RecoveryResult result; + result.recovery_id = request.recovery_id; + std::unique_lock operation_lock(impl_->operation_execution_mutex); + std::vector target_ids; + std::unordered_map + target_before_states; + SystemAdmissionState state_before_recovery{ + SystemAdmissionState::Starting}; + { + std::lock_guard lock(impl_->mutex); + result.previous_safety_epoch = impl_->safety_epoch; + state_before_recovery = impl_->system_state; + if (request.expected_safety_epoch != impl_->safety_epoch) { + result.result = RecoveryResultCode::EpochMismatch; + result.current_safety_epoch = impl_->safety_epoch; + result.system_state = impl_->system_state; + result.targets.push_back({ + "system", + false, + SafetyReason::RecoveryEpochMismatch, + "expected safety epoch does not match the active state"}); + } else if (impl_->system_state == + SystemAdmissionState::ShuttingDown) { + result.result = RecoveryResultCode::Failed; + result.current_safety_epoch = impl_->safety_epoch; + result.system_state = impl_->system_state; + result.targets.push_back({ + "system", + false, + SafetyReason::SystemStopping, + "recovery is unavailable during shutdown"}); + } else { + impl_->system_state = SystemAdmissionState::Recovering; + ++impl_->safety_epoch; + result.current_safety_epoch = impl_->safety_epoch; + impl_->active_operation_id = request.recovery_id; + impl_->active_operation_phase = "refresh"; + if (request.all_devices) { + target_ids.reserve(impl_->devices.size()); + for (auto& [id, slot] : impl_->devices) { + target_ids.push_back(id); + target_before_states.emplace(id, slot.admission_state); + if (slot.admission_state == + DeviceAdmissionState::Quarantined || + slot.admission_state == + DeviceAdmissionState::Blocked) { + slot.admission_state = + DeviceAdmissionState::Recovering; + } + } + } else { + target_ids = request.device_ids; + for (const auto& id : target_ids) { + const auto found = impl_->devices.find(id); + if (found != impl_->devices.end()) { + target_before_states.emplace( + id, found->second.admission_state); + } + if (found != impl_->devices.end() && + (found->second.admission_state == + DeviceAdmissionState::Quarantined || + found->second.admission_state == + DeviceAdmissionState::Blocked)) { + found->second.admission_state = + DeviceAdmissionState::Recovering; + } + } + } + impl_->addEventLocked( + SafetyReason::None, + "system", + request.recovery_id, + request.verify_only + ? "recovery verification started" + : "software latch recovery started"); + } + } + + if (!result.targets.empty()) { + std::lock_guard lock(round->mutex); + round->result = result; + round->done.store(true, std::memory_order_release); + round->completed.notify_all(); + return result; + } + + struct Target { + std::string id; + std::shared_ptr endpoint; + DeviceAdmissionState before{DeviceAdmissionState::Observing}; + std::uint64_t previous_sequence{0}; + SafetyPolicyFamily policy{SafetyPolicyFamily::Sensor}; + }; + std::vector targets; + { + std::lock_guard lock(impl_->mutex); + for (const auto& id : target_ids) { + const auto found = impl_->devices.find(id); + if (found == impl_->devices.end()) { + result.targets.push_back({ + id, + false, + SafetyReason::DeviceNotFound, + "device is not registered", + DeviceAdmissionState::Removed, + DeviceAdmissionState::Removed}); + continue; + } + const auto view = impl_->snapshots.get(id); + const auto before = target_before_states.find(id); + targets.push_back({ + id, + found->second.endpoint, + before == target_before_states.end() + ? found->second.admission_state + : before->second, + view.snapshot.sample_sequence, + found->second.descriptor.default_policy}); + } + } + + for (const auto& target : targets) { + if (target.endpoint) { + target.endpoint->requestSafetyRefresh(); + } + } + + bool all_verified = result.targets.empty(); + RecoveryContext recovery_context{ + request.recovery_id, + request.reason, + result.current_safety_epoch, + deadline, + request.verify_only}; + for (const auto& target : targets) { + SafetyTargetResult target_result; + target_result.target_id = target.id; + target_result.before_state = target.before; + target_result.after_state = target.before; + + SafetySnapshotView view; + if (!target.endpoint) { + target_result.reason = SafetyReason::SafetyStateMissing; + target_result.detail = "device has no safety endpoint"; + } else if (!impl_->snapshots.waitForNewerSample( + target.id, + target.previous_sequence, + deadline, + view)) { + 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; + } + + if (!target_result.success) { + all_verified = false; + } + result.targets.push_back(std::move(target_result)); + } + + std::vector retained_candidate_ids; + if (!request.verify_only) { + const bool has_device_clear_candidate = std::any_of( + result.targets.begin(), result.targets.end(), + [](const auto& target) { return target.success; }); + { + std::lock_guard lock(impl_->mutex); + retained_candidate_ids.reserve( + impl_->retained_barriers.size()); + for (const auto& [id, retained] : impl_->retained_barriers) { + (void)retained; + const auto device = impl_->devices.find(id); + if (device == impl_->devices.end()) { + retained_candidate_ids.push_back(id); + continue; + } + const auto target = std::find_if( + result.targets.begin(), result.targets.end(), + [&id](const auto& candidate) { + return candidate.target_id == id && + candidate.success; + }); + if (target != result.targets.end()) { + retained_candidate_ids.push_back(id); + } + } + } + const bool has_clear_candidate = + has_device_clear_candidate || + !retained_candidate_ids.empty(); + bool clear_authorized = true; + if (has_clear_candidate && request.authorize_clear) { + try { + clear_authorized = request.authorize_clear(); + } catch (...) { + clear_authorized = false; + } + } + if (!clear_authorized) { + for (auto& target : result.targets) { + if (!target.success) { + continue; + } + target.success = false; + target.reason = SafetyReason::RecoveryAuditFailed; + target.detail = + "persistent recovery audit did not authorize latch clear"; + } + for (const auto& id : retained_candidate_ids) { + const auto existing = std::find_if( + result.targets.begin(), result.targets.end(), + [&id](const auto& target) { + return target.target_id == id; + }); + if (existing == result.targets.end()) { + result.targets.push_back({ + id, + false, + SafetyReason::RecoveryAuditFailed, + "persistent recovery audit did not authorize subsystem latch clear", + DeviceAdmissionState::Quarantined, + DeviceAdmissionState::Quarantined}); + } + } + all_verified = false; + } else { + for (auto& target_result : result.targets) { + if (!target_result.success) { + continue; + } + const auto target = std::find_if( + targets.begin(), targets.end(), + [&target_result](const auto& candidate) { + return candidate.id == target_result.target_id; + }); + if (target == targets.end() || !target->endpoint) { + target_result.success = false; + target_result.reason = SafetyReason::SafetyStateMissing; + target_result.detail = + "device recovery endpoint disappeared"; + all_verified = false; + continue; + } + + RecoveryCheckResult reconciled{ + true, SafetyReason::None, {}}; + try { + reconciled = target->endpoint->reconcileAdmissionState( + recovery_context); + } catch (const std::exception& error) { + reconciled = { + false, SafetyReason::InternalError, error.what()}; + } catch (...) { + reconciled = { + false, + SafetyReason::InternalError, + "device recovery reconciliation threw an unknown exception"}; + } + target_result.success = reconciled.reconciled; + target_result.reason = reconciled.reason; + target_result.detail = reconciled.detail; + if (!target_result.success) { + all_verified = false; + } + } + } + } + + struct BarrierRecovery { + std::size_t target_index{0}; + std::shared_ptr participant; + BarrierToken token; + }; + std::vector barrier_recoveries; + if (!request.verify_only) { + std::lock_guard lock(impl_->mutex); + for (const auto& id : retained_candidate_ids) { + const auto barrier = impl_->retained_barriers.find(id); + if (barrier == impl_->retained_barriers.end()) { + continue; + } + auto target = std::find_if( + result.targets.begin(), result.targets.end(), + [&id](const auto& candidate) { + return candidate.target_id == id; + }); + if (target == result.targets.end()) { + result.targets.push_back({ + id, + true, + SafetyReason::None, + {}, + DeviceAdmissionState::Quarantined, + DeviceAdmissionState::Quarantined}); + target = std::prev(result.targets.end()); + } + if (!target->success) { + continue; + } + barrier_recoveries.push_back({ + static_cast( + std::distance(result.targets.begin(), target)), + barrier->second.participant, + barrier->second.token}); + } + } + for (const auto& pending : barrier_recoveries) { + RecoveryCheckResult recovered; + try { + recovered = pending.participant->recoverAdmission( + pending.token, recovery_context); + } catch (const std::exception& error) { + recovered = {false, SafetyReason::InternalError, error.what()}; + } catch (...) { + recovered = { + false, + SafetyReason::InternalError, + "participant recovery threw an unknown exception"}; + } + if (!recovered.reconciled) { + auto& target = result.targets[pending.target_index]; + target.success = false; + target.reason = recovered.reason; + target.detail = recovered.detail; + all_verified = false; + continue; + } + + bool release = false; + { + std::lock_guard lock(impl_->mutex); + const auto found = impl_->retained_barriers.find( + pending.token.participant_id); + if (found != impl_->retained_barriers.end() && + found->second.token.operation_id == pending.token.operation_id && + found->second.token.safety_epoch == pending.token.safety_epoch && + found->second.token.generation == pending.token.generation) { + release = true; + } + } + if (release) { + const auto released = + pending.participant->releaseBarrier(pending.token); + { + std::lock_guard lock(impl_->mutex); + const auto runtime = impl_->participant_runtime.find( + pending.token.participant_id); + if (runtime != impl_->participant_runtime.end()) { + runtime->second.last_release = { + true, + released.success, + released.reason, + released.detail}; + } + } + if (!released.success) { + auto& target = result.targets[pending.target_index]; + target.success = false; + target.reason = released.reason == SafetyReason::None + ? SafetyReason::StopUnconfirmed + : released.reason; + target.detail = released.detail; + all_verified = false; + continue; + } + { + std::lock_guard lock(impl_->mutex); + const auto found = impl_->retained_barriers.find( + pending.token.participant_id); + if (found != impl_->retained_barriers.end() && + found->second.token.operation_id == + pending.token.operation_id && + found->second.token.safety_epoch == + pending.token.safety_epoch && + found->second.token.generation == + pending.token.generation) { + impl_->retained_barriers.erase(found); + const auto runtime = impl_->participant_runtime.find( + pending.token.participant_id); + if (runtime != impl_->participant_runtime.end()) { + runtime->second.barrier_active = false; + runtime->second.barrier_retained = false; + if (!runtime->second.registered) { + impl_->participant_runtime.erase(runtime); + } + } + } + } + result.targets[pending.target_index].after_state = + DeviceAdmissionState::Open; + } + } + + { + std::lock_guard lock(impl_->mutex); + impl_->active_operation_phase = "reconcile"; + for (auto& target : result.targets) { + const auto device = impl_->devices.find(target.target_id); + if (device == impl_->devices.end()) { + continue; + } + if (target.success && !request.verify_only) { + device->second.quarantined = false; + device->second.software_blockers.clear(); + } + const auto view = impl_->snapshots.get(target.target_id); + device->second.admission_state = + impl_->admissionStateFor(device->second, view); + target.after_state = device->second.admission_state; + } + + const bool latch_remains = impl_->hasGlobalLatchLocked(); + if (request.verify_only) { + impl_->system_state = state_before_recovery == + SystemAdmissionState::Open && + !latch_remains + ? SystemAdmissionState::Open + : SystemAdmissionState::Latched; + result.result = all_verified + ? RecoveryResultCode::VerifiedButStillBlocked + : RecoveryResultCode::BlockerRemains; + } else if (all_verified && !latch_remains) { + impl_->system_state = SystemAdmissionState::Open; + result.result = RecoveryResultCode::Recovered; + } else { + impl_->system_state = SystemAdmissionState::Latched; + result.result = RecoveryResultCode::BlockerRemains; + } + result.system_state = impl_->system_state; + impl_->active_operation_id.clear(); + impl_->active_operation_phase.clear(); + impl_->addEventLocked( + result.result == RecoveryResultCode::Recovered + ? SafetyReason::None + : SafetyReason::SafetyLatched, + "system", + request.recovery_id, + std::string("recovery completed with result ") + + toString(result.result)); + impl_->state_changed.notify_all(); + } + + if (targets.empty() && result.targets.empty()) { + result.result = RecoveryResultCode::NothingToRecover; + } + { + std::lock_guard lock(round->mutex); + round->result = result; + round->done.store(true, std::memory_order_release); + } + round->completed.notify_all(); + return result; +} + +SafetyCoordinatorSnapshot SafetyCoordinator::snapshot() const +{ + SafetyCoordinatorSnapshot result; + std::lock_guard lock(impl_->mutex); + result.system_state = impl_->system_state; + result.safety_epoch = impl_->safety_epoch; + result.service_instance_id = impl_->service_instance_id; + result.enforcement_mode = impl_->config.enforcement_mode; + result.active_operation_id = impl_->active_operation_id; + result.active_operation_phase = impl_->active_operation_phase; + result.devices.reserve(impl_->devices.size()); + for (const auto& [id, slot] : impl_->devices) { + DeviceSafetyStateView view; + view.descriptor = slot.descriptor; + view.safety = impl_->snapshots.get(id); + view.lifecycle = slot.lifecycle; + view.health = slot.health; + view.admission_state = slot.admission_state; + view.blockers = view.safety.snapshot.blockers; + view.blockers.insert( + view.blockers.end(), + slot.software_blockers.begin(), + slot.software_blockers.end()); + result.devices.push_back(std::move(view)); + } + std::sort(result.devices.begin(), result.devices.end(), + [](const auto& lhs, const auto& rhs) { + return lhs.descriptor.device_id < rhs.descriptor.device_id; + }); + result.participants.reserve(impl_->participant_runtime.size()); + for (const auto& [id, runtime] : impl_->participant_runtime) { + (void)id; + ParticipantSafetyStateView view; + view.descriptor = runtime.descriptor; + view.registered = runtime.registered; + view.barrier_active = runtime.barrier_active; + view.barrier_retained = runtime.barrier_retained; + view.operation_id = runtime.operation_id; + view.safety_epoch = runtime.safety_epoch; + view.last_request = runtime.last_request; + view.last_verify = runtime.last_verify; + view.last_release = runtime.last_release; + result.participants.push_back(std::move(view)); + } + std::sort(result.participants.begin(), result.participants.end(), + [](const auto& lhs, const auto& rhs) { + return lhs.descriptor.participant_id < + rhs.descriptor.participant_id; + }); + result.recent_events.assign(impl_->events.begin(), impl_->events.end()); + return result; +} + +SafetySnapshotStore& SafetyCoordinator::snapshotStore() noexcept +{ + return impl_->snapshots; +} + +const SafetySnapshotStore& SafetyCoordinator::snapshotStore() const noexcept +{ + return impl_->snapshots; +} + +CommandLedger& SafetyCoordinator::commandLedger() noexcept +{ + return impl_->ledger; +} + +const CommandLedger& SafetyCoordinator::commandLedger() const noexcept +{ + return impl_->ledger; +} + +const std::string& SafetyCoordinator::serviceInstanceId() const noexcept +{ + return impl_->service_instance_id; +} + +const SafetyCoordinatorConfig& SafetyCoordinator::config() const noexcept +{ + return impl_->config; +} + +const char* toString(const RecoveryResultCode value) noexcept +{ + switch (value) { + case RecoveryResultCode::Recovered: return "Recovered"; + case RecoveryResultCode::VerifiedButStillBlocked: + return "VerifiedButStillBlocked"; + case RecoveryResultCode::BlockerRemains: return "BlockerRemains"; + case RecoveryResultCode::EpochMismatch: return "EpochMismatch"; + case RecoveryResultCode::NothingToRecover: return "NothingToRecover"; + case RecoveryResultCode::TimedOut: return "TimedOut"; + case RecoveryResultCode::Failed: return "Failed"; + } + return "Failed"; +} + +} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety/src/safety_reason.cpp b/cmvr-es/manager/safety/src/safety_reason.cpp new file mode 100644 index 00000000..14a9717b --- /dev/null +++ b/cmvr-es/manager/safety/src/safety_reason.cpp @@ -0,0 +1,92 @@ +#include "manager/safety/include/safety_reason.h" + +namespace cmvr::safety { + +const char* toString(const SafetyReason reason) noexcept +{ + switch (reason) { + case SafetyReason::None: return "NONE"; + case SafetyReason::InvalidArgument: return "INVALID_ARGUMENT"; + case SafetyReason::Unauthenticated: return "UNAUTHENTICATED"; + case SafetyReason::PermissionDenied: return "PERMISSION_DENIED"; + case SafetyReason::RecoveryRpcDisabled: return "RECOVERY_RPC_DISABLED"; + case SafetyReason::DeviceNotFound: return "DEVICE_NOT_FOUND"; + case SafetyReason::DeviceUnavailable: return "DEVICE_UNAVAILABLE"; + case SafetyReason::UnsupportedCommand: return "UNSUPPORTED_COMMAND"; + case SafetyReason::SystemStarting: return "SYSTEM_STARTING"; + case SafetyReason::SystemStopping: return "SYSTEM_STOPPING"; + case SafetyReason::SafetyLatched: return "SAFETY_LATCHED"; + case SafetyReason::SafetyStateMissing: return "SAFETY_STATE_MISSING"; + case SafetyReason::SafetyStateStale: return "SAFETY_STATE_STALE"; + case SafetyReason::HardwareUnsafe: return "HARDWARE_UNSAFE"; + case SafetyReason::EmergencyStopActive: return "EMERGENCY_STOP_ACTIVE"; + case SafetyReason::ProtectiveStopActive: return "PROTECTIVE_STOP_ACTIVE"; + case SafetyReason::DeviceDisconnected: return "DEVICE_DISCONNECTED"; + case SafetyReason::DeviceFault: return "DEVICE_FAULT"; + case SafetyReason::DeviceNotReady: return "DEVICE_NOT_READY"; + case SafetyReason::DeviceStillMoving: return "DEVICE_STILL_MOVING"; + case SafetyReason::ControlBusy: return "CONTROL_BUSY"; + case SafetyReason::GenerationMismatch: return "GENERATION_MISMATCH"; + case SafetyReason::CommandIdRequired: return "COMMAND_ID_REQUIRED"; + case SafetyReason::CommandIdConflict: return "COMMAND_ID_CONFLICT"; + case SafetyReason::ResultEvicted: return "RESULT_EVICTED"; + case SafetyReason::LedgerExhausted: return "LEDGER_EXHAUSTED"; + case SafetyReason::Backpressure: return "BACKPRESSURE"; + case SafetyReason::DeadlineExceededBeforeDispatch: + return "DEADLINE_EXCEEDED_BEFORE_DISPATCH"; + case SafetyReason::OutcomeUnknown: return "OUTCOME_UNKNOWN"; + case SafetyReason::ParticipantTimeout: return "PARTICIPANT_TIMEOUT"; + case SafetyReason::StopUnconfirmed: return "STOP_UNCONFIRMED"; + case SafetyReason::RecoveryEpochMismatch: return "RECOVERY_EPOCH_MISMATCH"; + case SafetyReason::RecoveryReasonRequired: return "RECOVERY_REASON_REQUIRED"; + case SafetyReason::RecoveryAuditFailed: return "RECOVERY_AUDIT_FAILED"; + case SafetyReason::InternalError: return "INTERNAL_ERROR"; + } + return "INTERNAL_ERROR"; +} + +bool retryWithSameCommandId(const SafetyReason reason) noexcept +{ + switch (reason) { + case SafetyReason::RecoveryRpcDisabled: + case SafetyReason::SystemStopping: + return true; + case SafetyReason::None: + case SafetyReason::InvalidArgument: + case SafetyReason::Unauthenticated: + case SafetyReason::PermissionDenied: + case SafetyReason::DeviceNotFound: + case SafetyReason::DeviceUnavailable: + case SafetyReason::UnsupportedCommand: + case SafetyReason::SystemStarting: + case SafetyReason::SafetyLatched: + case SafetyReason::SafetyStateMissing: + case SafetyReason::SafetyStateStale: + case SafetyReason::HardwareUnsafe: + case SafetyReason::EmergencyStopActive: + case SafetyReason::ProtectiveStopActive: + case SafetyReason::DeviceDisconnected: + case SafetyReason::DeviceFault: + case SafetyReason::DeviceNotReady: + case SafetyReason::DeviceStillMoving: + case SafetyReason::ControlBusy: + case SafetyReason::GenerationMismatch: + case SafetyReason::CommandIdRequired: + case SafetyReason::CommandIdConflict: + case SafetyReason::ResultEvicted: + case SafetyReason::LedgerExhausted: + case SafetyReason::Backpressure: + case SafetyReason::DeadlineExceededBeforeDispatch: + case SafetyReason::OutcomeUnknown: + case SafetyReason::ParticipantTimeout: + case SafetyReason::StopUnconfirmed: + case SafetyReason::RecoveryEpochMismatch: + case SafetyReason::RecoveryReasonRequired: + case SafetyReason::RecoveryAuditFailed: + case SafetyReason::InternalError: + return false; + } + return false; +} + +} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety/src/safety_snapshot_store.cpp b/cmvr-es/manager/safety/src/safety_snapshot_store.cpp new file mode 100644 index 00000000..dc7341d8 --- /dev/null +++ b/cmvr-es/manager/safety/src/safety_snapshot_store.cpp @@ -0,0 +1,305 @@ +#include "manager/safety/include/safety_snapshot_store.h" + +#include +#include +#include +#include + +namespace cmvr::safety { + +namespace { + +std::uint64_t unixTimeMs() noexcept +{ + const auto value = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + return value > 0 ? static_cast(value) : 1U; +} + +} // namespace + +bool SafetySnapshotStore::registerDevice( + const DeviceSafetyDescriptor& descriptor, + const std::uint64_t initial_generation) +{ + if (descriptor.device_id.empty() || + descriptor.maximum_snapshot_age <= std::chrono::milliseconds::zero() || + initial_generation == 0) { + return false; + } + + Slot slot; + slot.descriptor = descriptor; + slot.snapshot.device_id = descriptor.device_id; + slot.snapshot.device_generation = initial_generation; + slot.snapshot.condition = SafetyCondition::Unknown; + + std::unique_lock lock(mutex_); + const auto inserted = slots_.emplace(descriptor.device_id, std::move(slot)); + if (inserted.second) { + changed_.notify_all(); + } + return inserted.second; +} + +bool SafetySnapshotStore::unregisterDevice(const std::string& device_id) +{ + std::unique_lock lock(mutex_); + const bool removed = slots_.erase(device_id) != 0; + if (removed) { + changed_.notify_all(); + } + return removed; +} + +bool SafetySnapshotStore::publish(DeviceSafetySnapshot snapshot) +{ + if (snapshot.device_id.empty() || snapshot.device_generation == 0 || + snapshot.sample_sequence == 0 || + snapshot.observed_at == SafetyClock::time_point{}) { + return false; + } + + std::unique_lock lock(mutex_); + const auto found = slots_.find(snapshot.device_id); + if (found == slots_.end()) { + return false; + } + + auto& slot = found->second; + const auto current_generation = slot.snapshot.device_generation; + if (snapshot.device_generation < current_generation) { + return false; + } + if (snapshot.device_generation == current_generation && + slot.has_sample && + snapshot.sample_sequence <= slot.snapshot.sample_sequence) { + return false; + } + if (snapshot.observed_at_unix_ms == 0) { + snapshot.observed_at_unix_ms = unixTimeMs(); + } + slot.snapshot = std::move(snapshot); + slot.has_sample = true; + changed_.notify_all(); + return true; +} + +bool SafetySnapshotStore::markUnknown( + const std::string& device_id, + const SafetyReason reason, + std::string source_id) +{ + std::unique_lock lock(mutex_); + const auto found = slots_.find(device_id); + if (found == slots_.end()) { + return false; + } + + auto& slot = found->second; + DeviceSafetySnapshot snapshot; + snapshot.device_id = device_id; + snapshot.device_generation = slot.snapshot.device_generation; + snapshot.sample_sequence = slot.snapshot.sample_sequence + 1U; + if (snapshot.sample_sequence == 0) { + snapshot.sample_sequence = 1U; + } + snapshot.observed_at = SafetyClock::now(); + snapshot.observed_at_unix_ms = unixTimeMs(); + snapshot.condition = SafetyCondition::Unknown; + snapshot.blockers.push_back(SafetyBlocker{ + reason, + BlockerScope::Device, + RecoveryRequirement::RefreshOnly, + source_id.empty() ? device_id : std::move(source_id), + {}, + snapshot.observed_at_unix_ms, + snapshot.observed_at_unix_ms}); + slot.snapshot = std::move(snapshot); + slot.has_sample = true; + changed_.notify_all(); + return true; +} + +std::optional SafetySnapshotStore::bumpGeneration( + const std::string& device_id) +{ + std::unique_lock lock(mutex_); + const auto found = slots_.find(device_id); + if (found == slots_.end()) { + return std::nullopt; + } + auto& slot = found->second; + if (slot.snapshot.device_generation == + std::numeric_limits::max()) { + return std::nullopt; + } + ++slot.snapshot.device_generation; + slot.snapshot.sample_sequence = 0; + slot.snapshot.observed_at = {}; + slot.snapshot.observed_at_unix_ms = 0; + slot.snapshot.condition = SafetyCondition::Unknown; + slot.snapshot.blockers.clear(); + slot.has_sample = false; + changed_.notify_all(); + return slot.snapshot.device_generation; +} + +SafetySnapshotView SafetySnapshotStore::viewOf_( + const Slot& slot, + const SafetyClock::time_point now) +{ + SafetySnapshotView view; + view.descriptor = slot.descriptor; + view.snapshot = slot.snapshot; + view.registered = true; + view.has_sample = slot.has_sample; + if (!slot.has_sample || + slot.snapshot.observed_at == SafetyClock::time_point{}) { + return view; + } + + const auto elapsed = now <= slot.snapshot.observed_at + ? SafetyClock::duration::zero() + : now - slot.snapshot.observed_at; + view.sample_age = std::chrono::duration_cast( + elapsed); + view.fresh = view.sample_age <= slot.descriptor.maximum_snapshot_age; + return view; +} + +SafetySnapshotView SafetySnapshotStore::get( + const std::string& device_id, + const SafetyClock::time_point now) const +{ + std::shared_lock lock(mutex_); + const auto found = slots_.find(device_id); + if (found == slots_.end()) { + return {}; + } + return viewOf_(found->second, now); +} + +std::vector SafetySnapshotStore::snapshot( + const SafetyClock::time_point now) const +{ + std::vector result; + std::shared_lock lock(mutex_); + result.reserve(slots_.size()); + for (const auto& [id, slot] : slots_) { + (void)id; + result.push_back(viewOf_(slot, now)); + } + std::sort(result.begin(), result.end(), [](const auto& lhs, const auto& rhs) { + return lhs.descriptor.device_id < rhs.descriptor.device_id; + }); + return result; +} + +bool SafetySnapshotStore::waitForNewerSample( + const std::string& device_id, + const std::uint64_t previous_sequence, + const SafetyClock::time_point deadline, + SafetySnapshotView& result) const +{ + std::unique_lock lock(mutex_); + const auto ready = [&]() { + const auto found = slots_.find(device_id); + return found == slots_.end() || + (found->second.has_sample && + found->second.snapshot.sample_sequence > previous_sequence); + }; + if (!changed_.wait_until(lock, deadline, ready)) { + return false; + } + const auto found = slots_.find(device_id); + if (found == slots_.end()) { + return false; + } + result = viewOf_(found->second, SafetyClock::now()); + return result.has_sample && + result.snapshot.sample_sequence > previous_sequence; +} + +const char* toString(const TriState value) noexcept +{ + switch (value) { + case TriState::Unknown: return "Unknown"; + case TriState::False: return "False"; + case TriState::True: return "True"; + } + return "Unknown"; +} + +const char* toString(const SafetyCondition value) noexcept +{ + switch (value) { + case SafetyCondition::Nominal: return "Nominal"; + case SafetyCondition::Restricted: return "Restricted"; + case SafetyCondition::Unsafe: return "Unsafe"; + case SafetyCondition::Unknown: return "Unknown"; + } + return "Unknown"; +} + +const char* toString(const CommandIntent value) noexcept +{ + switch (value) { + case CommandIntent::Observe: return "Observe"; + case CommandIntent::StartActivity: return "StartActivity"; + case CommandIntent::Configure: return "Configure"; + case CommandIntent::Actuate: return "Actuate"; + case CommandIntent::Stop: return "Stop"; + case CommandIntent::ResetFault: return "ResetFault"; + case CommandIntent::RecoverAdmission: return "RecoverAdmission"; + } + return "Observe"; +} + +const char* toString(const SafetyPolicyFamily value) noexcept +{ + switch (value) { + case SafetyPolicyFamily::Sensor: return "Sensor"; + case SafetyPolicyFamily::Control: return "Control"; + } + return "Sensor"; +} + +const char* toString(const SystemAdmissionState value) noexcept +{ + switch (value) { + case SystemAdmissionState::Starting: return "Starting"; + case SystemAdmissionState::Open: return "Open"; + case SystemAdmissionState::Stopping: return "Stopping"; + case SystemAdmissionState::Latched: return "Latched"; + case SystemAdmissionState::Recovering: return "Recovering"; + case SystemAdmissionState::ShuttingDown: return "ShuttingDown"; + } + return "Starting"; +} + +const char* toString(const DeviceAdmissionState value) noexcept +{ + switch (value) { + case DeviceAdmissionState::Observing: return "Observing"; + case DeviceAdmissionState::Open: return "Open"; + case DeviceAdmissionState::Blocked: return "Blocked"; + case DeviceAdmissionState::Quarantined: return "Quarantined"; + case DeviceAdmissionState::Recovering: return "Recovering"; + case DeviceAdmissionState::Removed: return "Removed"; + } + return "Observing"; +} + +const char* toString(const EnforcementMode value) noexcept +{ + switch (value) { + case EnforcementMode::Legacy: return "Legacy"; + case EnforcementMode::Shadow: return "Shadow"; + case EnforcementMode::EnforceSelected: return "EnforceSelected"; + case EnforcementMode::EnforceAll: return "EnforceAll"; + } + return "Legacy"; +} + +} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety/tests/command_ledger_test.cpp b/cmvr-es/manager/safety/tests/command_ledger_test.cpp new file mode 100644 index 00000000..4515e356 --- /dev/null +++ b/cmvr-es/manager/safety/tests/command_ledger_test.cpp @@ -0,0 +1,122 @@ +#include "manager/safety/include/command_ledger.h" + +#include +#include +#include +#include + +#include + +namespace cmvr::safety { +namespace { + +CommandKey key(const std::string& id) +{ + return {"anonymous", id}; +} + +CommandOutcome completedOutcome() +{ + CommandOutcome outcome; + outcome.lifecycle = CommandLifecycle::Completed; + outcome.reason = SafetyReason::None; + outcome.serialized_response = "done"; + return outcome; +} + +TEST(CommandLedgerTest, SameIdJoinsAndDifferentPayloadConflicts) +{ + CommandLedger ledger; + const auto first = ledger.reserve(key("command-1"), "payload-a"); + ASSERT_EQ(first.status, CommandReservationStatus::AcceptedNew); + EXPECT_EQ( + ledger.reserve(key("command-1"), "payload-a").status, + CommandReservationStatus::JoinedInFlight); + EXPECT_EQ( + ledger.reserve(key("command-1"), "payload-b").status, + CommandReservationStatus::CommandIdConflict); + + ASSERT_TRUE(ledger.complete(first.ticket, completedOutcome())); + const auto cached = ledger.reserve(key("command-1"), "payload-a"); + ASSERT_EQ(cached.status, CommandReservationStatus::CachedResult); + ASSERT_TRUE(cached.cached_outcome.has_value()); + EXPECT_EQ(cached.cached_outcome->serialized_response, "done"); +} + +TEST(CommandLedgerTest, ConcurrentCallersNeverCreateASecondReservation) +{ + CommandLedger ledger; + std::atomic accepted{0}; + std::vector workers; + for (int index = 0; index < 16; ++index) { + workers.emplace_back([&] { + const auto result = ledger.reserve(key("shared"), "payload"); + if (result.status == CommandReservationStatus::AcceptedNew) { + accepted.fetch_add(1); + } + }); + } + for (auto& worker : workers) { + worker.join(); + } + EXPECT_EQ(accepted.load(), 1); + EXPECT_EQ(ledger.acceptedIdCount(), 1U); +} + +TEST(CommandLedgerTest, OutcomeUnknownIsCachedAndNeverReservedAgain) +{ + CommandLedger ledger; + const auto first = ledger.reserve(key("uncertain"), "payload"); + ASSERT_EQ(first.status, CommandReservationStatus::AcceptedNew); + + CommandOutcome outcome; + outcome.lifecycle = CommandLifecycle::OutcomeUnknown; + outcome.reason = SafetyReason::OutcomeUnknown; + outcome.hardware_submission_possible = true; + ASSERT_TRUE(ledger.complete(first.ticket, outcome)); + + const auto retry = ledger.reserve(key("uncertain"), "payload"); + ASSERT_EQ(retry.status, CommandReservationStatus::CachedResult); + ASSERT_TRUE(retry.cached_outcome.has_value()); + EXPECT_EQ( + retry.cached_outcome->lifecycle, CommandLifecycle::OutcomeUnknown); + EXPECT_TRUE(retry.cached_outcome->hardware_submission_possible); +} + +TEST(CommandLedgerTest, EvictedResultLeavesAnExactTombstone) +{ + CommandLedger ledger({1, 3}); + auto first = ledger.reserve(key("first"), "payload-1"); + ASSERT_TRUE(ledger.complete(first.ticket, completedOutcome())); + auto second = ledger.reserve(key("second"), "payload-2"); + ASSERT_TRUE(ledger.complete(second.ticket, completedOutcome())); + + EXPECT_EQ( + ledger.reserve(key("first"), "payload-1").status, + CommandReservationStatus::ResultEvicted); + EXPECT_EQ( + ledger.reserve(key("first"), "different").status, + CommandReservationStatus::CommandIdConflict); + + auto third = ledger.reserve(key("third"), "payload-3"); + ASSERT_EQ(third.status, CommandReservationStatus::AcceptedNew); + EXPECT_EQ( + ledger.reserve(key("fourth"), "payload-4").status, + CommandReservationStatus::LedgerExhausted); +} + +TEST(CommandLedgerTest, WaitTimesOutWithoutChangingExecution) +{ + CommandLedger ledger; + const auto reservation = ledger.reserve(key("slow"), "payload"); + ASSERT_EQ(reservation.status, CommandReservationStatus::AcceptedNew); + EXPECT_FALSE(ledger.wait( + reservation.ticket, + SafetyClock::now() + std::chrono::milliseconds(5)).has_value()); + EXPECT_EQ( + ledger.reserve(key("slow"), "payload").status, + CommandReservationStatus::JoinedInFlight); +} + +} // namespace +} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety/tests/safety_coordinator_test.cpp b/cmvr-es/manager/safety/tests/safety_coordinator_test.cpp new file mode 100644 index 00000000..fe43d4b6 --- /dev/null +++ b/cmvr-es/manager/safety/tests/safety_coordinator_test.cpp @@ -0,0 +1,507 @@ +#include "manager/safety/include/safety_coordinator.h" + +#include +#include +#include +#include + +#include + +namespace cmvr::safety { +namespace { + +DeviceSafetyDescriptor controlDescriptor() +{ + DeviceSafetyDescriptor descriptor; + descriptor.device_id = "arm"; + descriptor.kind = device::DeviceKind::Arm; + descriptor.default_policy = SafetyPolicyFamily::Control; + descriptor.maximum_snapshot_age = std::chrono::seconds(1); + descriptor.requires_safe_stop = true; + descriptor.supports_active_refresh = true; + return descriptor; +} + +class FakeEndpoint final : public DeviceSafetyEndpoint { +public: + explicit FakeEndpoint(DeviceSafetyDescriptor descriptor) + : descriptor_(std::move(descriptor)) + { + } + + DeviceSafetyDescriptor descriptor() const override + { + return descriptor_; + } + + void bindPublisher(SafetySnapshotPublisher publisher) override + { + publisher_ = std::move(publisher); + } + + void requestSafetyRefresh() noexcept override + { + if (!publisher_ || !publish_on_refresh) { + return; + } + DeviceSafetySnapshot snapshot; + snapshot.device_id = descriptor_.device_id; + snapshot.condition = condition; + snapshot.device_generation = generation; + snapshot.sample_sequence = ++sequence; + snapshot.observed_at = SafetyClock::now(); + snapshot.connected = connected; + snapshot.operational_ready = ready; + snapshot.quiescent = quiescent; + snapshot.motion_active = + quiescent == TriState::True ? TriState::False : TriState::Unknown; + snapshot.emergency_stop_active = emergency_stop; + snapshot.protective_stop_active = protective_stop; + snapshot.fault_active = fault; + (void)publisher_(std::move(snapshot)); + } + + HardwareCheckResult validateBeforeDispatch( + const AdmissionPermit&) override + { + ++hardware_checks; + return final_check; + } + + RecoveryCheckResult reconcileAdmissionState( + const RecoveryContext&) override + { + ++recoveries; + return recovery_check; + } + + DeviceSafetyDescriptor descriptor_; + SafetySnapshotPublisher publisher_; + SafetyCondition condition{SafetyCondition::Nominal}; + TriState connected{TriState::True}; + TriState ready{TriState::True}; + TriState quiescent{TriState::True}; + TriState emergency_stop{TriState::False}; + TriState protective_stop{TriState::False}; + TriState fault{TriState::False}; + HardwareCheckResult final_check{true, SafetyReason::None, {}}; + RecoveryCheckResult recovery_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}; +}; + +class FakeParticipant final : public SafetyParticipant { +public: + ParticipantDescriptor descriptor() const override + { + return {"arm", ParticipantPhase::Actuator, true, + std::chrono::milliseconds(100)}; + } + + BarrierToken beginBarrier( + const SafetyOperationContext& context) override + { + ++barriers; + return {"arm", context.operation_id, context.safety_epoch, + static_cast(barriers.load())}; + } + + ParticipantResult requestQuiesce( + const BarrierToken&, + const SafetyOperationContext&) override + { + ++stop_requests; + return stop_result; + } + + ParticipantResult verifyQuiescent( + const BarrierToken&, + const SafetyOperationContext&) override + { + ++verifications; + return verify_result; + } + + RecoveryCheckResult recoverAdmission( + const BarrierToken&, + const RecoveryContext&) override + { + ++recoveries; + return recovery_result; + } + + ParticipantResult releaseBarrier( + const BarrierToken&) noexcept override + { + ++releases; + return release_result; + } + + ParticipantResult stop_result{true, SafetyReason::None, {}}; + ParticipantResult verify_result{true, SafetyReason::None, {}}; + RecoveryCheckResult recovery_result{true, SafetyReason::None, {}}; + ParticipantResult release_result{true, SafetyReason::None, {}}; + std::atomic barriers{0}; + std::atomic stop_requests{0}; + std::atomic verifications{0}; + std::atomic recoveries{0}; + std::atomic releases{0}; +}; + +AdmissionRequest actuateRequest() +{ + AdmissionRequest request; + request.command = { + "/cmvr.api.ArmService/moveJ", + CommandIntent::Actuate, + SafetyPolicyFamily::Control, + true, + false}; + request.device_id = "arm"; + request.command_id = "command-1"; + request.deadline = SafetyClock::now() + std::chrono::seconds(1); + return request; +} + +TEST(SafetyCoordinatorTest, ShadowReportsDenyWithoutChangingLegacyBehavior) +{ + SafetyCoordinator coordinator; + ASSERT_TRUE(coordinator.registerDevice({controlDescriptor(), {}, {}})); + coordinator.markStartupComplete(); + + const auto result = coordinator.admit(actuateRequest()); + EXPECT_TRUE(result.decision.allowed); + EXPECT_FALSE(result.decision.policy_allowed); + EXPECT_FALSE(result.decision.enforced); + EXPECT_EQ(result.decision.reason, SafetyReason::SafetyStateMissing); + EXPECT_TRUE(result.permit.has_value()); +} + +TEST(SafetyCoordinatorTest, + EnforceSelectedStartupRejectsEmptyOrUnknownCoverage) +{ + SafetyCoordinatorConfig empty_config; + empty_config.enforcement_mode = EnforcementMode::EnforceSelected; + SafetyCoordinator empty(empty_config); + const auto empty_result = empty.validateStartupCoverage( + SafetyClock::now() + std::chrono::milliseconds(10)); + ASSERT_FALSE(empty_result.ready); + ASSERT_EQ(empty_result.issues.size(), 1U); + EXPECT_EQ(empty_result.issues.front().reason, SafetyReason::InvalidArgument); + + SafetyCoordinatorConfig missing_config; + missing_config.enforcement_mode = EnforcementMode::EnforceSelected; + missing_config.enforced_device_ids.insert("missing-arm"); + SafetyCoordinator missing(missing_config); + const auto missing_result = missing.validateStartupCoverage( + SafetyClock::now() + std::chrono::milliseconds(10)); + ASSERT_FALSE(missing_result.ready); + ASSERT_EQ(missing_result.issues.size(), 1U); + EXPECT_EQ(missing_result.issues.front().target_id, "missing-arm"); + EXPECT_EQ(missing_result.issues.front().reason, SafetyReason::DeviceNotFound); +} + +TEST(SafetyCoordinatorTest, + EnforceAllStartupRequiresEndpointParticipantAndFreshSnapshot) +{ + SafetyCoordinatorConfig config; + config.enforcement_mode = EnforcementMode::EnforceAll; + + SafetyCoordinator missing_capability(config); + ASSERT_TRUE(missing_capability.registerDevice( + {controlDescriptor(), {}, {}})); + const auto structural = missing_capability.validateStartupCoverage( + SafetyClock::now() + std::chrono::milliseconds(10)); + EXPECT_FALSE(structural.ready); + EXPECT_EQ(structural.issues.size(), 2U); + + SafetyCoordinator missing_sample(config); + auto silent_endpoint = + std::make_shared(controlDescriptor()); + silent_endpoint->publish_on_refresh = false; + ASSERT_TRUE(missing_sample.registerDevice({ + controlDescriptor(), + silent_endpoint, + std::make_shared()})); + const auto stale = missing_sample.validateStartupCoverage( + SafetyClock::now() + std::chrono::milliseconds(10)); + ASSERT_FALSE(stale.ready); + ASSERT_EQ(stale.issues.size(), 1U); + EXPECT_EQ(stale.issues.front().reason, SafetyReason::SafetyStateMissing); +} + +TEST(SafetyCoordinatorTest, + HardwareUnsafeSnapshotBlocksAdmissionButNotStructuralStartup) +{ + SafetyCoordinatorConfig config; + config.enforcement_mode = EnforcementMode::EnforceAll; + SafetyCoordinator coordinator(config); + auto endpoint = std::make_shared(controlDescriptor()); + endpoint->condition = SafetyCondition::Unsafe; + endpoint->emergency_stop = TriState::True; + ASSERT_TRUE(coordinator.registerDevice({ + controlDescriptor(), endpoint, std::make_shared()})); + coordinator.updateDeviceRuntimeState( + "arm", device::ManagedDeviceState::Running, + {device::DeviceHealthState::Healthy, {}}); + + const auto coverage = coordinator.validateStartupCoverage( + SafetyClock::now() + std::chrono::milliseconds(50)); + EXPECT_TRUE(coverage.ready); + coordinator.markStartupComplete(); + const auto admission = coordinator.admit(actuateRequest()); + EXPECT_FALSE(admission.decision.allowed); + EXPECT_EQ( + admission.decision.reason, SafetyReason::EmergencyStopActive); +} + +TEST(SafetyCoordinatorTest, EnforceAllFailsClosedOnUnknownControlState) +{ + SafetyCoordinatorConfig config; + config.enforcement_mode = EnforcementMode::EnforceAll; + SafetyCoordinator coordinator(config); + ASSERT_TRUE(coordinator.registerDevice({controlDescriptor(), {}, {}})); + coordinator.markStartupComplete(); + + const auto result = coordinator.admit(actuateRequest()); + EXPECT_FALSE(result.decision.allowed); + EXPECT_FALSE(result.decision.policy_allowed); + EXPECT_TRUE(result.decision.enforced); + EXPECT_FALSE(result.permit.has_value()); +} + +TEST(SafetyCoordinatorTest, ControlSafetyBitsMustBeExplicitlyFalse) +{ + SafetyCoordinatorConfig config; + config.enforcement_mode = EnforcementMode::EnforceAll; + SafetyCoordinator coordinator(config); + auto endpoint = std::make_shared(controlDescriptor()); + endpoint->protective_stop = TriState::Unknown; + ASSERT_TRUE(coordinator.registerDevice( + {controlDescriptor(), endpoint, {}})); + coordinator.updateDeviceRuntimeState( + "arm", device::ManagedDeviceState::Running, + {device::DeviceHealthState::Healthy, {}}); + coordinator.markStartupComplete(); + + const auto result = coordinator.admit(actuateRequest()); + EXPECT_FALSE(result.decision.allowed); + EXPECT_EQ(result.decision.reason, SafetyReason::ProtectiveStopActive); + ASSERT_EQ(coordinator.snapshot().devices.size(), 1U); + EXPECT_EQ( + coordinator.snapshot().devices.front().admission_state, + DeviceAdmissionState::Blocked); +} + +TEST(SafetyCoordinatorTest, EnforcedDispatchRunsFinalHardwareCheck) +{ + SafetyCoordinatorConfig config; + config.enforcement_mode = EnforcementMode::EnforceAll; + SafetyCoordinator coordinator(config); + auto endpoint = std::make_shared(controlDescriptor()); + ASSERT_TRUE(coordinator.registerDevice( + {controlDescriptor(), endpoint, {}})); + coordinator.updateDeviceRuntimeState( + "arm", device::ManagedDeviceState::Running, + {device::DeviceHealthState::Healthy, {}}); + coordinator.markStartupComplete(); + + auto admission = coordinator.admit(actuateRequest()); + ASSERT_TRUE(admission.decision.allowed) << admission.decision.detail; + ASSERT_TRUE(admission.permit.has_value()); + auto dispatch = coordinator.beginDispatch(*admission.permit); + EXPECT_TRUE(dispatch.acquired()); + EXPECT_EQ(endpoint->hardware_checks.load(), 1); +} + +TEST(SafetyCoordinatorTest, + StartActivityMayEnterFromRestrictedButActuationMayNot) +{ + SafetyCoordinatorConfig config; + config.enforcement_mode = EnforcementMode::EnforceAll; + SafetyCoordinator coordinator(config); + auto endpoint = std::make_shared(controlDescriptor()); + endpoint->condition = SafetyCondition::Restricted; + endpoint->ready = TriState::False; + ASSERT_TRUE(coordinator.registerDevice( + {controlDescriptor(), endpoint, {}})); + coordinator.updateDeviceRuntimeState( + "arm", device::ManagedDeviceState::Running, + {device::DeviceHealthState::Healthy, {}}); + coordinator.markStartupComplete(); + + auto start = actuateRequest(); + start.command.intent = CommandIntent::StartActivity; + const auto start_result = coordinator.admit(start); + ASSERT_TRUE(start_result.decision.allowed) + << start_result.decision.detail; + ASSERT_TRUE(start_result.permit.has_value()); + EXPECT_TRUE(coordinator.beginDispatch(*start_result.permit).acquired()); + + const auto actuation = coordinator.admit(actuateRequest()); + EXPECT_FALSE(actuation.decision.allowed); + EXPECT_EQ(actuation.decision.reason, SafetyReason::HardwareUnsafe); +} + +TEST(SafetyCoordinatorTest, SuccessfulStopInvalidatesOldPermitAndReopens) +{ + SafetyCoordinatorConfig config; + config.enforcement_mode = EnforcementMode::EnforceAll; + SafetyCoordinator coordinator(config); + auto endpoint = std::make_shared(controlDescriptor()); + auto participant = std::make_shared(); + ASSERT_TRUE(coordinator.registerDevice( + {controlDescriptor(), endpoint, participant})); + coordinator.updateDeviceRuntimeState( + "arm", device::ManagedDeviceState::Running, + {device::DeviceHealthState::Healthy, {}}); + coordinator.markStartupComplete(); + + auto admission = coordinator.admit(actuateRequest()); + ASSERT_TRUE(admission.permit.has_value()); + const auto old_epoch = admission.permit->safety_epoch; + const auto stopped = coordinator.stopAll( + "stop-1", SafetyClock::now() + std::chrono::seconds(1)); + ASSERT_TRUE(stopped.success); + EXPECT_EQ(stopped.system_state, SystemAdmissionState::Open); + EXPECT_GT(stopped.current_safety_epoch, old_epoch); + const auto revalidated = + coordinator.revalidatePermit(*admission.permit); + EXPECT_FALSE(revalidated.safe); + EXPECT_EQ(revalidated.reason, SafetyReason::SafetyLatched); + EXPECT_FALSE(coordinator.beginDispatch(*admission.permit).acquired()); + EXPECT_EQ(participant->stop_requests.load(), 1); + EXPECT_EQ(participant->verifications.load(), 1); + const auto snapshot = coordinator.snapshot(); + ASSERT_EQ(snapshot.participants.size(), 1U); + EXPECT_EQ(snapshot.participants.front().descriptor.participant_id, "arm"); + EXPECT_FALSE(snapshot.participants.front().barrier_active); + EXPECT_FALSE(snapshot.participants.front().barrier_retained); + EXPECT_TRUE(snapshot.participants.front().last_request.recorded); + EXPECT_TRUE(snapshot.participants.front().last_request.success); + EXPECT_TRUE(snapshot.participants.front().last_verify.recorded); + EXPECT_TRUE(snapshot.participants.front().last_verify.success); + EXPECT_TRUE(snapshot.participants.front().last_release.recorded); + EXPECT_TRUE(snapshot.participants.front().last_release.success); +} + +TEST(SafetyCoordinatorTest, FailedStopRequiresVerifiedRecovery) +{ + SafetyCoordinatorConfig config; + config.enforcement_mode = EnforcementMode::EnforceAll; + SafetyCoordinator coordinator(config); + auto endpoint = std::make_shared(controlDescriptor()); + auto participant = std::make_shared(); + participant->verify_result = { + false, SafetyReason::StopUnconfirmed, "motion not confirmed"}; + ASSERT_TRUE(coordinator.registerDevice( + {controlDescriptor(), endpoint, participant})); + coordinator.updateDeviceRuntimeState( + "arm", device::ManagedDeviceState::Running, + {device::DeviceHealthState::Healthy, {}}); + coordinator.markStartupComplete(); + + const auto stopped = coordinator.stopAll( + "stop-failed", SafetyClock::now() + std::chrono::seconds(1)); + ASSERT_FALSE(stopped.success); + ASSERT_EQ(stopped.system_state, SystemAdmissionState::Latched); + const auto latched = coordinator.snapshot(); + ASSERT_EQ(latched.participants.size(), 1U); + EXPECT_TRUE(latched.participants.front().barrier_active); + EXPECT_TRUE(latched.participants.front().barrier_retained); + EXPECT_TRUE(latched.participants.front().last_verify.recorded); + EXPECT_FALSE(latched.participants.front().last_verify.success); + EXPECT_FALSE(latched.participants.front().last_release.recorded); + + participant->verify_result = {true, SafetyReason::None, {}}; + RecoveryRequest request; + request.recovery_id = "recovery-1"; + request.all_devices = true; + request.expected_safety_epoch = stopped.current_safety_epoch; + request.verify_only = false; + request.reason = "operator confirmed work cell is clear"; + request.deadline = SafetyClock::now() + std::chrono::seconds(1); + const auto recovered = coordinator.recover(request); + EXPECT_EQ(recovered.result, RecoveryResultCode::Recovered); + EXPECT_EQ(recovered.system_state, SystemAdmissionState::Open); + EXPECT_EQ(endpoint->recoveries.load(), 1); + EXPECT_EQ(participant->recoveries.load(), 1); + const auto reopened = coordinator.snapshot(); + ASSERT_EQ(reopened.participants.size(), 1U); + EXPECT_FALSE(reopened.participants.front().barrier_active); + EXPECT_FALSE(reopened.participants.front().barrier_retained); + EXPECT_TRUE(reopened.participants.front().last_release.recorded); + EXPECT_TRUE(reopened.participants.front().last_release.success); +} + +TEST(SafetyCoordinatorTest, RecoveryCannotIgnoreEmergencyStop) +{ + SafetyCoordinator coordinator; + auto endpoint = std::make_shared(controlDescriptor()); + auto participant = std::make_shared(); + ASSERT_TRUE(coordinator.registerDevice( + {controlDescriptor(), endpoint, participant})); + coordinator.markStartupComplete(); + coordinator.quarantineDevice("arm", SafetyReason::OutcomeUnknown); + const auto before = coordinator.snapshot(); + + endpoint->emergency_stop = TriState::True; + RecoveryRequest request; + request.recovery_id = "recovery-estop"; + request.all_devices = true; + request.expected_safety_epoch = before.safety_epoch; + request.verify_only = false; + request.reason = "test"; + request.deadline = SafetyClock::now() + std::chrono::seconds(1); + const auto recovered = coordinator.recover(request); + EXPECT_EQ(recovered.result, RecoveryResultCode::BlockerRemains); + EXPECT_EQ(recovered.system_state, SystemAdmissionState::Latched); + ASSERT_FALSE(recovered.targets.empty()); + EXPECT_EQ( + recovered.targets.front().reason, + SafetyReason::EmergencyStopActive); + EXPECT_EQ(endpoint->recoveries.load(), 0); +} + +TEST(SafetyCoordinatorTest, RecoveryAuditFailureCannotReleaseLatch) +{ + SafetyCoordinator coordinator; + auto endpoint = std::make_shared(controlDescriptor()); + auto participant = std::make_shared(); + participant->verify_result = { + false, SafetyReason::StopUnconfirmed, "motion not confirmed"}; + ASSERT_TRUE(coordinator.registerDevice( + {controlDescriptor(), endpoint, participant})); + coordinator.markStartupComplete(); + + const auto stopped = coordinator.stopAll( + "stop-audit", SafetyClock::now() + std::chrono::seconds(1)); + ASSERT_FALSE(stopped.success); + participant->verify_result = {true, SafetyReason::None, {}}; + + RecoveryRequest request; + request.recovery_id = "recovery-audit"; + request.all_devices = true; + request.expected_safety_epoch = stopped.current_safety_epoch; + request.verify_only = false; + request.reason = "operator inspected the work cell"; + request.deadline = SafetyClock::now() + std::chrono::seconds(1); + request.authorize_clear = [] { return false; }; + const auto recovered = coordinator.recover(request); + + EXPECT_EQ(recovered.result, RecoveryResultCode::BlockerRemains); + EXPECT_EQ(recovered.system_state, SystemAdmissionState::Latched); + ASSERT_FALSE(recovered.targets.empty()); + EXPECT_EQ( + recovered.targets.front().reason, + SafetyReason::RecoveryAuditFailed); + EXPECT_EQ(endpoint->recoveries.load(), 0); + EXPECT_EQ(participant->recoveries.load(), 0); + EXPECT_EQ(participant->releases.load(), 0); +} + +} // namespace +} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety/tests/safety_snapshot_store_test.cpp b/cmvr-es/manager/safety/tests/safety_snapshot_store_test.cpp new file mode 100644 index 00000000..c5cdba4d --- /dev/null +++ b/cmvr-es/manager/safety/tests/safety_snapshot_store_test.cpp @@ -0,0 +1,111 @@ +#include "manager/safety/include/safety_snapshot_store.h" + +#include +#include +#include + +#include + +namespace cmvr::safety { +namespace { + +DeviceSafetyDescriptor descriptor() +{ + DeviceSafetyDescriptor result; + result.device_id = "arm"; + result.kind = device::DeviceKind::Arm; + result.default_policy = SafetyPolicyFamily::Control; + result.maximum_snapshot_age = std::chrono::milliseconds(50); + result.requires_safe_stop = true; + return result; +} + +DeviceSafetySnapshot sample( + const std::uint64_t generation, + const std::uint64_t sequence, + const SafetyClock::time_point observed_at = SafetyClock::now()) +{ + DeviceSafetySnapshot result; + result.device_id = "arm"; + result.condition = SafetyCondition::Nominal; + result.device_generation = generation; + result.sample_sequence = sequence; + result.observed_at = observed_at; + result.connected = TriState::True; + result.operational_ready = TriState::True; + result.quiescent = TriState::True; + result.motion_active = TriState::False; + result.emergency_stop_active = TriState::False; + result.protective_stop_active = TriState::False; + result.fault_active = TriState::False; + return result; +} + +TEST(SafetySnapshotStoreTest, RejectsOldGenerationAndSequence) +{ + SafetySnapshotStore store; + ASSERT_TRUE(store.registerDevice(descriptor(), 3)); + EXPECT_FALSE(store.publish(sample(2, 1))); + ASSERT_TRUE(store.publish(sample(3, 2))); + EXPECT_FALSE(store.publish(sample(3, 2))); + EXPECT_FALSE(store.publish(sample(3, 1))); + EXPECT_TRUE(store.publish(sample(4, 1))); + + const auto view = store.get("arm"); + ASSERT_TRUE(view.registered); + EXPECT_EQ(view.snapshot.device_generation, 4U); + EXPECT_EQ(view.snapshot.sample_sequence, 1U); +} + +TEST(SafetySnapshotStoreTest, FreshnessUsesMonotonicObservationTime) +{ + SafetySnapshotStore store; + ASSERT_TRUE(store.registerDevice(descriptor())); + const auto observed = SafetyClock::now(); + ASSERT_TRUE(store.publish(sample(1, 1, observed))); + + EXPECT_TRUE(store.get( + "arm", observed + std::chrono::milliseconds(49)).fresh); + const auto stale = store.get( + "arm", observed + std::chrono::milliseconds(51)); + EXPECT_FALSE(stale.fresh); + EXPECT_EQ(stale.sample_age, std::chrono::milliseconds(51)); +} + +TEST(SafetySnapshotStoreTest, BumpGenerationRequiresANewSample) +{ + SafetySnapshotStore store; + ASSERT_TRUE(store.registerDevice(descriptor())); + ASSERT_TRUE(store.publish(sample(1, 8))); + ASSERT_EQ(store.bumpGeneration("arm"), 2U); + + const auto view = store.get("arm"); + EXPECT_FALSE(view.has_sample); + EXPECT_FALSE(view.fresh); + EXPECT_EQ(view.snapshot.device_generation, 2U); + EXPECT_FALSE(store.publish(sample(1, 9))); + EXPECT_TRUE(store.publish(sample(2, 1))); +} + +TEST(SafetySnapshotStoreTest, WaiterObservesOnlyANewerSample) +{ + SafetySnapshotStore store; + ASSERT_TRUE(store.registerDevice(descriptor())); + ASSERT_TRUE(store.publish(sample(1, 1))); + + std::atomic published{false}; + std::thread writer([&] { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + published.store(store.publish(sample(1, 2))); + }); + + SafetySnapshotView view; + EXPECT_TRUE(store.waitForNewerSample( + "arm", 1, SafetyClock::now() + std::chrono::seconds(1), view)); + writer.join(); + EXPECT_TRUE(published.load()); + EXPECT_EQ(view.snapshot.sample_sequence, 2U); +} + +} // namespace +} // namespace cmvr::safety diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index 1244e7cb..75f57cc9 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -6,7 +6,12 @@ add_library(service grpc/src/media_activity_coordinator.cpp grpc/src/motor_activity_coordinator.cpp grpc/src/grpc_camera_service.cpp + grpc/src/grpc_command_transaction.cpp grpc/src/grpc_error_logging_interceptor.cpp + grpc/src/grpc_recovery_audit.cpp + grpc/src/grpc_safety_proto.cpp + grpc/src/grpc_safety_participants.cpp + grpc/src/grpc_security.cpp grpc/src/grpc_system_service.cpp grpc/src/grpc_speaker_service.cpp grpc/src/grpc_microphone_service.cpp @@ -242,6 +247,56 @@ if(BUILD_TESTING) ENVIRONMENT "${_grpc_system_test_environment}" ) + add_executable(grpc_security_test + grpc/tests/grpc_security_test.cpp + grpc/src/grpc_security.cpp + ) + target_include_directories(grpc_security_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(grpc_security_test + PRIVATE + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_security_test + COMMAND grpc_security_test + ) + set_tests_properties(grpc_security_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_grpc_system_test_environment}" + ) + + add_executable(grpc_command_transaction_test + grpc/tests/grpc_command_transaction_test.cpp + grpc/src/grpc_command_transaction.cpp + grpc/src/grpc_safety_proto.cpp + grpc/src/grpc_security.cpp + ) + target_include_directories(grpc_command_transaction_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(grpc_command_transaction_test PRIVATE + cmvr_es::safety_coordinator + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_command_transaction_test + COMMAND grpc_command_transaction_test + ) + set_tests_properties(grpc_command_transaction_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_grpc_system_test_environment}" + ) + add_executable(grpc_arm_service_test grpc/tests/grpc_arm_service_test.cpp ) diff --git a/cmvr-es/service/action/include/action_queue_executor.h b/cmvr-es/service/action/include/action_queue_executor.h index 13344465..84587832 100644 --- a/cmvr-es/service/action/include/action_queue_executor.h +++ b/cmvr-es/service/action/include/action_queue_executor.h @@ -9,6 +9,7 @@ #include #include "cmvr/api/system_command.pb.h" +#include "manager/safety/include/safety_types.h" namespace cmvr::device { class DeviceManager; @@ -59,7 +60,8 @@ public: WaitResult submitAndWait( const api::ActionQueueCommand_Request& request, api::ActionQueueCommand_Feedback& feedback, - const std::function& waiter_canceled = {}); + const std::function& waiter_canceled = {}, + safety::CommandActor actor = {}); // Starts (or joins) a temporary StopAll round. New action IDs are rejected // and queued/active actions are canceled. By default the executor also diff --git a/cmvr-es/service/action/src/action_queue_executor.cpp b/cmvr-es/service/action/src/action_queue_executor.cpp index f21bf5ba..7a4508b6 100644 --- a/cmvr-es/service/action/src/action_queue_executor.cpp +++ b/cmvr-es/service/action/src/action_queue_executor.cpp @@ -38,6 +38,7 @@ #include "devices/arm/robot_arm.h" #include "manager/control_authority/include/control_authority_manager.h" #include "manager/device_manager/include/device_manager.h" +#include "manager/safety/include/safety_coordinator.h" #include "service/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { @@ -84,6 +85,66 @@ struct PreparedAction { std::vector resource_ids; }; +safety::CommandDescriptor safetyDescriptorFor( + const PreparedStepKind kind) +{ + using safety::CommandIntent; + using safety::SafetyPolicyFamily; + switch (kind) { + case PreparedStepKind::ArmMoveJ: + return {"/cmvr.api.ArmService/moveJ", CommandIntent::Actuate, + SafetyPolicyFamily::Control, true, false}; + case PreparedStepKind::ArmMoveL: + return {"/cmvr.api.ArmService/moveL", CommandIntent::Actuate, + SafetyPolicyFamily::Control, true, false}; + case PreparedStepKind::AgvNavigateToPose: + return {"/cmvr.api.AgvService/navigateToPose", + CommandIntent::Actuate, SafetyPolicyFamily::Control, + true, false}; + case PreparedStepKind::AgvNavigateToStation: + return {"/cmvr.api.AgvService/navigateToStation", + CommandIntent::Actuate, SafetyPolicyFamily::Control, + true, false}; + case PreparedStepKind::AgvFollowPath: + return {"/cmvr.api.AgvService/followPath", + CommandIntent::Actuate, SafetyPolicyFamily::Control, + true, false}; + case PreparedStepKind::Delay: + break; + } + return {}; +} + +const api::CommandHeader_Request* commandHeaderFor( + const api::ActionStep& step) +{ + switch (step.command_case()) { + case api::ActionStep::kArmMoveJ: + return &step.arm_move_j().header(); + case api::ActionStep::kArmMoveL: + return &step.arm_move_l().header(); + case api::ActionStep::kAgvNavigateToPose: + return &step.agv_navigate_to_pose().header(); + case api::ActionStep::kAgvNavigateToStation: + return &step.agv_navigate_to_station().header(); + case api::ActionStep::kAgvFollowPath: + return &step.agv_follow_path().header(); + case api::ActionStep::kDelay: + case api::ActionStep::COMMAND_NOT_SET: + return nullptr; + } + return nullptr; +} + +std::string actionStepCommandId( + const std::string& action_id, + const std::string& step_id) +{ + return "action:" + std::to_string(action_id.size()) + ':' + + action_id + ":step:" + std::to_string(step_id.size()) + ':' + + step_id; +} + struct ValidationResult { bool valid{false}; std::string error; @@ -743,6 +804,7 @@ struct ActionQueueExecutor::Impl { api::ActionQueueCommand_Request request; RequestFingerprint fingerprint; PreparedAction prepared; + safety::CommandActor actor; Clock::time_point deadline; std::atomic cancel_requested{false}; std::atomic timed_out{false}; @@ -1226,7 +1288,8 @@ struct ActionQueueExecutor::Impl { ActionQueueExecutor::WaitResult submitAndWait( const api::ActionQueueCommand_Request& request, api::ActionQueueCommand_Feedback& feedback, - const std::function& waiter_canceled) + const std::function& waiter_canceled, + safety::CommandActor actor) { api::ActionDeduplicationStatus deduplication_status = api::ACTION_DEDUPLICATION_STATUS_UNSPECIFIED; @@ -1375,6 +1438,7 @@ struct ActionQueueExecutor::Impl { candidate->request = request; candidate->fingerprint = *fingerprint; candidate->prepared = validation.prepared; + candidate->actor = std::move(actor); candidate->deadline = Clock::now() + total_timeout; const bool canceled_before_admission = @@ -1870,6 +1934,7 @@ struct ActionQueueExecutor::Impl { enum class StepOutcome { Completed, + Rejected, Failed, Canceled, TimedOut, @@ -1880,6 +1945,49 @@ struct ActionQueueExecutor::Impl { std::string message; }; + safety::AdmissionResult admitStep( + const std::shared_ptr& record, + const PreparedStep& prepared, + const api::ActionStep& source, + const control::ControlLeaseToken& token, + const Clock::time_point deadline) + { + safety::AdmissionRequest request; + request.command = safetyDescriptorFor(prepared.kind); + request.actor = record->actor; + request.command_id = actionStepCommandId( + record->request.action_id(), source.step_id()); + request.device_id = prepared.device_id; + if (const auto* header = commandHeaderFor(source); + header && header->has_expected_device_generation()) { + request.expected_device_generation = + header->expected_device_generation(); + } + request.authority_generation = token.generation; + request.deadline = deadline; + return device_manager.safetyCoordinator().admit(request); + } + + static std::string admissionFailure( + const safety::AdmissionDecision& decision) + { + return decision.detail.empty() + ? std::string("ActionQueue step safety admission was rejected: ") + + safety::toString(decision.reason) + : "ActionQueue step safety admission was rejected: " + + decision.detail; + } + + static std::string dispatchFailure( + const safety::HardwareCheckResult& result) + { + return result.detail.empty() + ? std::string("ActionQueue step final safety check failed: ") + + safety::toString(result.reason) + : "ActionQueue step final safety check failed: " + + result.detail; + } + StepResult executeArmStep( const std::shared_ptr& record, const PreparedStep& prepared, @@ -1889,6 +1997,26 @@ struct ActionQueueExecutor::Impl { { auto& authority = control::ControlAuthorityManager::instance(); + auto safety_admission = admitStep( + record, prepared, source, token, deadline); + if (!safety_admission.permit) { + return { + StepOutcome::Rejected, + admissionFailure(safety_admission.decision)}; + } + auto authority_dispatch = authority.tryBeginDispatch(token); + if (!authority_dispatch.acquired()) { + return { + StepOutcome::Canceled, + "RobotArm ActionQueue control was preempted before dispatch"}; + } + auto safety_dispatch = device_manager.safetyCoordinator() + .beginDispatch(*safety_admission.permit); + if (!safety_dispatch.acquired()) { + return { + StepOutcome::Rejected, + dispatchFailure(safety_dispatch.hardwareCheck())}; + } auto cancellation_requested = [record, token, deadline, &authority]() { return record->cancel_requested.load( @@ -2030,6 +2158,26 @@ struct ActionQueueExecutor::Impl { { auto& authority = control::ControlAuthorityManager::instance(); + auto safety_admission = admitStep( + record, prepared, source, token, deadline); + if (!safety_admission.permit) { + return { + StepOutcome::Rejected, + admissionFailure(safety_admission.decision)}; + } + auto authority_dispatch = authority.tryBeginDispatch(token); + if (!authority_dispatch.acquired()) { + return { + StepOutcome::Canceled, + "AGV ActionQueue control was preempted before dispatch"}; + } + auto safety_dispatch = device_manager.safetyCoordinator() + .beginDispatch(*safety_admission.permit); + if (!safety_dispatch.acquired()) { + return { + StepOutcome::Rejected, + dispatchFailure(safety_dispatch.hardwareCheck())}; + } auto cancellation_requested = [record, token, deadline, &authority]() { return record->cancel_requested.load( @@ -2267,6 +2415,11 @@ struct ActionQueueExecutor::Impl { case StepOutcome::Completed: ++completed_steps; break; + case StepOutcome::Rejected: + complete( + record, api::ACTION_RESULT_CODE_REJECTED, + completed_steps, step_result.message, index); + return; case StepOutcome::Failed: complete( record, api::ACTION_RESULT_CODE_FAILED, @@ -2330,10 +2483,11 @@ ActionQueueExecutor::~ActionQueueExecutor() = default; ActionQueueExecutor::WaitResult ActionQueueExecutor::submitAndWait( const api::ActionQueueCommand_Request& request, api::ActionQueueCommand_Feedback& feedback, - const std::function& waiter_canceled) + const std::function& waiter_canceled, + safety::CommandActor actor) { - const auto result = - impl_->submitAndWait(request, feedback, waiter_canceled); + const auto result = impl_->submitAndWait( + request, feedback, waiter_canceled, std::move(actor)); feedback.set_service_instance_id(impl_->instance_id); return result; } diff --git a/cmvr-es/service/grpc/include/camera_operational_activity_registry.h b/cmvr-es/service/grpc/include/camera_operational_activity_registry.h index 43762e47..cf63aedb 100644 --- a/cmvr-es/service/grpc/include/camera_operational_activity_registry.h +++ b/cmvr-es/service/grpc/include/camera_operational_activity_registry.h @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -21,9 +22,12 @@ public: enum class DispatchResult { Success, RejectedByStopAll, + RejectedByDispatchFence, DeviceFailure, }; + using DispatchFence = std::function; + struct ActivityToken { std::string device_id; std::uint64_t activity_generation{0U}; @@ -38,7 +42,8 @@ public: DispatchResult start( const std::string& device_id, const std::shared_ptr& camera, - ActivityToken* token = nullptr); + ActivityToken* token = nullptr, + DispatchFence dispatch_fence = {}); // Rolls back only the exact activity created by start(). A newer start for // the same device is never stopped by an older request finishing late. @@ -48,7 +53,8 @@ public: // serialized here so it cannot race an operational StopAll stop. DispatchResult stopLifecycle( const std::string& device_id, - const std::shared_ptr& camera); + const std::shared_ptr& camera, + DispatchFence dispatch_fence = {}); // Reconciles a lifecycle stop performed outside CameraService. void markCameraStopped(const std::string& device_id); diff --git a/cmvr-es/service/grpc/include/camera_ptz_activity_registry.h b/cmvr-es/service/grpc/include/camera_ptz_activity_registry.h index 9bf69711..7cbbf006 100644 --- a/cmvr-es/service/grpc/include/camera_ptz_activity_registry.h +++ b/cmvr-es/service/grpc/include/camera_ptz_activity_registry.h @@ -2,6 +2,7 @@ #define CMVR_ES_CAMERA_PTZ_ACTIVITY_REGISTRY_H #include +#include #include #include #include @@ -20,15 +21,19 @@ public: enum class DispatchResult { Success, RejectedByStopAll, + RejectedByDispatchFence, DeviceFailure, }; + using DispatchFence = std::function; + DispatchResult control( const std::string& device_id, const std::shared_ptr& camera, device::PtzCommand command, bool stop, - int speed); + int speed, + DispatchFence dispatch_fence = {}); // Reconciles externally stopped camera PTZ state with this registry. A // camera backend can call this if it stops PTZ outside CameraService. diff --git a/cmvr-es/service/grpc/include/grpc_agv_service.h b/cmvr-es/service/grpc/include/grpc_agv_service.h index 56e6115c..d7975948 100644 --- a/cmvr-es/service/grpc/include/grpc_agv_service.h +++ b/cmvr-es/service/grpc/include/grpc_agv_service.h @@ -1,15 +1,21 @@ #ifndef CMVR_ES_GRPC_AGV_SERVICE_H #define CMVR_ES_GRPC_AGV_SERVICE_H +#include + #include "cmvr/api/agv_service.grpc.pb.h" #include "devices/agv/abstract_agv.h" #include "manager/device_manager/include/device_manager.h" namespace cmvr::service { +class GrpcSecurityGateway; + class gRPCAgvServiceImpl final : public api::AgvService::Service { public: gRPCAgvServiceImpl(); + explicit gRPCAgvServiceImpl( + std::shared_ptr security_gateway); ~gRPCAgvServiceImpl() override = default; grpc::Status getRuntimeState(grpc::ServerContext* context, @@ -79,6 +85,7 @@ public: private: device::DeviceManager& dmgr_; + std::shared_ptr security_gateway_; }; } // namespace cmvr::service diff --git a/cmvr-es/service/grpc/include/grpc_arm_service.h b/cmvr-es/service/grpc/include/grpc_arm_service.h index 230e021e..b87291c4 100644 --- a/cmvr-es/service/grpc/include/grpc_arm_service.h +++ b/cmvr-es/service/grpc/include/grpc_arm_service.h @@ -1,14 +1,20 @@ #pragma once +#include + #include "cmvr/api/arm_service.grpc.pb.h" #include "devices/arm/robot_arm.h" #include "manager/device_manager/include/device_manager.h" namespace cmvr::service { +class GrpcSecurityGateway; + class gRPCArmServiceImpl final : public api::ArmService::Service { public: gRPCArmServiceImpl(); + explicit gRPCArmServiceImpl( + std::shared_ptr security_gateway); ~gRPCArmServiceImpl() override = default; grpc::Status torqueOff(grpc::ServerContext* context, @@ -59,6 +65,7 @@ public: private: device::DeviceManager& dmgr_; + std::shared_ptr security_gateway_; }; } // namespace cmvr::service diff --git a/cmvr-es/service/grpc/include/grpc_arm_teleop_service.h b/cmvr-es/service/grpc/include/grpc_arm_teleop_service.h index 8684f91d..4e7eaa12 100644 --- a/cmvr-es/service/grpc/include/grpc_arm_teleop_service.h +++ b/cmvr-es/service/grpc/include/grpc_arm_teleop_service.h @@ -13,6 +13,16 @@ namespace cmvr::service { +class GrpcSecurityGateway; + +} // namespace cmvr::service + +namespace cmvr::safety { +class SafetyCoordinator; +} + +namespace cmvr::service { + namespace arm_teleop = cmvr::api::armteleop::v1; struct ArmTeleopBackendResult { @@ -75,7 +85,9 @@ public: explicit ArmTeleopServiceImpl( std::shared_ptr backend = makeDisabledArmTeleopBackend(), - control::ControlAuthorityManager* authority = nullptr); + control::ControlAuthorityManager* authority = nullptr, + std::shared_ptr security_gateway = nullptr, + safety::SafetyCoordinator* safety_coordinator = nullptr); ~ArmTeleopServiceImpl() override = default; grpc::Status Teleoperate( @@ -86,6 +98,8 @@ public: private: std::shared_ptr backend_; control::ControlAuthorityManager* authority_{nullptr}; + std::shared_ptr security_gateway_; + safety::SafetyCoordinator* safety_coordinator_{nullptr}; }; } // namespace cmvr::service diff --git a/cmvr-es/service/grpc/include/grpc_camera_service.h b/cmvr-es/service/grpc/include/grpc_camera_service.h index 62e72277..c6c2f07d 100644 --- a/cmvr-es/service/grpc/include/grpc_camera_service.h +++ b/cmvr-es/service/grpc/include/grpc_camera_service.h @@ -5,6 +5,8 @@ #ifndef GRPC_CAMERA_SERVICE_H #define GRPC_CAMERA_SERVICE_H +#include + #include "cmvr/api/camera_service.grpc.pb.h" #include "common/base/grpc_utils.h" #include "manager/device_manager/include/device_manager.h" @@ -13,10 +15,13 @@ namespace cmvr::service { + class GrpcSecurityGateway; + class gRPCCameraServiceImpl final: public api::CameraService::Service { public: explicit gRPCCameraServiceImpl( - CameraStreamLowLatencyConfig stream_config = {}); + CameraStreamLowLatencyConfig stream_config = {}, + std::shared_ptr security_gateway = nullptr); ~gRPCCameraServiceImpl() override = default; grpc::Status GetStatus(grpc::ServerContext* context, const api::GetCameraStateCommand_Request* request, api::GetCameraStateCommand_Feedback* response) override; grpc::Status StartCamera(grpc::ServerContext* context, const api::StartCameraCommand_Request* request, api::StartCameraCommand_Feedback* response) override; @@ -33,6 +38,7 @@ namespace cmvr::service { private: device::DeviceManager& dmgr_; CameraStreamLowLatencyConfig stream_config_; + std::shared_ptr security_gateway_; //双向流读写线程 std::shared_ptr read_thread_ = nullptr; diff --git a/cmvr-es/service/grpc/include/grpc_command_transaction.h b/cmvr-es/service/grpc/include/grpc_command_transaction.h new file mode 100644 index 00000000..94818f26 --- /dev/null +++ b/cmvr-es/service/grpc/include/grpc_command_transaction.h @@ -0,0 +1,211 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include + +#include + +#include "cmvr/api/common.pb.h" +#include "manager/safety/include/safety_coordinator.h" +#include "service/grpc/include/grpc_security.h" + +namespace cmvr::service { + +grpc::Status grpcStatusForSafetyReason( + safety::SafetyReason reason, + const std::string& detail = {}); + +struct GrpcStreamingSafetyOpen { + std::string full_method_name; + std::string device_id; + std::string session_id; + std::string expected_service_instance_id; + std::optional expected_device_generation; + std::uint64_t authority_generation{0}; + safety::SafetyClock::time_point deadline{ + safety::SafetyClock::time_point::max()}; +}; + +// Binds a long-lived control stream to one coordinator permit. Stream +// protocols retain their own sequence, watchdog, and control-lease rules; +// this object owns the safety epoch/device-generation checks shared by all of +// them. It deliberately does not use the unary idempotency ledger. +class GrpcStreamingSafetySession final { +public: + GrpcStreamingSafetySession( + safety::SafetyCoordinator& coordinator, + const GrpcRequestContext& request_context, + GrpcStreamingSafetyOpen open); + + GrpcStreamingSafetySession( + GrpcStreamingSafetySession&&) noexcept = default; + GrpcStreamingSafetySession& operator=( + GrpcStreamingSafetySession&&) noexcept = default; + GrpcStreamingSafetySession( + const GrpcStreamingSafetySession&) = delete; + GrpcStreamingSafetySession& operator=( + const GrpcStreamingSafetySession&) = delete; + + bool admitted() const noexcept { return permit_.has_value(); } + const grpc::Status& status() const noexcept { return status_; } + const safety::AdmissionDecision& admissionDecision() const noexcept + { + return admission_decision_; + } + + bool revalidate(); + safety::DispatchGuard beginDispatch(); + std::uint64_t safetyEpoch() const noexcept; + std::uint64_t deviceGeneration() const noexcept; + std::uint64_t authorityGeneration() const noexcept; + +private: + void reject_(safety::SafetyReason reason, std::string detail); + + safety::SafetyCoordinator* coordinator_{nullptr}; + std::optional permit_; + safety::AdmissionDecision admission_decision_; + grpc::Status status_; +}; + +// Owns one unary command from identity reservation through the final hardware +// dispatch fence. Legacy/Shadow calls without a command ID still use admission, +// but deliberately remain outside the idempotency ledger for wire compatibility. +class GrpcCommandTransaction final { +public: + GrpcCommandTransaction( + safety::SafetyCoordinator& coordinator, + GrpcRequestContext request_context, + GrpcMethodPolicy method_policy, + const google::protobuf::Message& request, + google::protobuf::Message& response); + ~GrpcCommandTransaction() noexcept; + + GrpcCommandTransaction(GrpcCommandTransaction&& other) noexcept; + GrpcCommandTransaction& operator=( + GrpcCommandTransaction&& other) noexcept; + GrpcCommandTransaction(const GrpcCommandTransaction&) = delete; + GrpcCommandTransaction& operator=(const GrpcCommandTransaction&) = delete; + + bool shouldExecute() const noexcept { return should_execute_; } + const grpc::Status& status() const noexcept { return status_; } + const safety::AdmissionDecision& admissionDecision() const noexcept + { + return admission_decision_; + } + + // Must be called immediately before the first driver/SDK mutation. The + // returned guard remains owned by this transaction until finish(). + bool beginDispatch(); + // Long-running unary commands may submit more than one hardware command. + // Revalidate the original permit between submissions, then hold the + // returned guard only around one driver/SDK mutation. + bool revalidate(); + safety::DispatchGuard beginScopedDispatch(); + // Internal mitigation for a command-owned activity. This obtains a fresh + // Stop-lane permit, so an expired/revoked Actuate permit cannot suppress a + // physical stop. + safety::DispatchGuard beginSafetyStopDispatch(); + const grpc::Status& dispatchStatus() const noexcept + { + return dispatch_status_; + } + + grpc::Status finish( + grpc::Status operation_status, + safety::SafetyReason reason = safety::SafetyReason::None, + std::optional lifecycle = std::nullopt); + grpc::Status finishException(std::string detail) noexcept; + + const std::string& deviceId() const noexcept { return device_id_; } + const std::string& commandId() const noexcept { return command_id_; } + std::uint64_t safetyEpoch() const noexcept + { + return admission_decision_.safety_epoch; + } + std::uint64_t deviceGeneration() const noexcept + { + return admission_decision_.device_generation; + } + +private: + void initialize_(const google::protobuf::Message& request); + void rejectBeforeDispatch_( + safety::SafetyReason reason, + std::string detail, + grpc::Status status, + bool complete_reserved_record); + bool restoreOutcome_(const safety::CommandOutcome& outcome); + bool completeLedger_( + safety::CommandLifecycle lifecycle, + safety::SafetyReason reason, + const std::string& detail, + bool hardware_submission_possible) noexcept; + void populateFeedback_( + bool success, + safety::SafetyReason reason, + safety::CommandLifecycle lifecycle, + const std::string& detail); + void abandon_() noexcept; + + safety::SafetyCoordinator* coordinator_{nullptr}; + GrpcRequestContext request_context_; + GrpcMethodPolicy method_policy_; + google::protobuf::Message* response_{nullptr}; + safety::CommandLedger::Ticket ledger_ticket_; + std::optional permit_; + std::optional dispatch_guard_; + safety::AdmissionDecision admission_decision_; + grpc::Status status_; + grpc::Status dispatch_status_; + std::string device_id_; + std::string command_id_; + std::string payload_hash_; + safety::SafetyClock::time_point deadline_{ + safety::SafetyClock::time_point::max()}; + bool should_execute_{false}; + bool owns_ledger_record_{false}; + bool dispatch_started_{false}; + bool completed_{false}; + std::uint64_t safety_stop_sequence_{0}; +}; + +using GrpcUnaryCommandOperation = + std::function; + +grpc::Status executeRegisteredGrpcCommand( + const std::shared_ptr& gateway, + grpc::ServerContext* server_context, + safety::SafetyCoordinator& coordinator, + const std::string& full_method_name, + const google::protobuf::Message* request, + google::protobuf::Message* response, + GrpcUnaryCommandOperation operation); + +// For a legacy RPC whose validated request selects one of several fixed +// server-side intents. Gateway authorization still uses the registered method +// policy; the supplied policy only narrows Coordinator admission after the +// server has parsed the request (for example PTZ START versus STOP). +grpc::Status executeServerDerivedGrpcCommand( + const std::shared_ptr& gateway, + grpc::ServerContext* server_context, + safety::SafetyCoordinator& coordinator, + const std::string& full_method_name, + GrpcMethodPolicy effective_policy, + const google::protobuf::Message* request, + google::protobuf::Message* response, + GrpcUnaryCommandOperation operation); + +// Stable across processes and protobuf map iteration order. Transport identity +// fields and client timestamps are excluded; device generation remains part of +// the semantic payload. +std::string deterministicGrpcPayloadHash( + const std::string& full_method_name, + const google::protobuf::Message& request); + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/include/grpc_dexhand_service.h b/cmvr-es/service/grpc/include/grpc_dexhand_service.h index 865a9870..39dc8930 100644 --- a/cmvr-es/service/grpc/include/grpc_dexhand_service.h +++ b/cmvr-es/service/grpc/include/grpc_dexhand_service.h @@ -5,14 +5,19 @@ #ifndef GRPC_DEXHAND_SERVICE_H #define GRPC_DEXHAND_SERVICE_H +#include + #include "cmvr/api/dexhand_service.grpc.pb.h" #include "common/base/grpc_utils.h" #include "manager/device_manager/include/device_manager.h" #include "devices/dexhand/abstract_dexhand.h" namespace cmvr::service { + class GrpcSecurityGateway; class gRPCDexHandServiceImpl final: public api::DexHandService::Service { public: gRPCDexHandServiceImpl(); + explicit gRPCDexHandServiceImpl( + std::shared_ptr security_gateway); ~gRPCDexHandServiceImpl() override = default; grpc::Status GetStatus(grpc::ServerContext* context, const api::GetDexHandStateCommand_Request* request,api::GetDexHandStateCommand_Feedback* response) override; grpc::Status SetDexHandPos(grpc::ServerContext* context, const cmvr::api::SetDexHandPositionsCommand_Request* request, cmvr::api::SetDexHandPositionsCommand_Feedback* response) override; @@ -24,6 +29,7 @@ namespace cmvr::service { grpc::Status GetSensorDataStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream) override; private: device::DeviceManager& dmgr_; + std::shared_ptr security_gateway_; }; } diff --git a/cmvr-es/service/grpc/include/grpc_head_service.h b/cmvr-es/service/grpc/include/grpc_head_service.h index 4b8b20c3..04b4b5bf 100644 --- a/cmvr-es/service/grpc/include/grpc_head_service.h +++ b/cmvr-es/service/grpc/include/grpc_head_service.h @@ -1,15 +1,20 @@ #ifndef BIO_HEAD_SERVICE_H #define BIO_HEAD_SERVICE_H +#include + #include "cmvr/api/biohead_service.grpc.pb.h" #include "manager/device_manager/include/device_manager.h" #include "devices/biohead/abstract_biohead.h" namespace cmvr::service { + class GrpcSecurityGateway; class gRPCMBioHeadServiceImpl : public api::BioHeadService::Service { public: gRPCMBioHeadServiceImpl(); + explicit gRPCMBioHeadServiceImpl( + std::shared_ptr security_gateway); ~gRPCMBioHeadServiceImpl() override = default; grpc::Status SetExpression(grpc::ServerContext* context, @@ -44,6 +49,7 @@ namespace cmvr::service grpc::Status ExpressionYawn(grpc::ServerContext* context, const cmvr::api::ExpressionYawn_Request* request, cmvr::api::ExpressionYawn_Feedback* response) override; private: device::DeviceManager& dmgr_; + std::shared_ptr security_gateway_; }; } // namespace cmvr::service diff --git a/cmvr-es/service/grpc/include/grpc_hlc_service.h b/cmvr-es/service/grpc/include/grpc_hlc_service.h index 9fc593cc..607e049d 100644 --- a/cmvr-es/service/grpc/include/grpc_hlc_service.h +++ b/cmvr-es/service/grpc/include/grpc_hlc_service.h @@ -3,15 +3,24 @@ // #pragma once +#include + #include "cmvr/api/hlc_service.grpc.pb.h" +#include "manager/device_manager/include/device_manager.h" namespace cmvr { namespace service { + class GrpcSecurityGateway; class gRPCHlcServiceImpl final : public api::HlcService::Service { public: gRPCHlcServiceImpl(); + explicit gRPCHlcServiceImpl( + std::shared_ptr security_gateway); ~gRPCHlcServiceImpl() = default; grpc::Status touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) override; + private: + device::DeviceManager& dmgr_; + std::shared_ptr security_gateway_; }; diff --git a/cmvr-es/service/grpc/include/grpc_microphone_service.h b/cmvr-es/service/grpc/include/grpc_microphone_service.h index 1e4eac04..afbe34d7 100644 --- a/cmvr-es/service/grpc/include/grpc_microphone_service.h +++ b/cmvr-es/service/grpc/include/grpc_microphone_service.h @@ -5,6 +5,7 @@ #ifndef GRPC_MICROPHONE_SERVICE_H #define GRPC_MICROPHONE_SERVICE_H +#include #include "cmvr/api/microphone_service.grpc.pb.h" #include "manager/device_manager/include/device_manager.h" #include "devices/microphone/abstract_microphone.h" @@ -12,10 +13,13 @@ namespace cmvr::service { + class GrpcSecurityGateway; class gRPCMicroPhoneServiceImpl: public api::MicPhoneService::Service { public: gRPCMicroPhoneServiceImpl(); + explicit gRPCMicroPhoneServiceImpl( + std::shared_ptr security_gateway); ~gRPCMicroPhoneServiceImpl() override = default; grpc::Status GetStatus(grpc::ServerContext* context, const api::GetMicStateCommand_Request* request,api::GetMicStateCommand_Feedback* response) override; grpc::Status StartRecord(grpc::ServerContext* context, const api::StartMicRecordingCommand_Request* request,api::StartMicRecordingCommand_Feedback* response) override; @@ -27,6 +31,7 @@ namespace cmvr::service grpc::Status GetVolume(grpc::ServerContext* context, const api::GetMicPhoneVolumeCommand_Request* request,api::GetMicPhoneVolumeCommand_Feedback* response) override; private: device::DeviceManager& dmgr_; + std::shared_ptr security_gateway_; }; } #endif //GRPC_MICROPHONE_SERVICE_H diff --git a/cmvr-es/service/grpc/include/grpc_motor_service.h b/cmvr-es/service/grpc/include/grpc_motor_service.h index 67d68910..bbcd54f4 100644 --- a/cmvr-es/service/grpc/include/grpc_motor_service.h +++ b/cmvr-es/service/grpc/include/grpc_motor_service.h @@ -19,6 +19,9 @@ class DeviceManager; namespace cmvr::service { class gRPCMotorServiceImplTestAccess; +class GrpcCommandTransaction; +class GrpcSecurityGateway; +struct GrpcRequestContext; // A deliberately thin synchronous gRPC facade over AbstractMotor. It does not // schedule trajectories or retain asynchronous operations. The small amount of @@ -27,6 +30,8 @@ class gRPCMotorServiceImplTestAccess; class gRPCMotorServiceImpl final : public api::MotorService::Service { public: gRPCMotorServiceImpl(); + explicit gRPCMotorServiceImpl( + std::shared_ptr security_gateway); ~gRPCMotorServiceImpl() override = default; grpc::Status setZero(grpc::ServerContext* context, @@ -136,7 +141,8 @@ private: double max_velocity_rad_s, double acceleration_rad_s2, const api::MotorWaitOptions& wait, - api::MotorCommandResponse* response); + api::MotorCommandResponse* response, + GrpcCommandTransaction& command); grpc::Status waitForPosition(grpc::ServerContext* context, const ResolvedMotor& resolved, std::uint64_t generation, @@ -154,39 +160,47 @@ private: grpc::Status setZeroImpl(grpc::ServerContext* context, const api::SetMotorZeroRequest* request, - api::MotorCommandResponse* response); + api::MotorCommandResponse* response, + GrpcCommandTransaction& command); grpc::Status moveToZeroImpl(grpc::ServerContext* context, const api::MoveMotorToZeroRequest* request, - api::MotorCommandResponse* response); + api::MotorCommandResponse* response, + GrpcCommandTransaction& command); grpc::Status profilePositionImpl( grpc::ServerContext* context, const api::ProfilePositionRequest* request, - api::MotorCommandResponse* response); + api::MotorCommandResponse* response, + GrpcCommandTransaction& command); grpc::Status profileVelocityImpl( grpc::ServerContext* context, const api::ProfileVelocityRequest* request, - api::MotorCommandResponse* response); + api::MotorCommandResponse* response, + GrpcCommandTransaction& command); grpc::Status emergencyStopImpl( grpc::ServerContext* context, const api::EmergencyStopRequest* request, - api::MotorCommandResponse* response); + api::MotorCommandResponse* response, + GrpcCommandTransaction& command); grpc::Status getStatusImpl(grpc::ServerContext* context, const api::GetMotorStatusRequest* request, api::GetMotorStatusResponse* response); grpc::Status setEnabledImpl( grpc::ServerContext* context, const api::SetMotorEnabledRequest* request, - api::MotorCommandResponse* response); + api::MotorCommandResponse* response, + GrpcCommandTransaction& command); grpc::Status streamCyclicPositionImpl( grpc::ServerContext* context, grpc::ServerReaderWriter* stream, - std::optional& cleanup_target); + std::optional& cleanup_target, + const GrpcRequestContext& request_context); grpc::Status streamCyclicVelocityImpl( grpc::ServerContext* context, grpc::ServerReaderWriter* stream, - std::optional& cleanup_target); + std::optional& cleanup_target, + const GrpcRequestContext& request_context); void bestEffortQuickStop(const api::MotorTarget& target, const std::string& error) noexcept; @@ -199,6 +213,7 @@ private: const std::string& error) const; device::DeviceManager& dmgr_; + std::shared_ptr security_gateway_; mutable std::mutex states_mutex_; mutable std::unordered_map states_; diff --git a/cmvr-es/service/grpc/include/grpc_recovery_audit.h b/cmvr-es/service/grpc/include/grpc_recovery_audit.h new file mode 100644 index 00000000..9d25d55e --- /dev/null +++ b/cmvr-es/service/grpc/include/grpc_recovery_audit.h @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#include +#include + +namespace cmvr::service { + +struct RecoveryAuditRecord { + std::uint64_t occurred_at_unix_ms{0}; + std::string stage; + std::string correlation_id; + std::string principal_id; + std::string peer; + std::string recovery_id; + std::string reason; + std::string mode; + bool all_devices{false}; + std::vector device_ids; + std::uint64_t expected_safety_epoch{0}; + std::uint64_t previous_safety_epoch{0}; + std::uint64_t current_safety_epoch{0}; + std::string result; +}; + +class RecoveryAuditSink { +public: + virtual ~RecoveryAuditSink() = default; + virtual bool append( + const RecoveryAuditRecord& record, + std::string* error = nullptr) noexcept = 0; +}; + +std::shared_ptr makeFileRecoveryAuditSink( + std::string path); + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/include/grpc_safety_participants.h b/cmvr-es/service/grpc/include/grpc_safety_participants.h new file mode 100644 index 00000000..d3fe5328 --- /dev/null +++ b/cmvr-es/service/grpc/include/grpc_safety_participants.h @@ -0,0 +1,43 @@ +#pragma once + +#include + +namespace cmvr::safety { +class SafetyCoordinator; +} + +namespace cmvr::service { + +class ActionQueueExecutor; +class StopOperationDispatcher; + +class GrpcSafetyParticipantRegistration final { +public: + ~GrpcSafetyParticipantRegistration(); + GrpcSafetyParticipantRegistration( + GrpcSafetyParticipantRegistration&&) noexcept; + GrpcSafetyParticipantRegistration& operator=( + GrpcSafetyParticipantRegistration&&) noexcept; + GrpcSafetyParticipantRegistration( + const GrpcSafetyParticipantRegistration&) = delete; + GrpcSafetyParticipantRegistration& operator=( + const GrpcSafetyParticipantRegistration&) = delete; + +private: + friend std::unique_ptr + registerGrpcSafetyParticipants( + safety::SafetyCoordinator&, + std::shared_ptr, + std::shared_ptr); + struct Impl; + explicit GrpcSafetyParticipantRegistration(std::unique_ptr impl); + std::unique_ptr impl_; +}; + +std::unique_ptr +registerGrpcSafetyParticipants( + safety::SafetyCoordinator& coordinator, + std::shared_ptr action_queue, + std::shared_ptr stop_dispatcher); + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/include/grpc_safety_proto.h b/cmvr-es/service/grpc/include/grpc_safety_proto.h new file mode 100644 index 00000000..68515d9f --- /dev/null +++ b/cmvr-es/service/grpc/include/grpc_safety_proto.h @@ -0,0 +1,37 @@ +#pragma once + +#include "cmvr/api/safety_command.pb.h" +#include "manager/safety/include/safety_coordinator.h" + +namespace cmvr::service { + +api::CommandReasonCode toApiSafetyReason( + safety::SafetyReason value) noexcept; +safety::SafetyReason fromApiSafetyReason( + api::CommandReasonCode value) noexcept; +api::SafetyTriState toApiSafetyTriState( + safety::TriState value) noexcept; +api::SafetyCondition toApiSafetyCondition( + safety::SafetyCondition value) noexcept; +api::SystemAdmissionState toApiSystemAdmissionState( + safety::SystemAdmissionState value) noexcept; +api::DeviceAdmissionState toApiDeviceAdmissionState( + safety::DeviceAdmissionState value) noexcept; +api::SafetyBlockerScope toApiSafetyBlockerScope( + safety::BlockerScope value) noexcept; +api::SafetyRecoveryRequirement toApiRecoveryRequirement( + safety::RecoveryRequirement value) noexcept; +api::SafetyOperationResult toApiRecoveryResult( + safety::RecoveryResultCode value) noexcept; + +void populateDeviceSafetyState( + const safety::DeviceSafetyStateView& source, + api::DeviceSafetyStateInfo& destination); +void populateSafetyTargetResult( + const safety::SafetyTargetResult& source, + api::SafetyOperationTargetResult& destination); +void populateSafetyParticipantState( + const safety::ParticipantSafetyStateView& source, + api::SafetyParticipantStateInfo& destination); + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/include/grpc_security.h b/cmvr-es/service/grpc/include/grpc_security.h new file mode 100644 index 00000000..9ed1cf01 --- /dev/null +++ b/cmvr-es/service/grpc/include/grpc_security.h @@ -0,0 +1,268 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "cmvr/config/grpc_server_config/grpc_server_config.pb.h" +#include "manager/safety/include/safety_types.h" + +namespace cmvr::service { + +enum class GrpcTransportSecurity { + Insecure, + ServerTls, + MutualTls, +}; + +enum class GrpcAuthenticationMethod { + Disabled, + StaticToken, + Jwt, + TlsClientCertificate, +}; + +enum class GrpcRole { + Anonymous, + Observer, + Operator, + SafetyAdmin, +}; + +enum class GrpcAccessClass { + Read, + Mutate, + Stop, + Recover, +}; + +enum class GrpcRecoveryExposure { + Disabled, + LocalOnly, + Authorized, +}; + +struct GrpcPrincipal { + std::string id{"anonymous"}; + GrpcAuthenticationMethod method{GrpcAuthenticationMethod::Disabled}; + bool authenticated{false}; + std::vector roles{GrpcRole::Anonymous}; +}; + +struct GrpcCallFacts { + std::string correlation_id; + std::string full_method_name; + std::string peer; + std::multimap metadata; + bool transport_encrypted{false}; + bool local_peer{false}; + std::chrono::steady_clock::time_point received_at; + std::chrono::steady_clock::time_point deadline; +}; + +struct GrpcRequestContext { + std::string correlation_id; + std::string full_method_name; + std::string peer; + GrpcPrincipal principal; + bool transport_encrypted{false}; + bool local_peer{false}; + std::chrono::steady_clock::time_point received_at; + std::chrono::steady_clock::time_point deadline; +}; + +struct GrpcMethodPolicy { + std::string full_method_name; + GrpcAccessClass access{GrpcAccessClass::Read}; + GrpcRole minimum_role{GrpcRole::Observer}; + safety::CommandIntent command_intent{safety::CommandIntent::Observe}; + safety::SafetyPolicyFamily policy_family{ + safety::SafetyPolicyFamily::Sensor}; + bool mutating{false}; + bool safety_lane{false}; + + safety::CommandDescriptor commandDescriptor() const + { + return { + full_method_name, + command_intent, + policy_family, + mutating, + safety_lane}; + } +}; + +struct GrpcSecurityRuntimeConfig { + GrpcTransportSecurity transport{GrpcTransportSecurity::Insecure}; + GrpcAuthenticationMethod authentication{ + GrpcAuthenticationMethod::Disabled}; + GrpcRecoveryExposure recovery_exposure{ + GrpcRecoveryExposure::Disabled}; + bool allow_insecure_non_loopback{false}; + bool insecure_non_loopback{false}; + bool legacy_compatibility{false}; + std::string recovery_audit_file; +}; + +struct GrpcSecurityConfigResult { + bool valid{false}; + GrpcSecurityRuntimeConfig config; + std::string error; + std::vector warnings; +}; + +GrpcSecurityConfigResult resolveGrpcSecurityConfig( + const config::GRPCServerConfig& config, + const std::string& effective_host); + +bool isLocalGrpcPeer(const std::string& peer) noexcept; +bool isLoopbackGrpcHost(const std::string& host) noexcept; + +const char* toString(GrpcTransportSecurity value) noexcept; +const char* toString(GrpcAuthenticationMethod value) noexcept; +const char* toString(GrpcRecoveryExposure value) noexcept; +const char* toString(GrpcRole value) noexcept; + +struct GrpcAuthenticationResult { + GrpcPrincipal principal; + grpc::Status status; + + bool ok() const noexcept { return status.ok(); } +}; + +class GrpcAuthenticationProvider { +public: + virtual ~GrpcAuthenticationProvider() = default; + virtual GrpcAuthenticationResult authenticate( + const GrpcCallFacts& facts) const = 0; +}; + +class DisabledGrpcAuthenticationProvider final + : public GrpcAuthenticationProvider { +public: + GrpcAuthenticationResult authenticate( + const GrpcCallFacts& facts) const override; +}; + +struct GrpcAuthorizationDecision { + bool allowed{false}; + grpc::Status status; +}; + +class GrpcAuthorizationPolicy { +public: + virtual ~GrpcAuthorizationPolicy() = default; + virtual GrpcAuthorizationDecision authorize( + const GrpcRequestContext& context, + const GrpcMethodPolicy& method) const = 0; +}; + +class CompatibilityGrpcAuthorizationPolicy final + : public GrpcAuthorizationPolicy { +public: + explicit CompatibilityGrpcAuthorizationPolicy( + GrpcRecoveryExposure recovery_exposure); + + GrpcAuthorizationDecision authorize( + const GrpcRequestContext& context, + const GrpcMethodPolicy& method) const override; + +private: + GrpcRecoveryExposure recovery_exposure_; +}; + +class GrpcMethodPolicyRegistry final { +public: + bool registerPolicy(GrpcMethodPolicy policy); + std::optional find( + const std::string& full_method_name) const; + std::vector snapshot() const; + +private: + std::unordered_map policies_; +}; + +const GrpcMethodPolicyRegistry& defaultGrpcMethodPolicyRegistry(); + +struct GrpcSecurityAuditRecord { + std::string correlation_id; + std::string full_method_name; + std::string principal_id; + std::string peer; + GrpcAuthenticationMethod authentication{ + GrpcAuthenticationMethod::Disabled}; + GrpcAccessClass access{GrpcAccessClass::Read}; + bool authenticated{false}; + bool allowed{false}; + grpc::StatusCode status_code{grpc::StatusCode::OK}; +}; + +using GrpcSecurityAuditSink = + std::function; + +class GrpcCallGuard final { +public: + GrpcCallGuard(GrpcRequestContext context, + GrpcAuthorizationDecision decision); + + bool allowed() const noexcept { return decision_.allowed; } + const grpc::Status& status() const noexcept { return decision_.status; } + const GrpcRequestContext& context() const noexcept { return context_; } + +private: + GrpcRequestContext context_; + GrpcAuthorizationDecision decision_; +}; + +class GrpcSecurityGateway final { +public: + GrpcSecurityGateway( + GrpcSecurityRuntimeConfig config, + std::shared_ptr authentication, + std::shared_ptr authorization, + GrpcSecurityAuditSink audit_sink = {}); + + GrpcCallGuard beginCall( + grpc::ServerContext* server_context, + const GrpcMethodPolicy& method) const; + GrpcCallGuard beginCall( + GrpcCallFacts facts, + const GrpcMethodPolicy& method) const; + + const GrpcSecurityRuntimeConfig& config() const noexcept { return config_; } + +private: + GrpcSecurityRuntimeConfig config_; + std::shared_ptr authentication_; + std::shared_ptr authorization_; + GrpcSecurityAuditSink audit_sink_; +}; + +std::shared_ptr makeGrpcSecurityGateway( + const GrpcSecurityRuntimeConfig& config, + GrpcSecurityAuditSink audit_sink = {}); + +std::shared_ptr makeDefaultGrpcSecurityGateway(); + +GrpcCallGuard beginRegisteredGrpcCall( + const std::shared_ptr& gateway, + grpc::ServerContext* server_context, + const std::string& full_method_name); + +} // namespace cmvr::service + +#define CMVR_GRPC_REQUIRE_REGISTERED_CALL(gateway, server_context, method) \ + const auto cmvr_grpc_call_guard = \ + ::cmvr::service::beginRegisteredGrpcCall( \ + gateway, server_context, method); \ + if (!cmvr_grpc_call_guard.allowed()) { \ + return cmvr_grpc_call_guard.status(); \ + } diff --git a/cmvr-es/service/grpc/include/grpc_speaker_service.h b/cmvr-es/service/grpc/include/grpc_speaker_service.h index 2bf0e4c1..daf10eaa 100644 --- a/cmvr-es/service/grpc/include/grpc_speaker_service.h +++ b/cmvr-es/service/grpc/include/grpc_speaker_service.h @@ -5,15 +5,20 @@ #ifndef GRPC_SPEAKER_SERVICE_H #define GRPC_SPEAKER_SERVICE_H +#include + #include "cmvr/api/speaker_service.grpc.pb.h" #include "manager/device_manager/include/device_manager.h" #include "devices/speaker/abstract_speaker.h" #include "common/base/grpc_utils.h" namespace cmvr::service { + class GrpcSecurityGateway; class gRPCSpeakerServiceImpl: public api::SpeakerService::Service { public: gRPCSpeakerServiceImpl(); + explicit gRPCSpeakerServiceImpl( + std::shared_ptr security_gateway); ~gRPCSpeakerServiceImpl() override = default; grpc::Status GetStatus(grpc::ServerContext* context, const api::GetSpeakerStateCommand_Request* request,api::GetSpeakerStateCommand_Feedback* response) override; grpc::Status PlayAudio(grpc::ServerContext* context, const api::PlayAudioCommand_Request* request,api::PlayAudioCommand_Feedback* response) override; @@ -25,6 +30,7 @@ namespace cmvr::service { grpc::Status GetVolume(grpc::ServerContext* context, const api::GetSpeakerVolumeCommand_Request* request,api::GetSpeakerVolumeCommand_Feedback* response) override; private: device::DeviceManager& dmgr_; + std::shared_ptr security_gateway_; }; } diff --git a/cmvr-es/service/grpc/include/grpc_system_service.h b/cmvr-es/service/grpc/include/grpc_system_service.h index 6b126b5f..f0ae3deb 100644 --- a/cmvr-es/service/grpc/include/grpc_system_service.h +++ b/cmvr-es/service/grpc/include/grpc_system_service.h @@ -15,6 +15,9 @@ namespace cmvr::service { class ActionQueueExecutor; + class RecoveryAuditSink; + class GrpcSafetyParticipantRegistration; + class GrpcSecurityGateway; class StopOperationDispatcher; class gRPCSystemServiceImpl: public api::SystemService::Service { @@ -22,6 +25,15 @@ namespace cmvr::service gRPCSystemServiceImpl(); explicit gRPCSystemServiceImpl( std::chrono::milliseconds stop_timeout); + explicit gRPCSystemServiceImpl( + std::shared_ptr security_gateway); + gRPCSystemServiceImpl( + std::chrono::milliseconds stop_timeout, + std::shared_ptr security_gateway); + gRPCSystemServiceImpl( + std::chrono::milliseconds stop_timeout, + std::shared_ptr security_gateway, + std::shared_ptr recovery_audit_sink); ~gRPCSystemServiceImpl() override; // Exposed only to synchronize lifecycle concurrency tests. static bool waitForStopDispatcherDestructionForTesting( @@ -36,13 +48,19 @@ namespace cmvr::service grpc::Status UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) override; grpc::Status StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) override; 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; private: device::DeviceManager& dmgr_; const std::chrono::milliseconds stop_timeout_; + std::shared_ptr security_gateway_; + std::shared_ptr recovery_audit_sink_; // Process instances share running jobs through a lifecycle registry. // The last service owner joins every worker before replacement. std::shared_ptr stop_dispatcher_; - std::unique_ptr action_queue_; + std::shared_ptr action_queue_; + std::unique_ptr + safety_participant_registration_; }; } diff --git a/cmvr-es/service/grpc/include/media_activity_coordinator.h b/cmvr-es/service/grpc/include/media_activity_coordinator.h index aa0860d5..d234c0b4 100644 --- a/cmvr-es/service/grpc/include/media_activity_coordinator.h +++ b/cmvr-es/service/grpc/include/media_activity_coordinator.h @@ -33,6 +33,12 @@ public: } }; + struct FinishStopAllResult { + bool ticket_consumed{false}; + bool participant_stopped{false}; + bool admission_resumed{false}; + }; + class Session final { public: Session() = default; @@ -108,6 +114,14 @@ public: const StopAllTicket& ticket, bool all_media_stopped); + FinishStopAllResult finishStopAllDetailed( + const StopAllTicket& ticket, + bool all_media_stopped); + + // Test/process teardown hook. Runtime recovery must use a new verified + // StopAll or Recover transaction instead of bypassing this latch. + void clearForTesting() noexcept; + private: std::shared_ptr impl_; }; diff --git a/cmvr-es/service/grpc/include/motor_activity_coordinator.h b/cmvr-es/service/grpc/include/motor_activity_coordinator.h index 171bdbda..6692482b 100644 --- a/cmvr-es/service/grpc/include/motor_activity_coordinator.h +++ b/cmvr-es/service/grpc/include/motor_activity_coordinator.h @@ -35,6 +35,12 @@ public: } }; + struct FinishStopAllResult { + bool ticket_consumed{false}; + bool participant_stopped{false}; + bool admission_resumed{false}; + }; + class Registration final { public: Registration() = default; @@ -131,6 +137,10 @@ public: const StopAllTicket& ticket, bool all_motors_stopped); + FinishStopAllResult finishStopAllDetailed( + const StopAllTicket& ticket, + bool all_motors_stopped); + // Wakes StopAll after a MotorControlState releases or changes ownership. void notifyStateChanged() noexcept; diff --git a/cmvr-es/service/grpc/src/camera_operational_activity_registry.cpp b/cmvr-es/service/grpc/src/camera_operational_activity_registry.cpp index 48b36c4d..32e769a8 100644 --- a/cmvr-es/service/grpc/src/camera_operational_activity_registry.cpp +++ b/cmvr-es/service/grpc/src/camera_operational_activity_registry.cpp @@ -11,6 +11,7 @@ namespace { template CameraOperationalActivityRegistry::DispatchResult dispatchIfAdmitted( std::mutex& device_mutex, + const CameraOperationalActivityRegistry::DispatchFence& dispatch_fence, Operation&& operation) { std::uint64_t admitted_generation = 0U; @@ -33,6 +34,11 @@ CameraOperationalActivityRegistry::DispatchResult dispatchIfAdmitted( } } + if (dispatch_fence && !dispatch_fence()) { + return CameraOperationalActivityRegistry::DispatchResult:: + RejectedByDispatchFence; + } + return operation(); } @@ -61,7 +67,8 @@ CameraOperationalActivityRegistry::DispatchResult CameraOperationalActivityRegistry::start( const std::string& device_id, const std::shared_ptr& camera, - ActivityToken* token) + ActivityToken* token, + DispatchFence dispatch_fence) { if (token) { *token = {}; @@ -71,7 +78,7 @@ CameraOperationalActivityRegistry::start( } const auto state = stateForDevice(device_id, true); - return dispatchIfAdmitted(state->mutex, [&] { + return dispatchIfAdmitted(state->mutex, dispatch_fence, [&] { const auto previous_camera = state->active_camera; const bool was_active = state->active.load(std::memory_order_acquire); if (was_active && previous_camera != camera) { @@ -153,14 +160,15 @@ bool CameraOperationalActivityRegistry::stopIfCurrent( CameraOperationalActivityRegistry::DispatchResult CameraOperationalActivityRegistry::stopLifecycle( const std::string& device_id, - const std::shared_ptr& camera) + const std::shared_ptr& camera, + DispatchFence dispatch_fence) { if (device_id.empty() || !camera) { return DispatchResult::DeviceFailure; } const auto state = stateForDevice(device_id, true); - return dispatchIfAdmitted(state->mutex, [&] { + return dispatchIfAdmitted(state->mutex, dispatch_fence, [&] { if (!camera->stop()) { return DispatchResult::DeviceFailure; } diff --git a/cmvr-es/service/grpc/src/camera_ptz_activity_registry.cpp b/cmvr-es/service/grpc/src/camera_ptz_activity_registry.cpp index 475b58e8..a1a50f5e 100644 --- a/cmvr-es/service/grpc/src/camera_ptz_activity_registry.cpp +++ b/cmvr-es/service/grpc/src/camera_ptz_activity_registry.cpp @@ -33,17 +33,18 @@ CameraPtzActivityRegistry::control( const std::shared_ptr& camera, const device::PtzCommand command, const bool stop, - const int speed) + const int speed, + DispatchFence dispatch_fence) { if (device_id.empty() || !camera) { return DispatchResult::DeviceFailure; } - // Check admission on both sides of the per-device dispatch queue. A - // command admitted before StopAll but still queued is rejected; one already - // in device I/O is completed before that device's stop begins. + // START checks admission on both sides of the per-device dispatch queue. A + // STOP is a safety-lane operation and remains available while StopAll is + // latched; its Coordinator dispatch fence still runs under the same queue. std::uint64_t admitted_generation = 0U; - { + if (!stop) { auto admission = globalStopAllAdmissionGate().lockAdmission(); if (!admission.accepting()) { return DispatchResult::RejectedByStopAll; @@ -53,7 +54,7 @@ CameraPtzActivityRegistry::control( const auto state = stateForDevice(device_id, true); std::lock_guard dispatch_lock(state->mutex); - { + if (!stop) { auto admission = globalStopAllAdmissionGate().lockAdmission(); if (!admission.accepting() || admission.generation() != admitted_generation) { @@ -61,6 +62,14 @@ CameraPtzActivityRegistry::control( } } + // The service-level safety transaction performs its final epoch, + // generation, and hardware-state validation here. Keeping the fence under + // the per-device lock prevents a request that waited in this queue from + // dispatching with a stale admission permit. + if (dispatch_fence && !dispatch_fence()) { + return DispatchResult::RejectedByDispatchFence; + } + if (!camera->controlPtz(command, stop, speed)) { return DispatchResult::DeviceFailure; } diff --git a/cmvr-es/service/grpc/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/src/grpc_agv_service.cpp index e1192ace..c04ac789 100644 --- a/cmvr-es/service/grpc/src/grpc_agv_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_agv_service.cpp @@ -12,6 +12,8 @@ #include "common/base/logging/logger.h" #include "manager/control_authority/include/control_authority_manager.h" +#include "service/grpc/include/grpc_command_transaction.h" +#include "service/grpc/include/grpc_security.h" #include "service/stop_all/include/stop_all_admission_gate.h" using google::protobuf::util::TimeUtil; @@ -605,14 +607,25 @@ void fillUnifiedMapUpdate(msgs::AgvUnifiedMapUpdate* dst, const device::AgvUnifi } // namespace gRPCAgvServiceImpl::gRPCAgvServiceImpl() - : dmgr_(device::DeviceManager::getInstance()) + : gRPCAgvServiceImpl(makeDefaultGrpcSecurityGateway()) { } -grpc::Status gRPCAgvServiceImpl::getRuntimeState(grpc::ServerContext*, +gRPCAgvServiceImpl::gRPCAgvServiceImpl( + std::shared_ptr security_gateway) + : dmgr_(device::DeviceManager::getInstance()), + security_gateway_(security_gateway + ? std::move(security_gateway) + : makeDefaultGrpcSecurityGateway()) +{ +} + +grpc::Status gRPCAgvServiceImpl::getRuntimeState(grpc::ServerContext* context, const api::AgvRuntimeStateCommand_Request* request, api::AgvRuntimeStateCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.AgvService/getRuntimeState"); try { const std::string device_id = request->header().device_id(); auto agv = dmgr_.getDevice(device_id); @@ -628,10 +641,13 @@ grpc::Status gRPCAgvServiceImpl::getRuntimeState(grpc::ServerContext*, } } -grpc::Status gRPCAgvServiceImpl::getNavigationStatus(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::getNavigationStatus(grpc::ServerContext* context, const api::AgvNavigationStatusCommand_Request* request, api::AgvNavigationStatusCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.AgvService/getNavigationStatus"); try { const std::string device_id = request->header().device_id(); auto agv = dmgr_.getDevice(device_id); @@ -647,11 +663,14 @@ grpc::Status gRPCAgvServiceImpl::getNavigationStatus(grpc::ServerContext*, } } -grpc::Status gRPCAgvServiceImpl::emergencyStop(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::emergencyStop(grpc::ServerContext* context, const api::CommandHeader_Request* request, api::CommandHeader_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.AgvService/emergencyStop", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); auto agv = dmgr_.getDevice(device_id); if (!agv) { @@ -663,20 +682,23 @@ grpc::Status gRPCAgvServiceImpl::emergencyStop(grpc::ServerContext*, return setControlLeaseConflict( response, device_id, control_barrier.detail()); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } return executeConfirmedAgvStop( response, agv, control_barrier, "emergencyStop", [&agv]() { return agv->emergencyStop(); }); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } -grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext* context, const api::CommandHeader_Request* request, api::CommandHeader_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.AgvService/clearFault", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); auto agv = dmgr_.getDevice(device_id); if (!agv) { @@ -693,18 +715,21 @@ grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext*, return setControlDispatchFailure( response, device_id, control_lease, "clearFault"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } return setResponseResult(response, agv->clearFault()); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext* context, const api::AgvNavigateToPoseCommand_Request* request, api::AgvNavigateToPoseCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.AgvService/navigateToPose", request, response, + [this, context, request, response](GrpcCommandTransaction& command) { if (context && context->IsCancelled()) { return setNavigationRequestCanceled(response); } @@ -719,27 +744,31 @@ grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext* context, return setControlAdmissionFailure( response, device_id, control_lease); } - if (!control_lease.current()) { + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { return setControlDispatchFailure( response, device_id, control_lease, "navigateToPose"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } return setResponseResult(response, agv->navigateToPose( toPose2d(request->pose()), toMotionOptions( request->options(), control_lease.cancellationRequested(context)), toAdapterParams(request->adapter_params()))); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext* context, const api::AgvNavigateToStationCommand_Request* request, api::AgvNavigateToStationCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.AgvService/navigateToStation", request, response, + [this, context, request, response](GrpcCommandTransaction& command) { if (context && context->IsCancelled()) { return setNavigationRequestCanceled(response); } @@ -754,28 +783,32 @@ grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext* context, return setControlAdmissionFailure( response, device_id, control_lease); } - if (!control_lease.current()) { + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { return setControlDispatchFailure( response, device_id, control_lease, "navigateToStation"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } return setResponseResult(response, agv->navigateToStation( request->station_id(), toMotionOptions( request->options(), control_lease.cancellationRequested(context)), toAdapterParams(request->adapter_params()))); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext* context, const api::AgvFollowPathCommand_Request* request, api::AgvFollowPathCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.AgvService/followPath", request, response, + [this, context, request, response](GrpcCommandTransaction& command) { if (context && context->IsCancelled()) { return setNavigationRequestCanceled(response); } @@ -790,7 +823,8 @@ grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext* context, return setControlAdmissionFailure( response, device_id, control_lease); } - if (!control_lease.current()) { + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { return setControlDispatchFailure( response, device_id, control_lease, "followPath"); } @@ -799,6 +833,9 @@ grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext* context, for (const auto& segment : request->path()) { path.push_back(toPathSegment(segment)); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } return setResponseResult( response, agv->followPath( @@ -806,10 +843,7 @@ grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext* context, toMotionOptions( request->options(), control_lease.cancellationRequested(context)))); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } @@ -819,7 +853,10 @@ grpc::Status gRPCAgvServiceImpl::translate( const api::AgvTranslateCommand_Request* request, api::AgvTranslateCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.AgvService/translate", request, response, + [this, context, request, response](GrpcCommandTransaction& command) { if (context && context->IsCancelled()) { return setNavigationRequestCanceled(response); } @@ -839,24 +876,25 @@ grpc::Status gRPCAgvServiceImpl::translate( return setControlDispatchFailure( response, device_id, control_lease, "translate"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } return setResponseResult( response, agv->translate(toTranslation(request->translation()))); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status( - grpc::StatusCode::INTERNAL, - e.what()); - } + }); } -grpc::Status gRPCAgvServiceImpl::pauseNavigation(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::pauseNavigation(grpc::ServerContext* context, const api::CommandHeader_Request* request, api::CommandHeader_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.AgvService/pauseNavigation", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); auto agv = dmgr_.getDevice(device_id); if (!agv) { @@ -874,18 +912,21 @@ grpc::Status gRPCAgvServiceImpl::pauseNavigation(grpc::ServerContext*, response, device_id, control_lease, "pauseNavigation"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } return setResponseResult(response, agv->pauseNavigation()); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } -grpc::Status gRPCAgvServiceImpl::resumeNavigation(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::resumeNavigation(grpc::ServerContext* context, const api::CommandHeader_Request* request, api::CommandHeader_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.AgvService/resumeNavigation", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); auto agv = dmgr_.getDevice(device_id); if (!agv) { @@ -903,18 +944,21 @@ grpc::Status gRPCAgvServiceImpl::resumeNavigation(grpc::ServerContext*, response, device_id, control_lease, "resumeNavigation"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } return setResponseResult(response, agv->resumeNavigation()); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } -grpc::Status gRPCAgvServiceImpl::cancelNavigation(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::cancelNavigation(grpc::ServerContext* context, const api::CommandHeader_Request* request, api::CommandHeader_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.AgvService/cancelNavigation", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); auto agv = dmgr_.getDevice(device_id); if (!agv) { @@ -926,20 +970,23 @@ grpc::Status gRPCAgvServiceImpl::cancelNavigation(grpc::ServerContext*, return setControlLeaseConflict( response, device_id, control_barrier.detail()); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } return executeConfirmedAgvStop( response, agv, control_barrier, "cancelNavigation", [&agv]() { return agv->cancelNavigation(); }); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } -grpc::Status gRPCAgvServiceImpl::setVelocity(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::setVelocity(grpc::ServerContext* context, const api::AgvSetVelocityCommand_Request* request, api::AgvSetVelocityCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.AgvService/setVelocity", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); auto agv = dmgr_.getDevice(device_id); if (!agv) { @@ -956,18 +1003,21 @@ grpc::Status gRPCAgvServiceImpl::setVelocity(grpc::ServerContext*, return setControlDispatchFailure( response, device_id, control_lease, "setVelocity"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } return setResponseResult(response, agv->setVelocity(toVelocity(request->velocity()))); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } -grpc::Status gRPCAgvServiceImpl::stopVelocityControl(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::stopVelocityControl(grpc::ServerContext* context, const api::CommandHeader_Request* request, api::CommandHeader_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.AgvService/stopVelocityControl", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); auto agv = dmgr_.getDevice(device_id); if (!agv) { @@ -979,19 +1029,21 @@ grpc::Status gRPCAgvServiceImpl::stopVelocityControl(grpc::ServerContext*, return setControlLeaseConflict( response, device_id, control_barrier.detail()); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } return executeConfirmedAgvStop( response, agv, control_barrier, "stopVelocityControl", [&agv]() { return agv->stopVelocityControl(); }); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } -grpc::Status gRPCAgvServiceImpl::listMaps(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::listMaps(grpc::ServerContext* context, const api::AgvListMapsCommand_Request* request, api::AgvListMapsCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.AgvService/listMaps"); try { const std::string device_id = request->header().device_id(); auto agv = dmgr_.getDevice(device_id); @@ -1012,10 +1064,12 @@ grpc::Status gRPCAgvServiceImpl::listMaps(grpc::ServerContext*, } } -grpc::Status gRPCAgvServiceImpl::listStations(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::listStations(grpc::ServerContext* context, const api::AgvListStationsCommand_Request* request, api::AgvListStationsCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.AgvService/listStations"); try { const std::string device_id = request->header().device_id(); auto agv = dmgr_.getDevice(device_id); @@ -1036,11 +1090,14 @@ grpc::Status gRPCAgvServiceImpl::listStations(grpc::ServerContext*, } } -grpc::Status gRPCAgvServiceImpl::switchMap(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::switchMap(grpc::ServerContext* context, const api::AgvMapCommand_Request* request, api::AgvMapCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.AgvService/switchMap", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); auto agv = dmgr_.getDevice(device_id); if (!agv) { @@ -1057,18 +1114,21 @@ grpc::Status gRPCAgvServiceImpl::switchMap(grpc::ServerContext*, return setControlDispatchFailure( response, device_id, control_lease, "switchMap"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } return setResponseResult(response, agv->switchMap(request->map_name())); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } -grpc::Status gRPCAgvServiceImpl::uploadMap(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::uploadMap(grpc::ServerContext* context, const api::AgvMapCommand_Request* request, api::AgvMapCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.AgvService/uploadMap", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); auto agv = dmgr_.getDevice(device_id); if (!agv) { @@ -1085,17 +1145,19 @@ grpc::Status gRPCAgvServiceImpl::uploadMap(grpc::ServerContext*, return setControlDispatchFailure( response, device_id, control_lease, "uploadMap"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } return setResponseResult(response, agv->uploadMap(request->map_name(), request->content())); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } -grpc::Status gRPCAgvServiceImpl::downloadMap(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::downloadMap(grpc::ServerContext* context, const api::AgvMapCommand_Request* request, api::AgvMapCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.AgvService/downloadMap"); try { const std::string device_id = request->header().device_id(); auto agv = dmgr_.getDevice(device_id); @@ -1114,11 +1176,14 @@ grpc::Status gRPCAgvServiceImpl::downloadMap(grpc::ServerContext*, } } -grpc::Status gRPCAgvServiceImpl::startMapping(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::startMapping(grpc::ServerContext* context, const api::AgvStartMappingCommand_Request* request, api::AgvStartMappingCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.AgvService/startMapping", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); auto agv = dmgr_.getDevice(device_id); if (!agv) { @@ -1139,21 +1204,23 @@ grpc::Status gRPCAgvServiceImpl::startMapping(grpc::ServerContext*, options.dimension = toMapDimension(request->dimension()); options.map_name = request->map_name(); options.real_time = request->real_time(); + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } const auto result = agv->startMapping(options); if (result.ok()) { response->set_session_id(device_id + "_mapping"); } return setResponseResult(response, result); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } grpc::Status gRPCAgvServiceImpl::streamMap(grpc::ServerContext* context, const api::AgvMapStreamCommand_Request* request, grpc::ServerWriter* writer) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.AgvService/streamMap"); try { const std::string device_id = request->header().device_id(); auto agv = dmgr_.getDevice(device_id); @@ -1220,11 +1287,14 @@ grpc::Status gRPCAgvServiceImpl::streamMap(grpc::ServerContext* context, } } -grpc::Status gRPCAgvServiceImpl::stopMapping(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::stopMapping(grpc::ServerContext* context, const api::CommandHeader_Request* request, api::CommandHeader_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.AgvService/stopMapping", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); auto agv = dmgr_.getDevice(device_id); if (!agv) { @@ -1241,11 +1311,11 @@ grpc::Status gRPCAgvServiceImpl::stopMapping(grpc::ServerContext*, return setControlDispatchFailure( response, device_id, control_lease, "stopMapping"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } return setResponseResult(response, agv->stopMapping()); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } } // namespace cmvr::service diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index 1fd6ef0e..8ffda776 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -8,6 +8,8 @@ #include "common/base/logging/logger.h" #include "manager/control_authority/include/control_authority_manager.h" +#include "service/grpc/include/grpc_command_transaction.h" +#include "service/grpc/include/grpc_security.h" #include "service/stop_all/include/stop_all_admission_gate.h" using google::protobuf::util::TimeUtil; @@ -361,15 +363,27 @@ grpc::Status setControlDispatchFailure( } // namespace gRPCArmServiceImpl::gRPCArmServiceImpl() - : dmgr_(device::DeviceManager::getInstance()) + : gRPCArmServiceImpl(makeDefaultGrpcSecurityGateway()) { } -grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*, +gRPCArmServiceImpl::gRPCArmServiceImpl( + std::shared_ptr security_gateway) + : dmgr_(device::DeviceManager::getInstance()), + security_gateway_(security_gateway + ? std::move(security_gateway) + : makeDefaultGrpcSecurityGateway()) +{ +} + +grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext* context, const api::CommandHeader_Request* request, api::CommandHeader_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.ArmService/torqueOff", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); auto arm = dmgr_.getDevice(device_id); if (!arm) { @@ -381,6 +395,9 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*, return setControlLeaseConflict( response, device_id, control_barrier.detail()); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } const auto result = executeConfirmedArmStop( control_barrier, "torqueOff", @@ -390,17 +407,17 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*, logRpcSuccess("torqueOff", device_id); } return resultToStatus(result); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext* context, const api::CommandHeader_Request* request, api::CommandHeader_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.ArmService/torqueOn", request, response, + [this, context, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); auto arm = dmgr_.getDevice(device_id); if (!arm) { @@ -417,6 +434,9 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext* context, return setControlDispatchFailure( response, device_id, control_lease, "torqueOn"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } const auto cancellation_requested = control_lease.cancellationRequested(context); const auto result = arm->torqueOn(cancellation_requested); @@ -443,17 +463,17 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext* context, logRpcSuccess("torqueOn", device_id); } return resultToStatus(result); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext* context, const api::MoveJ_Request* request, api::MoveJ_Response* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.ArmService/moveJ", request, response, + [this, context, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); auto arm = dmgr_.getDevice(device_id); if (!arm) { @@ -465,10 +485,14 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext* context, return setControlAdmissionFailure( response, device_id, control_lease); } - if (!control_lease.current()) { + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { return setControlDispatchFailure( response, device_id, control_lease, "moveJ"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } auto options = toMotionOptions( request->options(), control_lease.cancellationRequested(context)); @@ -479,17 +503,17 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext* context, << ", positions=" << request->target().position_size(); } return setResponseResult(response, result); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext* context, const api::MoveL_Request* request, api::MoveL_Response* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.ArmService/moveL", request, response, + [this, context, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); auto arm = dmgr_.getDevice(device_id); if (!arm) { @@ -501,10 +525,14 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext* context, return setControlAdmissionFailure( response, device_id, control_lease); } - if (!control_lease.current()) { + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { return setControlDispatchFailure( response, device_id, control_lease, "moveL"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } auto options = toMotionOptions( request->options(), control_lease.cancellationRequested(context)); @@ -517,17 +545,17 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext* context, << ", frame=" << request->frame(); } return setResponseResult(response, result); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } -grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*, +grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext* context, const api::SpeedJ_Request* request, api::SpeedJ_Response* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.ArmService/speedJ", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); auto arm = dmgr_.getDevice(device_id); if (!arm) { @@ -539,10 +567,14 @@ grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*, return setControlAdmissionFailure( response, device_id, control_lease); } - if (!control_lease.current()) { + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { return setControlDispatchFailure( response, device_id, control_lease, "speedJ"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } const auto result = arm->speedJ(toJointVelocityCommand(request->velocity()), request->acceleration(), request->duration()); @@ -553,17 +585,17 @@ grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*, << ", duration=" << request->duration(); } return setResponseResult(response, result); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } -grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*, +grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext* context, const api::SpeedL_Request* request, api::SpeedL_Response* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.ArmService/speedL", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); auto arm = dmgr_.getDevice(device_id); if (!arm) { @@ -575,10 +607,14 @@ grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*, return setControlAdmissionFailure( response, device_id, control_lease); } - if (!control_lease.current()) { + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { return setControlDispatchFailure( response, device_id, control_lease, "speedL"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } const auto result = arm->speedL(toCartesianVelocity(request->velocity()), request->acceleration(), request->duration(), @@ -590,17 +626,17 @@ grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*, << ", frame=" << request->frame(); } return setResponseResult(response, result); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } -grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext*, +grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext* context, const api::ServoJ_Request* request, api::ServoJ_Response* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.ArmService/servoJ", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); auto arm = dmgr_.getDevice(device_id); if (!arm) { @@ -617,23 +653,26 @@ grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext*, return setControlDispatchFailure( response, device_id, control_lease, "servoJ"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } const auto result = arm->servoJ(toJointPositionCommand(request->target())); if (result.ok()) { CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (servoJ): success, id=" << device_id << ", positions=" << request->target().position_size(); } return setResponseResult(response, result); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } -grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*, +grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext* context, const api::CommandHeader_Request* request, api::CommandHeader_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.ArmService/stopMotion", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); auto arm = dmgr_.getDevice(device_id); if (!arm) { @@ -645,6 +684,9 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*, return setControlLeaseConflict( response, device_id, control_barrier.detail()); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } const auto result = executeConfirmedArmStop( control_barrier, "stopMotion", @@ -654,16 +696,15 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*, logRpcSuccess("stopMotion", device_id); } return resultToStatus(result); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } -grpc::Status gRPCArmServiceImpl::getJointState(grpc::ServerContext*, +grpc::Status gRPCArmServiceImpl::getJointState(grpc::ServerContext* context, const api::JointRequest* request, api::JointResponse* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.ArmService/getJointState"); try { const std::string device_id = request->header().device_id(); auto arm = dmgr_.getDevice(device_id); @@ -690,10 +731,12 @@ grpc::Status gRPCArmServiceImpl::getJointState(grpc::ServerContext*, } } -grpc::Status gRPCArmServiceImpl::getPose(grpc::ServerContext*, +grpc::Status gRPCArmServiceImpl::getPose(grpc::ServerContext* context, const api::GetPose_Request* request, api::GetPose_Response* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.ArmService/getPose"); try { const std::string device_id = request->header().device_id(); auto arm = dmgr_.getDevice(device_id); @@ -715,11 +758,14 @@ grpc::Status gRPCArmServiceImpl::getPose(grpc::ServerContext*, } } -grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*, +grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext* context, const api::CalibrateZeroQ_Request* request, api::CalibrateZeroQ_Response* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.ArmService/calibrateZeroQ", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); auto arm = dmgr_.getDevice(device_id); if (!arm) { @@ -731,44 +777,53 @@ grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*, return setControlAdmissionFailure( response, device_id, control_lease); } - if (!control_lease.current()) { + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { return setControlDispatchFailure( response, device_id, control_lease, "calibrateZeroQ"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } const auto result = arm->calibrateZeroQ(request->joint_name()); if (result.ok()) { CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (calibrateZeroQ): success, id=" << device_id << ", joint=" << request->joint_name(); } return setResponseResult(response, result); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } -grpc::Status gRPCArmServiceImpl::getPoseMatrix(grpc::ServerContext*, +grpc::Status gRPCArmServiceImpl::getPoseMatrix(grpc::ServerContext* context, const api::GetPoseMatrix_Request*, api::GetPoseMatrix_Response* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.ArmService/getPoseMatrix"); fillFeedback(response->mutable_header(), false, "getPoseMatrix is not implemented"); return grpc::Status(grpc::StatusCode::UNIMPLEMENTED, "getPoseMatrix is not implemented"); } -grpc::Status gRPCArmServiceImpl::computeForwardKinematics(grpc::ServerContext*, +grpc::Status gRPCArmServiceImpl::computeForwardKinematics(grpc::ServerContext* context, const api::ComputeForwardKinematics_Request*, api::ComputeForwardKinematics_Response* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.ArmService/computeForwardKinematics"); fillFeedback(response->mutable_header(), false, "computeForwardKinematics is not implemented"); return grpc::Status(grpc::StatusCode::UNIMPLEMENTED, "computeForwardKinematics is not implemented"); } grpc::Status gRPCArmServiceImpl::ExecuteJsonCommand( - grpc::ServerContext*, + grpc::ServerContext* context, const api::JsonDeviceCommand_Request* request, api::JsonDeviceCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.ArmService/ExecuteJsonCommand", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); auto arm = dmgr_.getDevice(device_id); if (!arm) { @@ -785,11 +840,15 @@ grpc::Status gRPCArmServiceImpl::ExecuteJsonCommand( return setControlAdmissionFailure( response, device_id, control_lease); } - if (!control_lease.current()) { + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { return setControlDispatchFailure( response, device_id, control_lease, "ExecuteJsonCommand"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } std::string response_json; const bool success = arm->executeJsonCommand( @@ -803,17 +862,17 @@ grpc::Status gRPCArmServiceImpl::ExecuteJsonCommand( logRpcSuccess("ExecuteJsonCommand", device_id); } return grpc::Status::OK; - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context, const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.ArmService/clearFault", request, response, + [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); auto arm = dmgr_.getDevice(device_id); if (!arm) { @@ -830,12 +889,12 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context, return setControlDispatchFailure( response, device_id, control_lease, "clearFault"); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } const auto result = arm->clearFault(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); return resultToStatus(result); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + }); } } // namespace cmvr::service diff --git a/cmvr-es/service/grpc/src/grpc_arm_teleop_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_teleop_service.cpp index c90c75b3..486659ed 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_teleop_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_teleop_service.cpp @@ -15,6 +15,8 @@ #include #include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/include/grpc_command_transaction.h" +#include "service/grpc/include/grpc_security.h" namespace cmvr::service { @@ -436,11 +438,17 @@ std::shared_ptr makeDisabledArmTeleopBackend() ArmTeleopServiceImpl::ArmTeleopServiceImpl( std::shared_ptr backend, - control::ControlAuthorityManager* authority) + control::ControlAuthorityManager* authority, + std::shared_ptr security_gateway, + safety::SafetyCoordinator* safety_coordinator) : backend_(std::move(backend)), authority_( authority ? authority - : &control::ControlAuthorityManager::instance()) + : &control::ControlAuthorityManager::instance()), + security_gateway_(security_gateway + ? std::move(security_gateway) + : makeDefaultGrpcSecurityGateway()), + safety_coordinator_(safety_coordinator) { if (!backend_) { backend_ = makeDisabledArmTeleopBackend(); @@ -452,6 +460,9 @@ grpc::Status ArmTeleopServiceImpl::Teleoperate( grpc::ServerReaderWriter* stream) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.armteleop.v1.ArmTeleopService/Teleoperate"); if (context == nullptr || stream == nullptr) { return grpc::Status( grpc::StatusCode::INTERNAL, @@ -529,6 +540,25 @@ grpc::Status ArmTeleopServiceImpl::Teleoperate( authority_->release(control_lease); }); + std::optional safety_session; + if (safety_coordinator_) { + GrpcStreamingSafetyOpen safety_open; + safety_open.full_method_name = + "/cmvr.api.armteleop.v1.ArmTeleopService/Teleoperate"; + safety_open.device_id = backend_manifest.robot_id(); + safety_open.session_id = session_id; + safety_open.authority_generation = control_lease.generation; + safety_open.deadline = cmvr_grpc_call_guard.context().deadline; + safety_session.emplace( + *safety_coordinator_, + cmvr_grpc_call_guard.context(), + std::move(safety_open)); + if (!safety_session->admitted()) { + writeBareRejection(stream, safety_session->status()); + return safety_session->status(); + } + } + if (first_frame.open().request_force_feedback() && !backend_->supportsForceFeedback()) { const grpc::Status status( @@ -577,6 +607,13 @@ grpc::Status ArmTeleopServiceImpl::Teleoperate( writeBareRejection(stream, status); return status; } + auto safety_dispatch = safety_session + ? safety_session->beginDispatch() + : safety::DispatchGuard{}; + if (safety_session && !safety_dispatch.acquired()) { + writeBareRejection(stream, safety_session->status()); + return safety_session->status(); + } backend_open_attempted = true; backend_open = backend_->open(first_frame.open()); } @@ -831,6 +868,17 @@ grpc::Status ArmTeleopServiceImpl::Teleoperate( grpc::StatusCode::ABORTED, detail), true); } + if (safety_session && !safety_session->revalidate()) { + const std::string detail = + "arm teleoperation safety session was invalidated: " + + safety_session->status().error_message(); + return finish( + arm_teleop::SESSION_PHASE_LEASE_LOST, + arm_teleop::STOP_REASON_EMERGENCY_STOP, + detail, + safety_session->status(), + true); + } std::optional pending; bool ended = false; @@ -1117,6 +1165,20 @@ grpc::Status ArmTeleopServiceImpl::Teleoperate( grpc::StatusCode::ABORTED, detail), true); } + auto safety_dispatch = safety_session + ? safety_session->beginDispatch() + : safety::DispatchGuard{}; + if (safety_session && !safety_dispatch.acquired()) { + const std::string detail = + "arm teleoperation setpoint rejected by safety: " + + safety_session->status().error_message(); + return finish( + arm_teleop::SESSION_PHASE_LEASE_LOST, + arm_teleop::STOP_REASON_EMERGENCY_STOP, + detail, + safety_session->status(), + true); + } applied = backend_->applySetpoint( setpoint, command_deadline); } diff --git a/cmvr-es/service/grpc/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/src/grpc_camera_service.cpp index 1f7712b3..4c590814 100644 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_camera_service.cpp @@ -2,7 +2,9 @@ #include "manager/media_source_hub/include/device_media_source_adapter.h" #include "service/grpc/include/camera_operational_activity_registry.h" #include "service/grpc/include/camera_ptz_activity_registry.h" +#include "service/grpc/include/grpc_command_transaction.h" #include "service/grpc/include/media_activity_coordinator.h" +#include "service/grpc/include/grpc_security.h" // // Created by xtkuang on 2025/6/1. // @@ -13,6 +15,7 @@ #include #include #include +#include using namespace std; using namespace cmvr::service; @@ -76,9 +79,15 @@ class CameraStreamingLease final { public: CameraStreamingLease( std::shared_ptr camera, - const MediaActivityCoordinator::Session& session) + const MediaActivityCoordinator::Session& session, + cmvr::safety::SafetyCoordinator& coordinator) : camera_(std::move(camera)) { - (void)session.runIfCurrent([this] { + (void)session.runIfCurrent([this, &coordinator] { + auto dispatch = cmvr::media::beginMediaSourceStartDispatch( + coordinator, camera_ ? camera_->id() : std::string{}); + if (!dispatch.acquired()) { + return; + } active_ = camera_ && camera_->startStreaming(); }); } @@ -128,13 +137,19 @@ grpc::Status rejectStreamDuringStopAll(StreamT* stream) } gRPCCameraServiceImpl::gRPCCameraServiceImpl( - CameraStreamLowLatencyConfig stream_config) + CameraStreamLowLatencyConfig stream_config, + std::shared_ptr security_gateway) : dmgr_(DeviceManager::getInstance()), - stream_config_(stream_config) {} + stream_config_(stream_config), + security_gateway_(security_gateway + ? std::move(security_gateway) + : makeDefaultGrpcSecurityGateway()) {} grpc::Status gRPCCameraServiceImpl::GetStatus(grpc::ServerContext* context, const api::GetCameraStateCommand_Request* request, api::GetCameraStateCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.CameraService/GetStatus"); try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetStatus): id=" << dev_id; @@ -168,12 +183,15 @@ grpc::Status gRPCCameraServiceImpl::GetStatus(grpc::ServerContext* context, grpc::Status gRPCCameraServiceImpl::StartCamera(grpc::ServerContext* context, const api::StartCameraCommand_Request* request, api::StartCameraCommand_Feedback* response) { - auto media_session = globalMediaActivityCoordinator().beginSession(); - if (!media_session) { - return failResponse( - response, "Media activities are temporarily paused by StopAll"); - } - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.CameraService/StartCamera", request, response, + [this, request, response](GrpcCommandTransaction& command) { + auto media_session = globalMediaActivityCoordinator().beginSession(); + if (!media_session) { + return failResponse( + response, "Media activities are temporarily paused by StopAll"); + } string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StartCamera): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); @@ -184,12 +202,18 @@ grpc::Status gRPCCameraServiceImpl::StartCamera(grpc::ServerContext* context, CameraOperationalActivityRegistry::DispatchResult::DeviceFailure; const bool start_allowed = media_session.runIfCurrent([&] { dispatch = globalCameraOperationalActivityRegistry().start( - dev_id, dev); + dev_id, dev, nullptr, + [&command] { return command.beginDispatch(); }); }); if (!start_allowed) { return failResponse( response, "Camera start was canceled by StopAll"); } + if (dispatch == + CameraOperationalActivityRegistry::DispatchResult:: + RejectedByDispatchFence) { + return command.dispatchStatus(); + } if (dispatch == CameraOperationalActivityRegistry::DispatchResult:: RejectedByStopAll) { @@ -203,19 +227,16 @@ grpc::Status gRPCCameraServiceImpl::StartCamera(grpc::ServerContext* context, response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); return grpc::Status::OK; - } - catch (const exception &e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCCameraServiceImpl::StopCamera(grpc::ServerContext* context, const api::StopCameraCommand_Request* request, api::StopCameraCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.CameraService/StopCamera", request, response, + [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StopCamera): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); @@ -224,13 +245,19 @@ grpc::Status gRPCCameraServiceImpl::StopCamera(grpc::ServerContext* context, } const auto dispatch = globalCameraOperationalActivityRegistry().stopLifecycle( - dev_id, dev); + dev_id, dev, + [&command] { return command.beginDispatch(); }); if (dispatch == CameraOperationalActivityRegistry::DispatchResult:: RejectedByStopAll) { return failResponse( response, "Camera control is temporarily paused by StopAll"); } + if (dispatch == + CameraOperationalActivityRegistry::DispatchResult:: + RejectedByDispatchFence) { + return command.dispatchStatus(); + } if (dispatch == CameraOperationalActivityRegistry::DispatchResult::DeviceFailure) { return failResponse(response, "Failed to stop camera: " + dev_id); @@ -239,18 +266,14 @@ grpc::Status gRPCCameraServiceImpl::StopCamera(grpc::ServerContext* context, response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); return grpc::Status::OK; - } - catch (exception &e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context, const api::GetRGBImageCommand_Request* request, api::GetRGBImageCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.CameraService/GetRGBImage"); auto media_session = globalMediaActivityCoordinator().beginSession( [context] { if (context) { @@ -318,6 +341,8 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context, grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context, const api::GetDepthImageCommand_Request* request, api::GetDepthImageCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.CameraService/GetDepthImage"); auto media_session = globalMediaActivityCoordinator().beginSession( [context] { if (context) { @@ -388,6 +413,8 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context, grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, const api::GetRGBDImagesCommand_Request* request, api::GetRGBDImagesCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.CameraService/GetRGBDImages"); auto media_session = globalMediaActivityCoordinator().beginSession( [context] { if (context) { @@ -474,63 +501,90 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, grpc::Status gRPCCameraServiceImpl::StartRecording(grpc::ServerContext* context, const api::StartCameraRecordingCommand_Request* request, api::StartCameraRecordingCommand_Feedback* response) { - auto media_session = globalMediaActivityCoordinator().beginSession(); - if (!media_session) { - return failResponse( - response, "Media activities are temporarily paused by StopAll"); - } - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.CameraService/StartRecording", request, response, + [this, request, response](GrpcCommandTransaction& command) { + auto media_session = globalMediaActivityCoordinator().beginSession(); + if (!media_session) { + return failResponse( + response, "Media activities are temporarily paused by StopAll"); + } string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StartRecording): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); if (!dev) { return failResponse(response, "Camera device not found: " + dev_id); } + bool dispatch_allowed = false; if (!media_session.runIfCurrent([&] { + dispatch_allowed = command.beginDispatch(); + if (!dispatch_allowed) { + return; + } dev->startRecording(request->video_path()); })) { return failResponse( response, "Camera recording start was canceled by StopAll"); } + if (!dispatch_allowed) { + return command.dispatchStatus(); + } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); return grpc::Status::OK; - } - catch (exception &e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCCameraServiceImpl::StopRecording(grpc::ServerContext* context, const api::StopCameraRecordingCommand_Request* request, api::StopCameraRecordingCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.CameraService/StopRecording", request, response, + [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StopRecording): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); if (!dev) { return failResponse(response, "Camera device not found: " + dev_id); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } dev->stopRecording(); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); return grpc::Status::OK; - } - catch (exception &e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCCameraServiceImpl::ControlPtz(grpc::ServerContext* context, const api::ControlPtzCommand_Request* request, api::ControlPtzCommand_Feedback* response) { - try { + if (!request) { + return grpc::Status( + grpc::StatusCode::INTERNAL, "ControlPtz request is null"); + } + const auto registered = defaultGrpcMethodPolicyRegistry().find( + "/cmvr.api.CameraService/ControlPtz"); + if (!registered.has_value()) { + return grpc::Status( + grpc::StatusCode::INTERNAL, + "ControlPtz method policy is not registered"); + } + auto effective_policy = *registered; + if (request->action() == api::ControlPtzCommand_Action_STOP) { + effective_policy.access = GrpcAccessClass::Stop; + effective_policy.command_intent = safety::CommandIntent::Stop; + effective_policy.safety_lane = true; + } + + return executeServerDerivedGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.CameraService/ControlPtz", std::move(effective_policy), + request, response, + [this, request, response](GrpcCommandTransaction& command_tx) { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (ControlPtz): id=" << dev_id << ", command=" << request->command() @@ -556,11 +610,16 @@ grpc::Status gRPCCameraServiceImpl::ControlPtz(grpc::ServerContext* context, dev, command, stop, - static_cast(request->speed())); + static_cast(request->speed()), + [&command_tx] { return command_tx.beginDispatch(); }); if (dispatch == CameraPtzActivityRegistry::DispatchResult::RejectedByStopAll) { return failResponse( response, "PTZ control is temporarily paused by StopAll"); } + if (dispatch == + CameraPtzActivityRegistry::DispatchResult::RejectedByDispatchFence) { + return command_tx.dispatchStatus(); + } if (dispatch == CameraPtzActivityRegistry::DispatchResult::DeviceFailure) { CameraState state{}; dev->getState(state); @@ -572,18 +631,15 @@ grpc::Status gRPCCameraServiceImpl::ControlPtz(grpc::ServerContext* context, response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); return grpc::Status::OK; - } - catch (exception &e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* context , grpc::ServerReaderWriter* stream){ + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.CameraService/GetDepthImageStream"); auto media_session = globalMediaActivityCoordinator().beginSession( [context] { context->TryCancel(); }); if (!media_session) { @@ -609,7 +665,8 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con stream->Write(response); return grpc::Status::OK; } - CameraStreamingLease stream_lease(dev, media_session); + CameraStreamingLease stream_lease( + dev, media_session, dmgr_.safetyCoordinator()); if (!stream_lease) { api::GetDepthImageStreamCommand_Feedback response; response.mutable_header()->set_success(false); @@ -677,6 +734,9 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con } grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* context , grpc::ServerReaderWriter* stream){ + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.CameraService/GetRGBDImagesStream"); auto media_session = globalMediaActivityCoordinator().beginSession( [context] { context->TryCancel(); }); if (!media_session) { @@ -702,7 +762,8 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con stream->Write(response); return grpc::Status::OK; } - CameraStreamingLease stream_lease(dev, media_session); + CameraStreamingLease stream_lease( + dev, media_session, dmgr_.safetyCoordinator()); if (!stream_lease) { api::GetRGBDImagesStreamCommand_Feedback response; response.mutable_header()->set_success(false); @@ -776,6 +837,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con } } grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream){ + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.CameraService/GetRGBImageStream"); auto media_session = globalMediaActivityCoordinator().beginSession( [context] { context->TryCancel(); }); if (!media_session) { @@ -820,12 +884,16 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte stream->Write(response); return grpc::Status::OK; } - auto subscription = media_hub.subscribe( + auto source_dispatch = cmvr::media::beginMediaSourceStartDispatch( + dmgr_.safetyCoordinator(), dev_id); + auto subscription = source_dispatch.acquired() + ? media_hub.subscribe( track_id, cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED, [context, &media_session] { return context->IsCancelled() || media_session.cancelled(); - }); + }) + : cmvr::media::MediaSourceHub::Subscription{}; if (!subscription) { api::GetRGBImageStreamCommand_Feedback response; response.mutable_header()->set_success(false); diff --git a/cmvr-es/service/grpc/src/grpc_command_transaction.cpp b/cmvr-es/service/grpc/src/grpc_command_transaction.cpp new file mode 100644 index 00000000..f097ea11 --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_command_transaction.cpp @@ -0,0 +1,1269 @@ +#include "service/grpc/include/grpc_command_transaction.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "service/grpc/include/grpc_safety_proto.h" + +namespace cmvr::service { + +namespace { + +constexpr auto kDefaultCommandValidity = std::chrono::seconds(30); +constexpr auto kMaximumCommandValidity = std::chrono::minutes(5); +constexpr std::size_t kMaximumDeviceIdLength = 256; + +const api::CommandHeader_Request* findRequestHeader( + const google::protobuf::Message& message) +{ + if (const auto* header = + dynamic_cast(&message)) { + return header; + } + const auto* reflection = message.GetReflection(); + std::vector fields; + reflection->ListFields(message, &fields); + for (const auto* field : fields) { + if (field->cpp_type() != + google::protobuf::FieldDescriptor::CPPTYPE_MESSAGE) { + continue; + } + if (field->is_repeated()) { + for (int index = 0; + index < reflection->FieldSize(message, field); ++index) { + if (const auto* header = findRequestHeader( + reflection->GetRepeatedMessage( + message, field, index))) { + return header; + } + } + } else if (const auto* header = findRequestHeader( + reflection->GetMessage(message, field))) { + return header; + } + } + return nullptr; +} + +api::CommandHeader_Feedback* findFeedbackHeader( + google::protobuf::Message& message) +{ + if (auto* header = dynamic_cast(&message)) { + return header; + } + const auto* reflection = message.GetReflection(); + const auto* descriptor = message.GetDescriptor(); + for (int index = 0; index < descriptor->field_count(); ++index) { + const auto* field = descriptor->field(index); + if (field->cpp_type() != + google::protobuf::FieldDescriptor::CPPTYPE_MESSAGE || + field->is_repeated()) { + continue; + } + auto* nested = reflection->MutableMessage(&message, field); + if (auto* header = findFeedbackHeader(*nested)) { + return header; + } + } + return nullptr; +} + +void canonicalizeMessage(google::protobuf::Message& message) +{ + if (auto* header = dynamic_cast(&message)) { + header->clear_timestamp(); + header->clear_command_id(); + header->clear_expected_service_instance_id(); + header->clear_valid_for_ms(); + } + + const auto* reflection = message.GetReflection(); + std::vector fields; + reflection->ListFields(message, &fields); + for (const auto* field : fields) { + if (field->cpp_type() != + google::protobuf::FieldDescriptor::CPPTYPE_MESSAGE) { + continue; + } + if (field->is_repeated()) { + const int size = reflection->FieldSize(message, field); + for (int index = 0; index < size; ++index) { + canonicalizeMessage( + *reflection->MutableRepeatedMessage( + &message, field, index)); + } + } else { + canonicalizeMessage( + *reflection->MutableMessage(&message, field)); + } + } +} + +bool deterministicSerialize( + const google::protobuf::Message& message, + std::string& serialized) +{ + serialized.clear(); + google::protobuf::io::StringOutputStream stream(&serialized); + google::protobuf::io::CodedOutputStream coded_stream(&stream); + coded_stream.SetSerializationDeterministic(true); + if (!message.SerializeToCodedStream(&coded_stream)) { + serialized.clear(); + return false; + } + coded_stream.Trim(); + return !coded_stream.HadError(); +} + +std::uint64_t stableHash( + const std::string& value, + const std::uint64_t seed) noexcept +{ + std::uint64_t hash = 1469598103934665603ULL ^ seed; + for (const unsigned char byte : value) { + hash ^= static_cast(byte); + hash *= 1099511628211ULL; + } + hash ^= hash >> 33U; + hash *= 0xff51afd7ed558ccdULL; + hash ^= hash >> 33U; + hash *= 0xc4ceb9fe1a85ec53ULL; + hash ^= hash >> 33U; + return hash; +} + +bool isEnforced( + const safety::SafetyCoordinatorConfig& config, + const std::string& device_id) +{ + switch (config.enforcement_mode) { + case safety::EnforcementMode::Legacy: + case safety::EnforcementMode::Shadow: + return false; + case safety::EnforcementMode::EnforceSelected: + return config.enforced_device_ids.count(device_id) != 0; + case safety::EnforcementMode::EnforceAll: + return true; + } + return false; +} + +bool commandIdentityRequired( + const GrpcMethodPolicy& policy, + const bool enforced) noexcept +{ + return enforced && policy.mutating && + policy.command_intent != safety::CommandIntent::Stop && + policy.command_intent != safety::CommandIntent::RecoverAdmission; +} + +api::CommandExecutionState toApiLifecycle( + const safety::CommandLifecycle lifecycle) noexcept +{ + using safety::CommandLifecycle; + switch (lifecycle) { + case CommandLifecycle::Received: + return api::COMMAND_EXECUTION_STATE_RECEIVED; + case CommandLifecycle::Reserved: + return api::COMMAND_EXECUTION_STATE_RESERVED; + case CommandLifecycle::RejectedBeforeDispatch: + return api::COMMAND_EXECUTION_STATE_REJECTED_BEFORE_DISPATCH; + case CommandLifecycle::Admitted: + return api::COMMAND_EXECUTION_STATE_ADMITTED; + case CommandLifecycle::Dispatching: + return api::COMMAND_EXECUTION_STATE_DISPATCHING; + case CommandLifecycle::AcceptedByHardware: + return api::COMMAND_EXECUTION_STATE_ACCEPTED_BY_HARDWARE; + case CommandLifecycle::Completed: + return api::COMMAND_EXECUTION_STATE_COMPLETED; + case CommandLifecycle::Failed: + return api::COMMAND_EXECUTION_STATE_FAILED; + case CommandLifecycle::CanceledBeforeDispatch: + return api::COMMAND_EXECUTION_STATE_CANCELED_BEFORE_DISPATCH; + case CommandLifecycle::OutcomeUnknown: + return api::COMMAND_EXECUTION_STATE_OUTCOME_UNKNOWN; + } + return api::COMMAND_EXECUTION_STATE_UNSPECIFIED; +} + +grpc::Status statusForReason( + const safety::SafetyReason reason, + const std::string& detail) +{ + using safety::SafetyReason; + grpc::StatusCode code = grpc::StatusCode::FAILED_PRECONDITION; + switch (reason) { + case SafetyReason::None: return grpc::Status::OK; + case SafetyReason::InvalidArgument: + case SafetyReason::CommandIdRequired: + case SafetyReason::RecoveryReasonRequired: + code = grpc::StatusCode::INVALID_ARGUMENT; + break; + case SafetyReason::Unauthenticated: + code = grpc::StatusCode::UNAUTHENTICATED; + break; + case SafetyReason::PermissionDenied: + code = grpc::StatusCode::PERMISSION_DENIED; + break; + case SafetyReason::DeviceNotFound: + code = grpc::StatusCode::NOT_FOUND; + break; + case SafetyReason::UnsupportedCommand: + code = grpc::StatusCode::UNIMPLEMENTED; + break; + case SafetyReason::DeviceUnavailable: + case SafetyReason::DeviceDisconnected: + case SafetyReason::SafetyStateMissing: + case SafetyReason::SafetyStateStale: + case SafetyReason::SystemStarting: + code = grpc::StatusCode::UNAVAILABLE; + break; + case SafetyReason::SystemStopping: + case SafetyReason::GenerationMismatch: + case SafetyReason::OutcomeUnknown: + code = grpc::StatusCode::ABORTED; + break; + case SafetyReason::CommandIdConflict: + code = grpc::StatusCode::ALREADY_EXISTS; + break; + case SafetyReason::ResultEvicted: + code = grpc::StatusCode::FAILED_PRECONDITION; + break; + case SafetyReason::LedgerExhausted: + case SafetyReason::Backpressure: + case SafetyReason::ControlBusy: + code = grpc::StatusCode::RESOURCE_EXHAUSTED; + break; + case SafetyReason::DeadlineExceededBeforeDispatch: + case SafetyReason::ParticipantTimeout: + code = grpc::StatusCode::DEADLINE_EXCEEDED; + break; + case SafetyReason::InternalError: + code = grpc::StatusCode::INTERNAL; + break; + case SafetyReason::RecoveryRpcDisabled: + case SafetyReason::SafetyLatched: + case SafetyReason::HardwareUnsafe: + case SafetyReason::EmergencyStopActive: + case SafetyReason::ProtectiveStopActive: + case SafetyReason::DeviceFault: + case SafetyReason::DeviceNotReady: + case SafetyReason::DeviceStillMoving: + case SafetyReason::StopUnconfirmed: + case SafetyReason::RecoveryEpochMismatch: + case SafetyReason::RecoveryAuditFailed: + break; + } + return grpc::Status( + code, + detail.empty() ? safety::toString(reason) : detail); +} + +safety::SafetyReason reasonForStatus(const grpc::Status& status) noexcept +{ + if (status.ok()) { + return safety::SafetyReason::InternalError; + } + switch (status.error_code()) { + case grpc::StatusCode::INVALID_ARGUMENT: + case grpc::StatusCode::OUT_OF_RANGE: + return safety::SafetyReason::InvalidArgument; + case grpc::StatusCode::UNAUTHENTICATED: + return safety::SafetyReason::Unauthenticated; + case grpc::StatusCode::PERMISSION_DENIED: + return safety::SafetyReason::PermissionDenied; + case grpc::StatusCode::NOT_FOUND: + return safety::SafetyReason::DeviceNotFound; + case grpc::StatusCode::UNIMPLEMENTED: + return safety::SafetyReason::UnsupportedCommand; + case grpc::StatusCode::UNAVAILABLE: + return safety::SafetyReason::DeviceUnavailable; + case grpc::StatusCode::RESOURCE_EXHAUSTED: + return safety::SafetyReason::ControlBusy; + case grpc::StatusCode::DEADLINE_EXCEEDED: + return safety::SafetyReason::DeadlineExceededBeforeDispatch; + case grpc::StatusCode::ABORTED: + return safety::SafetyReason::GenerationMismatch; + default: + return safety::SafetyReason::InternalError; + } +} + +bool validDeviceId(const std::string& device_id) noexcept +{ + return !device_id.empty() && device_id.size() <= kMaximumDeviceIdLength; +} + +} // namespace + +grpc::Status grpcStatusForSafetyReason( + const safety::SafetyReason reason, + const std::string& detail) +{ + return statusForReason(reason, detail); +} + +GrpcStreamingSafetySession::GrpcStreamingSafetySession( + safety::SafetyCoordinator& coordinator, + const GrpcRequestContext& request_context, + GrpcStreamingSafetyOpen open) + : coordinator_(&coordinator), + status_(grpc::Status::OK) +{ + if (!validDeviceId(open.device_id) || open.session_id.empty()) { + reject_( + safety::SafetyReason::InvalidArgument, + "stream device_id and session_id are required"); + return; + } + const auto policy = + defaultGrpcMethodPolicyRegistry().find(open.full_method_name); + if (!policy.has_value() || !policy->mutating) { + reject_( + safety::SafetyReason::InternalError, + "stream requires a registered mutating method policy"); + return; + } + if (!open.expected_service_instance_id.empty() && + open.expected_service_instance_id != coordinator.serviceInstanceId()) { + reject_( + safety::SafetyReason::GenerationMismatch, + "expected_service_instance_id does not match the active control service instance"); + return; + } + + safety::AdmissionRequest request; + request.command = policy->commandDescriptor(); + request.actor.principal_id = request_context.principal.id; + request.actor.authenticated = request_context.principal.authenticated; + for (const auto role : request_context.principal.roles) { + request.actor.roles.emplace_back(toString(role)); + } + request.command_id = std::move(open.session_id); + request.device_id = std::move(open.device_id); + request.expected_device_generation = + open.expected_device_generation; + request.authority_generation = open.authority_generation; + request.deadline = std::min(open.deadline, request_context.deadline); + + auto admitted = coordinator.admit(request); + admission_decision_ = admitted.decision; + if (!admitted.permit.has_value()) { + reject_( + admitted.decision.reason == safety::SafetyReason::None + ? safety::SafetyReason::InternalError + : admitted.decision.reason, + admitted.decision.detail); + return; + } + permit_ = std::move(admitted.permit); +} + +bool GrpcStreamingSafetySession::revalidate() +{ + if (!coordinator_ || !permit_.has_value()) { + reject_( + safety::SafetyReason::InternalError, + "stream safety session is not admitted"); + return false; + } + const auto check = coordinator_->revalidatePermit(*permit_); + if (!check.safe) { + reject_( + check.reason == safety::SafetyReason::None + ? safety::SafetyReason::SafetyLatched + : check.reason, + check.detail); + return false; + } + status_ = grpc::Status::OK; + return true; +} + +safety::DispatchGuard GrpcStreamingSafetySession::beginDispatch() +{ + if (!coordinator_ || !permit_.has_value()) { + reject_( + safety::SafetyReason::InternalError, + "stream safety session is not dispatchable"); + return {}; + } + auto guard = coordinator_->beginDispatch(*permit_); + if (!guard.acquired()) { + const auto& check = guard.hardwareCheck(); + reject_( + check.reason == safety::SafetyReason::None + ? safety::SafetyReason::SafetyLatched + : check.reason, + check.detail); + return guard; + } + status_ = grpc::Status::OK; + return guard; +} + +std::uint64_t GrpcStreamingSafetySession::safetyEpoch() const noexcept +{ + return permit_ ? permit_->safety_epoch : 0U; +} + +std::uint64_t GrpcStreamingSafetySession::deviceGeneration() const noexcept +{ + return permit_ ? permit_->device_generation : 0U; +} + +std::uint64_t GrpcStreamingSafetySession::authorityGeneration() const noexcept +{ + return permit_ ? permit_->authority_generation : 0U; +} + +void GrpcStreamingSafetySession::reject_( + const safety::SafetyReason reason, + std::string detail) +{ + status_ = grpcStatusForSafetyReason(reason, detail); + permit_.reset(); +} + +std::string deterministicGrpcPayloadHash( + const std::string& full_method_name, + const google::protobuf::Message& request) +{ + std::unique_ptr normalized(request.New()); + if (!normalized) { + return {}; + } + normalized->CopyFrom(request); + normalized->DiscardUnknownFields(); + canonicalizeMessage(*normalized); + + std::string serialized; + if (!deterministicSerialize(*normalized, serialized)) { + return {}; + } + serialized.insert(0, full_method_name + '\0'); + constexpr std::array seeds{ + 0xa4093822299f31d0ULL, + 0x082efa98ec4e6c89ULL, + 0x452821e638d01377ULL, + 0xbe5466cf34e90c6cULL}; + std::ostringstream output; + output << std::hex << std::setfill('0'); + for (const auto seed : seeds) { + output << std::setw(16) << stableHash(serialized, seed); + } + output << ':' << std::dec << serialized.size(); + return output.str(); +} + +GrpcCommandTransaction::GrpcCommandTransaction( + safety::SafetyCoordinator& coordinator, + GrpcRequestContext request_context, + GrpcMethodPolicy method_policy, + const google::protobuf::Message& request, + google::protobuf::Message& response) + : coordinator_(&coordinator), + request_context_(std::move(request_context)), + method_policy_(std::move(method_policy)), + response_(&response), + status_(grpc::Status::OK), + dispatch_status_(grpc::Status::OK) +{ + initialize_(request); +} + +GrpcCommandTransaction::~GrpcCommandTransaction() noexcept +{ + abandon_(); +} + +GrpcCommandTransaction::GrpcCommandTransaction( + GrpcCommandTransaction&& other) noexcept + : coordinator_(std::exchange(other.coordinator_, nullptr)), + request_context_(std::move(other.request_context_)), + method_policy_(std::move(other.method_policy_)), + response_(std::exchange(other.response_, nullptr)), + ledger_ticket_(std::move(other.ledger_ticket_)), + permit_(std::move(other.permit_)), + dispatch_guard_(std::move(other.dispatch_guard_)), + admission_decision_(std::move(other.admission_decision_)), + status_(std::move(other.status_)), + dispatch_status_(std::move(other.dispatch_status_)), + device_id_(std::move(other.device_id_)), + command_id_(std::move(other.command_id_)), + payload_hash_(std::move(other.payload_hash_)), + deadline_(other.deadline_), + should_execute_(other.should_execute_), + owns_ledger_record_(other.owns_ledger_record_), + dispatch_started_(other.dispatch_started_), + completed_(other.completed_), + safety_stop_sequence_(other.safety_stop_sequence_) +{ + other.should_execute_ = false; + other.owns_ledger_record_ = false; + other.dispatch_started_ = false; + other.completed_ = true; +} + +GrpcCommandTransaction& GrpcCommandTransaction::operator=( + GrpcCommandTransaction&& other) noexcept +{ + if (this == &other) { + return *this; + } + abandon_(); + coordinator_ = std::exchange(other.coordinator_, nullptr); + request_context_ = std::move(other.request_context_); + method_policy_ = std::move(other.method_policy_); + response_ = std::exchange(other.response_, nullptr); + ledger_ticket_ = std::move(other.ledger_ticket_); + permit_ = std::move(other.permit_); + dispatch_guard_ = std::move(other.dispatch_guard_); + admission_decision_ = std::move(other.admission_decision_); + status_ = std::move(other.status_); + dispatch_status_ = std::move(other.dispatch_status_); + device_id_ = std::move(other.device_id_); + command_id_ = std::move(other.command_id_); + payload_hash_ = std::move(other.payload_hash_); + deadline_ = other.deadline_; + should_execute_ = other.should_execute_; + owns_ledger_record_ = other.owns_ledger_record_; + dispatch_started_ = other.dispatch_started_; + completed_ = other.completed_; + safety_stop_sequence_ = other.safety_stop_sequence_; + other.should_execute_ = false; + other.owns_ledger_record_ = false; + other.dispatch_started_ = false; + other.completed_ = true; + return *this; +} + +void GrpcCommandTransaction::initialize_( + const google::protobuf::Message& request) +{ + const auto* header = findRequestHeader(request); + if (!header) { + rejectBeforeDispatch_( + safety::SafetyReason::InvalidArgument, + "mutating unary request has no CommandHeader.Request", + grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "mutating unary request has no CommandHeader.Request"), + false); + return; + } + device_id_ = header->device_id(); + command_id_ = header->command_id(); + if (!validDeviceId(device_id_)) { + rejectBeforeDispatch_( + safety::SafetyReason::InvalidArgument, + "header.device_id is required and must not exceed 256 bytes", + grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "header.device_id is required and must not exceed 256 bytes"), + false); + return; + } + + const bool enforced = isEnforced(coordinator_->config(), device_id_); + if (commandIdentityRequired(method_policy_, enforced) && + command_id_.empty()) { + rejectBeforeDispatch_( + safety::SafetyReason::CommandIdRequired, + "header.command_id is required for an enforced mutating command", + statusForReason( + safety::SafetyReason::CommandIdRequired, + "header.command_id is required for an enforced mutating command"), + false); + return; + } + if (commandIdentityRequired(method_policy_, enforced) && + header->expected_service_instance_id().empty()) { + rejectBeforeDispatch_( + safety::SafetyReason::GenerationMismatch, + "header.expected_service_instance_id is required for an enforced mutating command", + statusForReason( + safety::SafetyReason::GenerationMismatch, + "header.expected_service_instance_id is required for an enforced mutating command"), + false); + return; + } + if (!header->expected_service_instance_id().empty() && + header->expected_service_instance_id() != + coordinator_->serviceInstanceId()) { + rejectBeforeDispatch_( + safety::SafetyReason::GenerationMismatch, + "header.expected_service_instance_id does not match the active control service instance", + statusForReason( + safety::SafetyReason::GenerationMismatch, + "header.expected_service_instance_id does not match the active control service instance"), + false); + return; + } + if (header->valid_for_ms() != 0 && + std::chrono::milliseconds(header->valid_for_ms()) > + kMaximumCommandValidity) { + rejectBeforeDispatch_( + safety::SafetyReason::InvalidArgument, + "header.valid_for_ms exceeds the five minute server limit", + statusForReason( + safety::SafetyReason::InvalidArgument, + "header.valid_for_ms exceeds the five minute server limit"), + false); + return; + } + if (enforced && + method_policy_.command_intent == safety::CommandIntent::Actuate && + header->valid_for_ms() == 0) { + rejectBeforeDispatch_( + safety::SafetyReason::InvalidArgument, + "header.valid_for_ms is required for an enforced actuation command", + statusForReason( + safety::SafetyReason::InvalidArgument, + "header.valid_for_ms is required for an enforced actuation command"), + false); + return; + } + + const auto validity = header->valid_for_ms() == 0 + ? kDefaultCommandValidity + : std::chrono::duration_cast( + std::chrono::milliseconds(header->valid_for_ms())); + deadline_ = request_context_.received_at + validity; + deadline_ = std::min(deadline_, request_context_.deadline); + + if (!command_id_.empty() && method_policy_.mutating) { + payload_hash_ = deterministicGrpcPayloadHash( + method_policy_.full_method_name, request); + if (payload_hash_.empty()) { + rejectBeforeDispatch_( + safety::SafetyReason::InternalError, + "request payload could not be serialized deterministically", + statusForReason( + safety::SafetyReason::InternalError, + "request payload could not be serialized deterministically"), + false); + return; + } + const safety::CommandKey key{ + request_context_.principal.id.empty() + ? std::string("anonymous") + : request_context_.principal.id, + command_id_}; + auto reservation = coordinator_->commandLedger().reserve( + key, payload_hash_); + switch (reservation.status) { + case safety::CommandReservationStatus::AcceptedNew: + ledger_ticket_ = std::move(reservation.ticket); + owns_ledger_record_ = true; + break; + case safety::CommandReservationStatus::JoinedInFlight: { + const auto outcome = coordinator_->commandLedger().wait( + reservation.ticket, deadline_); + if (!outcome.has_value()) { + rejectBeforeDispatch_( + safety::SafetyReason::DeadlineExceededBeforeDispatch, + "timed out waiting for the in-flight command result", + statusForReason( + safety::SafetyReason::DeadlineExceededBeforeDispatch, + "timed out waiting for the in-flight command result"), + false); + return; + } + if (!restoreOutcome_(*outcome)) { + rejectBeforeDispatch_( + safety::SafetyReason::InternalError, + "cached command response could not be decoded", + statusForReason( + safety::SafetyReason::InternalError, + "cached command response could not be decoded"), + false); + return; + } + status_ = grpc::Status::OK; + completed_ = true; + return; + } + case safety::CommandReservationStatus::CachedResult: + if (!reservation.cached_outcome.has_value() || + !restoreOutcome_(*reservation.cached_outcome)) { + rejectBeforeDispatch_( + safety::SafetyReason::InternalError, + "cached command response could not be decoded", + statusForReason( + safety::SafetyReason::InternalError, + "cached command response could not be decoded"), + false); + return; + } + status_ = grpc::Status::OK; + completed_ = true; + return; + case safety::CommandReservationStatus::CommandIdConflict: + rejectBeforeDispatch_( + safety::SafetyReason::CommandIdConflict, + "command_id is already associated with a different semantic payload", + statusForReason( + safety::SafetyReason::CommandIdConflict, + "command_id is already associated with a different semantic payload"), + false); + return; + case safety::CommandReservationStatus::ResultEvicted: + rejectBeforeDispatch_( + safety::SafetyReason::ResultEvicted, + "command result was evicted and the command will not be dispatched again", + statusForReason( + safety::SafetyReason::ResultEvicted, + "command result was evicted and the command will not be dispatched again"), + false); + return; + case safety::CommandReservationStatus::LedgerExhausted: + rejectBeforeDispatch_( + safety::SafetyReason::LedgerExhausted, + "command ledger capacity is exhausted", + statusForReason( + safety::SafetyReason::LedgerExhausted, + "command ledger capacity is exhausted"), + false); + return; + case safety::CommandReservationStatus::Invalid: + rejectBeforeDispatch_( + safety::SafetyReason::InvalidArgument, + "command_id or effective principal is invalid", + statusForReason( + safety::SafetyReason::InvalidArgument, + "command_id or effective principal is invalid"), + false); + return; + } + } + + safety::AdmissionRequest admission; + admission.command = method_policy_.commandDescriptor(); + admission.actor.principal_id = request_context_.principal.id; + admission.actor.authenticated = request_context_.principal.authenticated; + for (const auto role : request_context_.principal.roles) { + admission.actor.roles.emplace_back(toString(role)); + } + admission.command_id = command_id_.empty() + ? request_context_.correlation_id + : command_id_; + admission.device_id = device_id_; + if (header->has_expected_device_generation()) { + admission.expected_device_generation = + header->expected_device_generation(); + } + admission.deadline = deadline_; + + auto admitted = coordinator_->admit(admission); + admission_decision_ = admitted.decision; + if (!admitted.permit.has_value()) { + const auto reason = admitted.decision.reason == safety::SafetyReason::None + ? safety::SafetyReason::InternalError + : admitted.decision.reason; + rejectBeforeDispatch_( + reason, + admitted.decision.detail, + owns_ledger_record_ + ? grpc::Status::OK + : statusForReason(reason, admitted.decision.detail), + owns_ledger_record_); + return; + } + permit_ = std::move(admitted.permit); + if (owns_ledger_record_) { + (void)coordinator_->commandLedger().setLifecycle( + ledger_ticket_, + safety::CommandLifecycle::Admitted, + admission_decision_.safety_epoch, + admission_decision_.device_generation); + } + should_execute_ = true; + status_ = grpc::Status::OK; +} + +bool GrpcCommandTransaction::beginDispatch() +{ + if (dispatch_guard_.has_value() && dispatch_guard_->acquired()) { + return true; + } + auto guard = beginScopedDispatch(); + if (!guard.acquired()) { + return false; + } + dispatch_guard_.emplace(std::move(guard)); + return true; +} + +bool GrpcCommandTransaction::revalidate() +{ + if (!should_execute_ || completed_ || !permit_.has_value() || + !coordinator_) { + dispatch_status_ = statusForReason( + safety::SafetyReason::InternalError, + "command transaction is not revalidatable"); + return false; + } + const auto check = coordinator_->revalidatePermit(*permit_); + if (!check.safe) { + const auto reason = check.reason == safety::SafetyReason::None + ? safety::SafetyReason::SafetyLatched + : check.reason; + dispatch_status_ = statusForReason(reason, check.detail); + populateFeedback_( + false, + reason, + dispatch_started_ ? safety::CommandLifecycle::Failed + : safety::CommandLifecycle::RejectedBeforeDispatch, + check.detail); + return false; + } + dispatch_status_ = grpc::Status::OK; + return true; +} + +safety::DispatchGuard GrpcCommandTransaction::beginScopedDispatch() +{ + if (!should_execute_ || completed_ || !permit_.has_value() || + !coordinator_) { + dispatch_status_ = statusForReason( + safety::SafetyReason::InternalError, + "command transaction is not dispatchable"); + return {}; + } + auto guard = coordinator_->beginDispatch(*permit_); + if (!guard.acquired()) { + auto hardware = guard.hardwareCheck(); + if (hardware.reason == safety::SafetyReason::None) { + hardware.reason = safety::SafetyReason::SafetyLatched; + } + dispatch_status_ = statusForReason(hardware.reason, hardware.detail); + populateFeedback_( + false, + hardware.reason, + dispatch_started_ ? safety::CommandLifecycle::Failed + : safety::CommandLifecycle::RejectedBeforeDispatch, + hardware.detail); + return guard; + } + const bool first_dispatch = !dispatch_started_; + dispatch_started_ = true; + if (first_dispatch && owns_ledger_record_) { + (void)coordinator_->commandLedger().setLifecycle( + ledger_ticket_, + safety::CommandLifecycle::Dispatching, + admission_decision_.safety_epoch, + admission_decision_.device_generation, + true); + } + dispatch_status_ = grpc::Status::OK; + return guard; +} + +safety::DispatchGuard GrpcCommandTransaction::beginSafetyStopDispatch() +{ + if (!should_execute_ || completed_ || !coordinator_) { + dispatch_status_ = statusForReason( + safety::SafetyReason::InternalError, + "command transaction cannot dispatch an internal safety stop"); + return {}; + } + + safety::AdmissionRequest stop; + stop.command = { + method_policy_.full_method_name + "#internal-stop", + safety::CommandIntent::Stop, + method_policy_.policy_family, + true, + true}; + stop.actor.principal_id = request_context_.principal.id; + stop.actor.authenticated = request_context_.principal.authenticated; + for (const auto role : request_context_.principal.roles) { + stop.actor.roles.emplace_back(toString(role)); + } + const auto base_id = command_id_.empty() + ? request_context_.correlation_id + : command_id_; + stop.command_id = base_id + ":internal-stop:" + + std::to_string(++safety_stop_sequence_); + stop.device_id = device_id_; + stop.deadline = safety::SafetyClock::time_point::max(); + + auto admitted = coordinator_->admit(stop); + if (!admitted.permit.has_value()) { + const auto reason = admitted.decision.reason == safety::SafetyReason::None + ? safety::SafetyReason::SafetyLatched + : admitted.decision.reason; + dispatch_status_ = statusForReason(reason, admitted.decision.detail); + return {}; + } + auto guard = coordinator_->beginDispatch(*admitted.permit); + if (!guard.acquired()) { + const auto& hardware = guard.hardwareCheck(); + const auto reason = hardware.reason == safety::SafetyReason::None + ? safety::SafetyReason::StopUnconfirmed + : hardware.reason; + dispatch_status_ = statusForReason(reason, hardware.detail); + return guard; + } + dispatch_status_ = grpc::Status::OK; + return guard; +} + +grpc::Status GrpcCommandTransaction::finish( + grpc::Status operation_status, + safety::SafetyReason reason, + std::optional lifecycle) +{ + if (completed_) { + return status_; + } + auto* feedback = response_ ? findFeedbackHeader(*response_) : nullptr; + const bool success = feedback && feedback->success() && + operation_status.ok(); + if (reason == safety::SafetyReason::None && !success) { + if (feedback && feedback->reason_code() != + api::COMMAND_REASON_CODE_UNSPECIFIED && + feedback->reason_code() != api::COMMAND_REASON_CODE_NONE) { + reason = fromApiSafetyReason(feedback->reason_code()); + } else { + reason = reasonForStatus(operation_status); + } + } + if (success) { + reason = safety::SafetyReason::None; + } + const bool stop_unconfirmed = + !success && dispatch_started_ && + method_policy_.command_intent == safety::CommandIntent::Stop; + if (stop_unconfirmed) { + reason = safety::SafetyReason::StopUnconfirmed; + if (coordinator_) { + coordinator_->quarantineDevice( + device_id_, reason, command_id_); + } + } + const auto terminal_lifecycle = lifecycle.value_or( + success + ? safety::CommandLifecycle::Completed + : dispatch_started_ ? safety::CommandLifecycle::Failed + : safety::CommandLifecycle::RejectedBeforeDispatch); + const std::string detail = success + ? std::string{} + : feedback && !feedback->error_message().empty() + ? feedback->error_message() + : operation_status.error_message(); + populateFeedback_(success, reason, terminal_lifecycle, detail); + + dispatch_guard_.reset(); + const bool ledger_backed = owns_ledger_record_; + if (ledger_backed && + !completeLedger_( + terminal_lifecycle, reason, detail, dispatch_started_)) { + status_ = statusForReason( + safety::SafetyReason::InternalError, + "command ledger could not store the terminal result"); + completed_ = true; + should_execute_ = false; + return status_; + } + completed_ = true; + should_execute_ = false; + status_ = ledger_backed ? grpc::Status::OK + : std::move(operation_status); + return status_; +} + +grpc::Status GrpcCommandTransaction::finishException( + std::string detail) noexcept +{ + try { + const bool stop_failed = + dispatch_started_ && + method_policy_.command_intent == safety::CommandIntent::Stop; + const bool outcome_unknown = + dispatch_started_ && !stop_failed && + method_policy_.command_intent != + safety::CommandIntent::ResetFault; + const auto reason = stop_failed + ? safety::SafetyReason::StopUnconfirmed + : outcome_unknown ? safety::SafetyReason::OutcomeUnknown + : safety::SafetyReason::InternalError; + const auto lifecycle = outcome_unknown + ? safety::CommandLifecycle::OutcomeUnknown + : dispatch_started_ ? safety::CommandLifecycle::Failed + : safety::CommandLifecycle::RejectedBeforeDispatch; + if ((outcome_unknown || stop_failed) && coordinator_) { + coordinator_->quarantineDevice( + device_id_, reason, command_id_); + } + populateFeedback_(false, reason, lifecycle, detail); + dispatch_guard_.reset(); + const bool ledger_backed = owns_ledger_record_; + if (ledger_backed) { + (void)completeLedger_( + lifecycle, reason, detail, dispatch_started_); + } + completed_ = true; + should_execute_ = false; + status_ = ledger_backed + ? grpc::Status::OK + : statusForReason(reason, detail); + return status_; + } catch (...) { + completed_ = true; + should_execute_ = false; + status_ = grpc::Status( + grpc::StatusCode::INTERNAL, + "command transaction failed while handling an exception"); + return status_; + } +} + +void GrpcCommandTransaction::rejectBeforeDispatch_( + const safety::SafetyReason reason, + std::string detail, + grpc::Status status, + const bool complete_reserved_record) +{ + populateFeedback_( + false, + reason, + safety::CommandLifecycle::RejectedBeforeDispatch, + detail); + if (complete_reserved_record && owns_ledger_record_) { + if (!completeLedger_( + safety::CommandLifecycle::RejectedBeforeDispatch, + reason, + detail, + false)) { + status = statusForReason( + safety::SafetyReason::InternalError, + "command ledger could not store the rejected result"); + } else { + status = grpc::Status::OK; + } + } + status_ = std::move(status); + should_execute_ = false; + completed_ = true; +} + +bool GrpcCommandTransaction::restoreOutcome_( + const safety::CommandOutcome& outcome) +{ + if (!response_) { + return false; + } + if (!outcome.serialized_response.empty()) { + response_->Clear(); + if (!response_->ParseFromString(outcome.serialized_response)) { + return false; + } + } + const bool success = + outcome.lifecycle == safety::CommandLifecycle::Completed; + populateFeedback_( + success, + outcome.reason, + outcome.lifecycle, + outcome.detail); + return true; +} + +bool GrpcCommandTransaction::completeLedger_( + const safety::CommandLifecycle lifecycle, + const safety::SafetyReason reason, + const std::string& detail, + const bool hardware_submission_possible) noexcept +{ + try { + if (!owns_ledger_record_ || !coordinator_ || !response_) { + return !owns_ledger_record_; + } + std::string serialized; + if (!deterministicSerialize(*response_, serialized)) { + return false; + } + safety::CommandOutcome outcome; + outcome.lifecycle = lifecycle; + outcome.reason = reason; + outcome.detail = detail; + outcome.serialized_response = std::move(serialized); + outcome.safety_epoch = admission_decision_.safety_epoch; + outcome.device_generation = admission_decision_.device_generation; + outcome.hardware_submission_possible = hardware_submission_possible; + const bool completed = coordinator_->commandLedger().complete( + ledger_ticket_, std::move(outcome)); + if (completed) { + owns_ledger_record_ = false; + } + return completed; + } catch (...) { + return false; + } +} + +void GrpcCommandTransaction::populateFeedback_( + const bool success, + const safety::SafetyReason reason, + const safety::CommandLifecycle lifecycle, + const std::string& detail) +{ + if (!response_) { + return; + } + auto* feedback = findFeedbackHeader(*response_); + if (!feedback) { + return; + } + feedback->set_success(success); + if (!success && feedback->error_message().empty()) { + feedback->set_error_message( + detail.empty() ? safety::toString(reason) : detail); + } + if (success) { + feedback->clear_error_message(); + } + if (!feedback->has_timestamp()) { + *feedback->mutable_timestamp() = + google::protobuf::util::TimeUtil::GetCurrentTime(); + } + feedback->set_reason_code(toApiSafetyReason(reason)); + feedback->set_command_id(command_id_); + if (coordinator_) { + feedback->set_service_instance_id(coordinator_->serviceInstanceId()); + } + feedback->set_safety_epoch(admission_decision_.safety_epoch); + feedback->set_device_generation( + admission_decision_.device_generation); + feedback->set_execution_state(toApiLifecycle(lifecycle)); +} + +void GrpcCommandTransaction::abandon_() noexcept +{ + if (completed_ || !owns_ledger_record_) { + return; + } + (void)finishException( + dispatch_started_ + ? "command handler exited after crossing the hardware dispatch fence without recording a terminal result" + : "command handler exited without recording a terminal result"); +} + +grpc::Status executeRegisteredGrpcCommand( + const std::shared_ptr& gateway, + grpc::ServerContext* server_context, + safety::SafetyCoordinator& coordinator, + const std::string& full_method_name, + const google::protobuf::Message* request, + google::protobuf::Message* response, + GrpcUnaryCommandOperation operation) +{ + const auto call = beginRegisteredGrpcCall( + gateway, server_context, full_method_name); + if (!call.allowed()) { + return call.status(); + } + if (!request || !response || !operation) { + return grpc::Status( + grpc::StatusCode::INTERNAL, + "gRPC command transaction received a null argument"); + } + const auto policy = + defaultGrpcMethodPolicyRegistry().find(full_method_name); + if (!policy.has_value() || !policy->mutating) { + return grpc::Status( + grpc::StatusCode::INTERNAL, + "gRPC command transaction requires a registered mutating method"); + } + + GrpcCommandTransaction transaction( + coordinator, call.context(), *policy, *request, *response); + if (!transaction.shouldExecute()) { + return transaction.status(); + } + try { + return transaction.finish(operation(transaction)); + } catch (const std::exception& error) { + return transaction.finishException(error.what()); + } catch (...) { + return transaction.finishException( + "command handler threw an unknown exception"); + } +} + +grpc::Status executeServerDerivedGrpcCommand( + const std::shared_ptr& gateway, + grpc::ServerContext* server_context, + safety::SafetyCoordinator& coordinator, + const std::string& full_method_name, + GrpcMethodPolicy effective_policy, + const google::protobuf::Message* request, + google::protobuf::Message* response, + GrpcUnaryCommandOperation operation) +{ + const auto call = beginRegisteredGrpcCall( + gateway, server_context, full_method_name); + if (!call.allowed()) { + return call.status(); + } + if (!request || !response || !operation) { + return grpc::Status( + grpc::StatusCode::INTERNAL, + "server-derived gRPC command received a null argument"); + } + const auto registered = + defaultGrpcMethodPolicyRegistry().find(full_method_name); + if (!registered.has_value() || !registered->mutating || + effective_policy.full_method_name != full_method_name || + !effective_policy.mutating || + effective_policy.policy_family != registered->policy_family) { + return grpc::Status( + grpc::StatusCode::INTERNAL, + "server-derived gRPC command policy is inconsistent with the registered method"); + } + const bool valid_safety_lane = + !effective_policy.safety_lane || + effective_policy.command_intent == safety::CommandIntent::Stop || + effective_policy.command_intent == safety::CommandIntent::ResetFault || + effective_policy.command_intent == + safety::CommandIntent::RecoverAdmission; + if (!valid_safety_lane) { + return grpc::Status( + grpc::StatusCode::INTERNAL, + "server-derived gRPC command has an invalid safety lane"); + } + + GrpcCommandTransaction transaction( + coordinator, + call.context(), + std::move(effective_policy), + *request, + *response); + if (!transaction.shouldExecute()) { + return transaction.status(); + } + try { + return transaction.finish(operation(transaction)); + } catch (const std::exception& error) { + return transaction.finishException(error.what()); + } catch (...) { + return transaction.finishException( + "server-derived command handler threw an unknown exception"); + } +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp b/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp index d0fcecc1..f2b2f2f1 100644 --- a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp @@ -16,7 +16,9 @@ #include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h" #include "manager/control_authority/include/control_authority_manager.h" +#include "service/grpc/include/grpc_command_transaction.h" #include "service/grpc/include/media_activity_coordinator.h" +#include "service/grpc/include/grpc_security.h" #include "service/stop_all/include/stop_all_admission_gate.h" using namespace std; @@ -227,12 +229,16 @@ bool dispatchDexHandCommand( const std::string& device_id, const std::shared_ptr& dev, ScopedDexHandControlLease& lease, + GrpcCommandTransaction& command, Operation&& operation) { auto dispatch = lease.tryBeginDispatch(); if (!dispatch.acquired()) { (void)failControlDispatch(response, device_id, lease); return false; } + if (!command.beginDispatch()) { + return false; + } if (!dev->resumeOperationalActivity()) { (void)failResponse( response, @@ -304,10 +310,20 @@ void maybeConfigureRh56FullTactilePolling(const std::shared_ptr } // namespace -gRPCDexHandServiceImpl::gRPCDexHandServiceImpl(): dmgr_(DeviceManager::getInstance()) {} +gRPCDexHandServiceImpl::gRPCDexHandServiceImpl() + : gRPCDexHandServiceImpl(makeDefaultGrpcSecurityGateway()) {} + +gRPCDexHandServiceImpl::gRPCDexHandServiceImpl( + std::shared_ptr security_gateway) + : dmgr_(DeviceManager::getInstance()), + security_gateway_(security_gateway + ? std::move(security_gateway) + : makeDefaultGrpcSecurityGateway()) {} grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context, const api::GetDexHandStateCommand_Request* request, api::GetDexHandStateCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.DexHandService/GetStatus"); try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (GetStatus): id=" << dev_id; @@ -350,7 +366,10 @@ grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context, grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context , const cmvr::api::SetDexHandPositionsCommand_Request* request , cmvr::api::SetDexHandPositionsCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.DexHandService/SetDexHandPos", request, response, + [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPos): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); @@ -378,27 +397,27 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context dev_id, dev, control_lease, + command, [&] { dev->setPositions(finger_joint_targets); })) { - return grpc::Status::OK; + return command.dispatchStatus().ok() + ? grpc::Status::OK + : command.dispatchStatus(); } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandPos): success, id=" << dev_id << ", values=" << request->values_size(); return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* context , const cmvr::api::SetDexHandAnglesCommand_Request* request , cmvr::api::SetDexHandAnglesCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.DexHandService/SetDexHandAngle", request, response, + [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandAngle): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); @@ -421,8 +440,11 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* contex dev_id, dev, control_lease, + command, [&] { rh56->setAngles(finger_joint_targets); })) { - return grpc::Status::OK; + return command.dispatchStatus().ok() + ? grpc::Status::OK + : command.dispatchStatus(); } } else { std::vector finger_joint_targets(static_cast(kDexHandDofCount), -1); @@ -435,8 +457,11 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* contex dev_id, dev, control_lease, + command, [&] { dev->setAngles(finger_joint_targets); })) { - return grpc::Status::OK; + return command.dispatchStatus().ok() + ? grpc::Status::OK + : command.dispatchStatus(); } } @@ -445,19 +470,16 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* contex CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandAngle): success, id=" << dev_id << ", values=" << request->values_size(); return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* context , const cmvr::api::SetDexHandForceCommand_Request* request , cmvr::api::SetDexHandForceCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.DexHandService/SetDexHandForce", request, response, + [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandForce): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); @@ -485,27 +507,27 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* contex dev_id, dev, control_lease, + command, [&] { dev->setForce(finger_joint_targets); })) { - return grpc::Status::OK; + return command.dispatchStatus().ok() + ? grpc::Status::OK + : command.dispatchStatus(); } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandForce): success, id=" << dev_id << ", values=" << request->values_size(); return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* context , const cmvr::api::SetDexHandSpeedCommand_Request* request , cmvr::api::SetDexHandSpeedCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.DexHandService/SetDexHandSpeed", request, response, + [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandSpeed): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); @@ -533,27 +555,27 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* contex dev_id, dev, control_lease, + command, [&] { dev->setVelocities(finger_joint_targets); })) { - return grpc::Status::OK; + return command.dispatchStatus().ok() + ? grpc::Status::OK + : command.dispatchStatus(); } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandSpeed): success, id=" << dev_id << ", values=" << request->values_size(); return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCDexHandServiceImpl::SetDexHandPresetAct(grpc::ServerContext* context , const cmvr::api::SetDexHandPresetActCommand_Request* request , cmvr::api::SetDexHandPresetActCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.DexHandService/SetDexHandPresetAct", request, response, + [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPresetAct): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); @@ -578,26 +600,26 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPresetAct(grpc::ServerContext* co dev_id, dev, control_lease, + command, [&] { dev->setPresetAct(presetActId); })) { - return grpc::Status::OK; + return command.dispatchStatus().ok() + ? grpc::Status::OK + : command.dispatchStatus(); } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandPresetAct): success, id=" << dev_id << ", preset_act_id=" << presetActId; return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context , const cmvr::api::GetSensorDataCommand_Request* request , cmvr::api::GetSensorDataCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.DexHandService/GetSensorData"); auto media_session = globalMediaActivityCoordinator().beginSession(); if (!media_session) { @@ -652,6 +674,9 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* context , grpc::ServerReaderWriter* stream) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.DexHandService/GetSensorDataStream"); auto media_session = globalMediaActivityCoordinator().beginSession( [context] { if (context) { diff --git a/cmvr-es/service/grpc/src/grpc_head_service.cpp b/cmvr-es/service/grpc/src/grpc_head_service.cpp index 8e6934e7..aa541d67 100644 --- a/cmvr-es/service/grpc/src/grpc_head_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_head_service.cpp @@ -5,12 +5,15 @@ #include "manager/device_manager/include/device_manager.h" #include "common/base/grpc_utils.h" #include "biohead/biohead_esp32/include/biohead_esp32.h" +#include "service/grpc/include/grpc_command_transaction.h" #include "service/grpc/include/media_activity_coordinator.h" +#include "service/grpc/include/grpc_security.h" #include "service/stop_all/include/stop_all_admission_gate.h" #include #include #include #include +#include using namespace std; using namespace cmvr::service; @@ -49,10 +52,61 @@ grpc::Status failStoppedCommand(ResponseT* response) "Biohead command was rejected because StopAll is in progress or " "the command was preempted"); } + +template +grpc::Status executeHeadOperationalCommand( + const std::shared_ptr& security_gateway, + grpc::ServerContext* context, + DeviceManager& device_manager, + const char* full_method_name, + const char* rpc_name, + const RequestT* request, + ResponseT* response, + Operation&& operation) +{ + return executeRegisteredGrpcCommand( + security_gateway, context, device_manager.safetyCoordinator(), + full_method_name, request, response, + [&device_manager, request, response, rpc_name, + operation = std::forward(operation)]( + GrpcCommandTransaction& command) mutable { + const std::string device_id = request->header().device_id(); + const auto robot = + device_manager.getDevice(device_id); + if (!robot) { + return failResponse( + response, "Biohead device not found: " + device_id); + } + + const auto activity = admitHeadCommand(robot); + if (!activity) { + return failStoppedCommand(response); + } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } + if (!operation(*robot, *activity)) { + return failStoppedCommand(response); + } + + response->mutable_header()->set_success(true); + setCurrentTimestamp( + response->mutable_header()->mutable_timestamp()); + logSuccess(rpc_name, device_id); + return grpc::Status::OK; + }); +} } gRPCMBioHeadServiceImpl::gRPCMBioHeadServiceImpl() - : dmgr_(DeviceManager::getInstance()) {} + : gRPCMBioHeadServiceImpl(makeDefaultGrpcSecurityGateway()) {} + +gRPCMBioHeadServiceImpl::gRPCMBioHeadServiceImpl( + std::shared_ptr security_gateway) + : dmgr_(DeviceManager::getInstance()), + security_gateway_(security_gateway + ? std::move(security_gateway) + : makeDefaultGrpcSecurityGateway()) {} // 设置表情（一次性） @@ -60,57 +114,48 @@ grpc::Status gRPCMBioHeadServiceImpl::SetExpression( grpc::ServerContext* context, const SetFacialExpression_Request* request, SetFacialExpression_Feedback* response) { - - try { - std::string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - const auto activity = admitHeadCommand(robot); - if (!activity) { - return failStoppedCommand(response); - } - - FacialExpressionState expression_state; - - expression_state.left_eyebrow_outside_y = request->expression().eyebrow().left_outside_y(); - expression_state.left_eyebrow_inside_y = request->expression().eyebrow().left_inside_y(); - expression_state.right_eyebrow_outside_y = request->expression().eyebrow().right_outside_y(); - expression_state.right_eyebrow_inside_y = request->expression().eyebrow().right_inside_y(); - - expression_state.left_eye_upper_lid_y = request->expression().eyelid().left_upper_y(); - expression_state.left_eye_lower_lid_y = request->expression().eyelid().left_lower_y(); - expression_state.right_eye_upper_lid_y = request->expression().eyelid().right_upper_y(); - expression_state.right_eye_lower_lid_y = request->expression().eyelid().right_lower_y(); - - expression_state.left_eye_ball_x = request->expression().eyeball().left_x(); - expression_state.left_eye_ball_y = request->expression().eyeball().left_y(); - expression_state.right_eye_ball_x = request->expression().eyeball().right_x(); - expression_state.right_eye_ball_y = request->expression().eyeball().right_y(); - - expression_state.left_nose_y = request->expression().nose().left_y(); - expression_state.right_nose_y = request->expression().nose().right_y(); - - expression_state.upper_lip_y = request->expression().mouth().upper_lip_y(); - expression_state.lower_lip_y = request->expression().mouth().lower_lip_y(); - - if (!robot->setExpressionPoseIfCurrent( - *activity, expression_state)) { - return failStoppedCommand(response); - } - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("SetExpression", dev_id); - return grpc::Status::OK; - } catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + return executeHeadOperationalCommand( + security_gateway_, context, dmgr_, + "/cmvr.api.BioHeadService/SetExpression", "SetExpression", + request, response, + [request](AbstractBiohead& robot, + const AbstractBiohead::OperationalToken activity) { + FacialExpressionState expression_state; + expression_state.left_eyebrow_outside_y = + request->expression().eyebrow().left_outside_y(); + expression_state.left_eyebrow_inside_y = + request->expression().eyebrow().left_inside_y(); + expression_state.right_eyebrow_outside_y = + request->expression().eyebrow().right_outside_y(); + expression_state.right_eyebrow_inside_y = + request->expression().eyebrow().right_inside_y(); + expression_state.left_eye_upper_lid_y = + request->expression().eyelid().left_upper_y(); + expression_state.left_eye_lower_lid_y = + request->expression().eyelid().left_lower_y(); + expression_state.right_eye_upper_lid_y = + request->expression().eyelid().right_upper_y(); + expression_state.right_eye_lower_lid_y = + request->expression().eyelid().right_lower_y(); + expression_state.left_eye_ball_x = + request->expression().eyeball().left_x(); + expression_state.left_eye_ball_y = + request->expression().eyeball().left_y(); + expression_state.right_eye_ball_x = + request->expression().eyeball().right_x(); + expression_state.right_eye_ball_y = + request->expression().eyeball().right_y(); + expression_state.left_nose_y = + request->expression().nose().left_y(); + expression_state.right_nose_y = + request->expression().nose().right_y(); + expression_state.upper_lip_y = + request->expression().mouth().upper_lip_y(); + expression_state.lower_lip_y = + request->expression().mouth().lower_lip_y(); + return robot.setExpressionPoseIfCurrent( + activity, expression_state); + }); } @@ -119,11 +164,15 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression( grpc::ServerContext* context, grpc::ServerReaderWriter* stream) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.BioHeadService/StreamExpression"); StreamFacialExpression_Feedback feedback_msg; std::string dev_id; std::shared_ptr robot; bool first_message = true; AbstractBiohead::OperationalToken activity{0U}; + std::optional safety_session; auto media_session = globalMediaActivityCoordinator().beginSession( [context] { if (context) { @@ -178,8 +227,44 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression( } activity = *admitted; + const auto& header = request_msg.header(); + GrpcStreamingSafetyOpen safety_open; + safety_open.full_method_name = + "/cmvr.api.BioHeadService/StreamExpression"; + safety_open.device_id = dev_id; + safety_open.session_id = header.command_id().empty() + ? cmvr_grpc_call_guard.context().correlation_id + : header.command_id(); + safety_open.expected_service_instance_id = + header.expected_service_instance_id(); + if (header.has_expected_device_generation()) { + safety_open.expected_device_generation = + header.expected_device_generation(); + } + safety_open.authority_generation = activity; + safety_open.deadline = + cmvr_grpc_call_guard.context().deadline; + safety_session.emplace( + dmgr_.safetyCoordinator(), + cmvr_grpc_call_guard.context(), + std::move(safety_open)); + if (!safety_session->admitted()) { + feedback_msg.mutable_header()->set_success(false); + feedback_msg.mutable_header()->set_error_message( + safety_session->status().error_message()); + setCurrentTimestamp( + feedback_msg.mutable_header()->mutable_timestamp()); + stream->Write(feedback_msg); + return safety_session->status(); + } + first_message = false; CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (StreamExpression): streaming success, id=" << dev_id; + } else if (!request_msg.header().device_id().empty() && + request_msg.header().device_id() != dev_id) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "biohead stream cannot change device_id after its first frame"); } // ✅ 如果紧急停止触发，直接退出 @@ -189,6 +274,21 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression( break; } + if (!safety_session || !safety_session->revalidate()) { + const auto status = safety_session + ? safety_session->status() + : grpc::Status( + grpc::StatusCode::INTERNAL, + "biohead stream safety session was not initialized"); + feedback_msg.mutable_header()->set_success(false); + feedback_msg.mutable_header()->set_error_message( + status.error_message()); + setCurrentTimestamp( + feedback_msg.mutable_header()->mutable_timestamp()); + stream->Write(feedback_msg); + return status; + } + auto current_time = std::chrono::steady_clock::now(); auto elapsed_time = std::chrono::duration_cast(current_time - last_control_time); if (elapsed_time < time_interval) continue; @@ -233,10 +333,25 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression( expression_state.jaw_y = request_msg.expr().jaw().y(); bool dispatched = false; + grpc::Status dispatch_status = grpc::Status::OK; const bool current_session = media_session.runIfCurrent([&] { + auto dispatch = safety_session->beginDispatch(); + if (!dispatch.acquired()) { + dispatch_status = safety_session->status(); + return; + } dispatched = robot->streamFacialPoseIfCurrent( activity, expression_state, 0, 0); }); + if (!dispatch_status.ok()) { + feedback_msg.mutable_header()->set_success(false); + feedback_msg.mutable_header()->set_error_message( + dispatch_status.error_message()); + setCurrentTimestamp( + feedback_msg.mutable_header()->mutable_timestamp()); + stream->Write(feedback_msg); + return dispatch_status; + } if (!current_session || !dispatched) { CMVR_LOG(WARNING) << "[gRPCMBioHeadServiceImpl] StreamExpression was " @@ -271,6 +386,9 @@ grpc::Status gRPCMBioHeadServiceImpl::GetSystemStatus( const GetStatus_Request* request, GetStatus_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.BioHeadService/GetSystemStatus"); try { string dev_id = request->header().device_id(); auto robot = dmgr_.getDevice(dev_id); @@ -298,259 +416,146 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop( const EmergencyStop_Request* request, EmergencyStop_Feedback* response) { - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - if (!robot->stopOperationalActivity()) { - return failResponse( - response, - "Biohead could not confirm that operational activity " - "stopped: " + dev_id); - } - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("EmergencyStop", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.BioHeadService/EmergencyStop", request, response, + [this, request, response](GrpcCommandTransaction& command) { + const string dev_id = request->header().device_id(); + const auto robot = dmgr_.getDevice(dev_id); + if (!robot) { + return failResponse( + response, "Biohead device not found: " + dev_id); + } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } + if (!robot->stopOperationalActivity()) { + return failResponse( + response, + "Biohead could not confirm that operational activity " + "stopped: " + dev_id); + } + response->mutable_header()->set_success(true); + setCurrentTimestamp( + response->mutable_header()->mutable_timestamp()); + logSuccess("EmergencyStop", dev_id); + return grpc::Status::OK; + }); } grpc::Status gRPCMBioHeadServiceImpl::SpeakStart(grpc::ServerContext* context, const cmvr::api::SpeakStart_Request* request, cmvr::api::SpeakStart_Feedback* response) { - - { - - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - - - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - const auto activity = admitHeadCommand(robot); - if (!activity || !robot->speakStartIfCurrent(*activity)) { - return failStoppedCommand(response); - } - - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("SpeakStart", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } - } + return executeHeadOperationalCommand( + security_gateway_, context, dmgr_, + "/cmvr.api.BioHeadService/SpeakStart", "SpeakStart", + request, response, + [](AbstractBiohead& robot, + const AbstractBiohead::OperationalToken activity) { + return robot.speakStartIfCurrent(activity); + }); } grpc::Status gRPCMBioHeadServiceImpl::SpeakStop(grpc::ServerContext* context, const cmvr::api::SpeakStop_Request* request, cmvr::api::SpeakStop_Feedback* response) { - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - robot->speakstop(); // 停止执行 - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("SpeakStop", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.BioHeadService/SpeakStop", request, response, + [this, request, response](GrpcCommandTransaction& command) { + const string dev_id = request->header().device_id(); + const auto robot = dmgr_.getDevice(dev_id); + if (!robot) { + return failResponse( + response, "Biohead device not found: " + dev_id); + } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } + robot->speakstop(); + response->mutable_header()->set_success(true); + setCurrentTimestamp( + response->mutable_header()->mutable_timestamp()); + logSuccess("SpeakStop", dev_id); + return grpc::Status::OK; + }); } grpc::Status gRPCMBioHeadServiceImpl::Happy(grpc::ServerContext* context, const cmvr::api::Happy_Request* request, cmvr::api::Happy_Feedback* response) { - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - const auto activity = admitHeadCommand(robot); - if (!activity || !robot->expressionHappyIfCurrent(*activity)) { - return failStoppedCommand(response); - } - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("Happy", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + return executeHeadOperationalCommand( + security_gateway_, context, dmgr_, + "/cmvr.api.BioHeadService/Happy", "Happy", request, response, + [](AbstractBiohead& robot, + const AbstractBiohead::OperationalToken activity) { + return robot.expressionHappyIfCurrent(activity); + }); } grpc::Status gRPCMBioHeadServiceImpl::Surprise(grpc::ServerContext* context, const cmvr::api::Surprise_Request* request, cmvr::api::Surprise_Feedback* response) { - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - const auto activity = admitHeadCommand(robot); - if (!activity || !robot->expressionSurprisedIfCurrent(*activity)) { - return failStoppedCommand(response); - } - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("Surprise", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + return executeHeadOperationalCommand( + security_gateway_, context, dmgr_, + "/cmvr.api.BioHeadService/Surprise", "Surprise", request, + response, + [](AbstractBiohead& robot, + const AbstractBiohead::OperationalToken activity) { + return robot.expressionSurprisedIfCurrent(activity); + }); } grpc::Status gRPCMBioHeadServiceImpl::ExpressionTired(grpc::ServerContext* context, const cmvr::api::ExpressionTired_Request* request, cmvr::api::ExpressionTired_Feedback* response) { - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - const auto activity = admitHeadCommand(robot); - if (!activity || !robot->expressionTiredIfCurrent(*activity)) { - return failStoppedCommand(response); - } - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("ExpressionTired", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + return executeHeadOperationalCommand( + security_gateway_, context, dmgr_, + "/cmvr.api.BioHeadService/ExpressionTired", "ExpressionTired", + request, response, + [](AbstractBiohead& robot, + const AbstractBiohead::OperationalToken activity) { + return robot.expressionTiredIfCurrent(activity); + }); } grpc::Status gRPCMBioHeadServiceImpl::ExpressionAngry(grpc::ServerContext* context, const cmvr::api::ExpressionAngry_Request* request, cmvr::api::ExpressionAngry_Feedback* response) { - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - const auto activity = admitHeadCommand(robot); - if (!activity || !robot->expressionAngryIfCurrent(*activity)) { - return failStoppedCommand(response); - } - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("ExpressionAngry", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + return executeHeadOperationalCommand( + security_gateway_, context, dmgr_, + "/cmvr.api.BioHeadService/ExpressionAngry", "ExpressionAngry", + request, response, + [](AbstractBiohead& robot, + const AbstractBiohead::OperationalToken activity) { + return robot.expressionAngryIfCurrent(activity); + }); } grpc::Status gRPCMBioHeadServiceImpl::ExpressionSadness(grpc::ServerContext* context, const cmvr::api::ExpressionSadness_Request* request, cmvr::api::ExpressionSadness_Feedback* response) { - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - const auto activity = admitHeadCommand(robot); - if (!activity || !robot->expressionSadnessIfCurrent(*activity)) { - return failStoppedCommand(response); - } - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("ExpressionSadness", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + return executeHeadOperationalCommand( + security_gateway_, context, dmgr_, + "/cmvr.api.BioHeadService/ExpressionSadness", + "ExpressionSadness", request, response, + [](AbstractBiohead& robot, + const AbstractBiohead::OperationalToken activity) { + return robot.expressionSadnessIfCurrent(activity); + }); } grpc::Status gRPCMBioHeadServiceImpl::ExpressionYawn(grpc::ServerContext* context, const cmvr::api::ExpressionYawn_Request* request, cmvr::api::ExpressionYawn_Feedback* response) { - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - const auto activity = admitHeadCommand(robot); - if (!activity || !robot->expressionYawnIfCurrent(*activity)) { - return failStoppedCommand(response); - } - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("ExpressionYawn", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + return executeHeadOperationalCommand( + security_gateway_, context, dmgr_, + "/cmvr.api.BioHeadService/ExpressionYawn", "ExpressionYawn", + request, response, + [](AbstractBiohead& robot, + const AbstractBiohead::OperationalToken activity) { + return robot.expressionYawnIfCurrent(activity); + }); } diff --git a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp index b04997f6..7051d4bf 100644 --- a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp @@ -8,11 +8,14 @@ #include #include #include +#include #include #include "common/base/logging/logger.h" #include "manager/task_manager/include/task_manager.h" +#include "service/grpc/include/grpc_command_transaction.h" +#include "service/grpc/include/grpc_security.h" #include "service/stop_all/include/stop_all_admission_gate.h" #include "task/touch_screen_task/include/touch_screen_task.h" @@ -41,17 +44,59 @@ void fillTouchResponse(Touch_Response* response, } // namespace -gRPCHlcServiceImpl::gRPCHlcServiceImpl() = default; +gRPCHlcServiceImpl::gRPCHlcServiceImpl() + : gRPCHlcServiceImpl(makeDefaultGrpcSecurityGateway()) {} -grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) { - try { - auto touch_task = task::TaskManager::getInstance().getTouchScreenTask(); - if (!touch_task) { - const std::string error = "TouchScreenTask not found or not initialized"; - fillTouchResponse(response, false, error); - return grpc::Status(grpc::StatusCode::NOT_FOUND, error); - } +gRPCHlcServiceImpl::gRPCHlcServiceImpl( + std::shared_ptr security_gateway) + : dmgr_(device::DeviceManager::getInstance()), + security_gateway_(security_gateway + ? std::move(security_gateway) + : makeDefaultGrpcSecurityGateway()) {} +grpc::Status gRPCHlcServiceImpl::touch( + grpc::ServerContext* context, + const cmvr::api::Touch_Request* request, + cmvr::api::Touch_Response* response) +{ + if (!request || !response) { + return grpc::Status( + grpc::StatusCode::INTERNAL, + "touch received a null request or response"); + } + auto touch_task = task::TaskManager::getInstance().getTouchScreenTask(); + if (!touch_task) { + const std::string error = + "TouchScreenTask not found or not initialized"; + fillTouchResponse(response, false, error); + return grpc::Status(grpc::StatusCode::NOT_FOUND, error); + } + const auto arm_id = touch_task->controlDeviceId(); + if (arm_id.empty()) { + const std::string error = + "TouchScreenTask has no resolved arm safety target"; + fillTouchResponse(response, false, error); + return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, error); + } + + // HLC is a composite command. The server-resolved arm, rather than a + // caller-supplied task alias, is the authority and safety target. + api::Touch_Request normalized_request = *request; + if (!request->header().device_id().empty() && + request->header().device_id() != arm_id && + request->header().device_id() != touch_task->id()) { + CMVR_LOG(WARNING) + << "[gRPCHlcServiceImpl] ignoring composite touch target alias '" + << request->header().device_id() << "'; resolved arm=" << arm_id; + } + normalized_request.mutable_header()->set_device_id(arm_id); + + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.HlcService/touch", &normalized_request, response, + [context, request, response, touch_task]( + GrpcCommandTransaction& command) { + try { auto& admission_gate = globalStopAllAdmissionGate(); std::uint64_t admission_generation = 0U; { @@ -65,24 +110,61 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr: admission_generation = admission.generation(); } + task::TouchScreenTask::SafetyHooks safety_hooks; + safety_hooks.revalidate = [&command] { + return command.revalidate(); + }; + safety_hooks.dispatch_actuation = [&command]( + const task::TouchScreenTask::SafetyHooks::HardwareOperation& + operation) { + auto dispatch = command.beginScopedDispatch(); + return dispatch.acquired() && operation(); + }; + safety_hooks.dispatch_stop = [&command]( + const task::TouchScreenTask::SafetyHooks::HardwareOperation& + operation) { + auto dispatch = command.beginSafetyStopDispatch(); + return dispatch.acquired() && operation(); + }; + if (!touch_task->touchIfCurrent( request->u(), request->v(), - [&admission_gate, admission_generation] { + [&admission_gate, admission_generation, &command] { auto admission = admission_gate.lockAdmission(); return admission.accepting() && - admission.generation() == admission_generation; - })) { + admission.generation() == admission_generation && + command.revalidate(); + }, + std::move(safety_hooks))) { const std::string error = buildTouchFailureMessage(*touch_task, "TouchScreenTask touch request rejected"); fillTouchResponse(response, false, error); - return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, error); + return command.dispatchStatus().ok() + ? grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, error) + : command.dispatchStatus(); } while (touch_task->isBusy()) { if (context != nullptr && context->IsCancelled()) { + const bool stopped = touch_task->stopActivity(); const std::string error = "touch request cancelled"; fillTouchResponse(response, false, error); - return grpc::Status(grpc::StatusCode::CANCELLED, error); + return grpc::Status( + stopped ? grpc::StatusCode::CANCELLED + : grpc::StatusCode::ABORTED, + stopped ? error + : error + "; arm stop was not confirmed"); + } + if (!command.revalidate()) { + (void)touch_task->stopActivity(); + fillTouchResponse( + response, + false, + buildTouchFailureMessage( + *touch_task, + "TouchScreenTask safety admission was revoked")); + return command.dispatchStatus(); } std::this_thread::sleep_for(std::chrono::milliseconds(10)); } @@ -91,7 +173,9 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr: const std::string error = buildTouchFailureMessage(*touch_task, "TouchScreenTask touch failed"); fillTouchResponse(response, false, error); - return grpc::Status(grpc::StatusCode::INTERNAL, error); + return command.dispatchStatus().ok() + ? grpc::Status(grpc::StatusCode::INTERNAL, error) + : command.dispatchStatus(); } fillTouchResponse(response, true, ""); @@ -100,8 +184,9 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr: << ", phase=" << cmvr::task::TouchScreenTask::phaseToString(touch_task->phase()) << ", status=" << cmvr::task::TouchScreenTask::statusToString(touch_task->lastStatus()); return grpc::Status::OK; - } catch (const std::exception& e) { - fillTouchResponse(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } + } catch (...) { + (void)touch_task->stopActivity(); + throw; + } + }); } diff --git a/cmvr-es/service/grpc/src/grpc_microphone_service.cpp b/cmvr-es/service/grpc/src/grpc_microphone_service.cpp index e2cb0272..65ea67df 100644 --- a/cmvr-es/service/grpc/src/grpc_microphone_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_microphone_service.cpp @@ -1,10 +1,13 @@ #include "common/base/logging/logger.h" #include "manager/media_source_hub/include/device_media_source_adapter.h" +#include "service/grpc/include/grpc_command_transaction.h" #include "service/grpc/include/media_activity_coordinator.h" +#include "service/grpc/include/grpc_security.h" #include #include #include #include +#include // // Created by linbo on 2025/6/13. // Created by xtkuang on 2025/6/13. @@ -33,10 +36,20 @@ grpc::Status mediaStoppedStatus() } -gRPCMicroPhoneServiceImpl::gRPCMicroPhoneServiceImpl(): dmgr_(DeviceManager::getInstance()) {} +gRPCMicroPhoneServiceImpl::gRPCMicroPhoneServiceImpl() + : gRPCMicroPhoneServiceImpl(makeDefaultGrpcSecurityGateway()) {} + +gRPCMicroPhoneServiceImpl::gRPCMicroPhoneServiceImpl( + std::shared_ptr security_gateway) + : dmgr_(DeviceManager::getInstance()), + security_gateway_(security_gateway + ? std::move(security_gateway) + : makeDefaultGrpcSecurityGateway()) {} grpc::Status gRPCMicroPhoneServiceImpl::GetStatus(grpc::ServerContext* context, const api::GetMicStateCommand_Request* request, api::GetMicStateCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.MicPhoneService/GetStatus"); try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (GetStatus): id=" << dev_id; @@ -70,12 +83,15 @@ grpc::Status gRPCMicroPhoneServiceImpl::GetStatus(grpc::ServerContext* context, grpc::Status gRPCMicroPhoneServiceImpl::StartRecord(grpc::ServerContext* context, const api::StartMicRecordingCommand_Request* request, api::StartMicRecordingCommand_Feedback* response) { - auto media_session = globalMediaActivityCoordinator().beginSession(); - if (!media_session) { - return failResponse( - response, "Media activities are temporarily paused by StopAll"); - } - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.MicPhoneService/StartRecord", request, response, + [this, request, response](GrpcCommandTransaction& command) { + auto media_session = globalMediaActivityCoordinator().beginSession(); + if (!media_session) { + return failResponse( + response, "Media activities are temporarily paused by StopAll"); + } string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StartRecord): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); @@ -83,7 +99,12 @@ grpc::Status gRPCMicroPhoneServiceImpl::StartRecord(grpc::ServerContext* context return failResponse(response, "Microphone device not found: " + dev_id); } bool started = false; + bool dispatch_allowed = false; const bool start_allowed = media_session.runIfCurrent([&] { + dispatch_allowed = command.beginDispatch(); + if (!dispatch_allowed) { + return; + } started = dev->start(); if (started) { dev->startRecording(request->file_path()); @@ -93,6 +114,9 @@ grpc::Status gRPCMicroPhoneServiceImpl::StartRecord(grpc::ServerContext* context return failResponse( response, "Microphone recording start was canceled by StopAll"); } + if (!dispatch_allowed) { + return command.dispatchStatus(); + } if (!started) { return failResponse(response, "Failed to start microphone: " + dev_id); } @@ -101,95 +125,97 @@ grpc::Status gRPCMicroPhoneServiceImpl::StartRecord(grpc::ServerContext* context CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (StartRecord): success, id=" << dev_id << ", path=" << request->file_path(); return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCMicroPhoneServiceImpl::StopRecord(grpc::ServerContext* context, const api::StopMicRecordingCommand_Request* request, api::StopMicRecordingCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.MicPhoneService/StopRecord", request, response, + [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StopRecord): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); if (!dev) { return failResponse(response, "Microphone device not found: " + dev_id); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } dev->stopRecording(); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (StopRecord): success, id=" << dev_id; return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCMicroPhoneServiceImpl::PauseRecord(grpc::ServerContext* context, const api::PauseMicRecordingCommand_Request* request, api::PauseMicRecordingCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.MicPhoneService/PauseRecord", request, response, + [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (PauseRecord): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); if (!dev) { return failResponse(response, "Microphone device not found: " + dev_id); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } dev->pause(); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (PauseRecord): success, id=" << dev_id; return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCMicroPhoneServiceImpl::ResumeRecord(grpc::ServerContext* context, const api::ResumeMicRecordingCommand_Request* request, api::ResumeMicRecordingCommand_Feedback* response) { - auto media_session = globalMediaActivityCoordinator().beginSession(); - if (!media_session) { - return failResponse( - response, "Media activities are temporarily paused by StopAll"); - } - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.MicPhoneService/ResumeRecord", request, response, + [this, request, response](GrpcCommandTransaction& command) { + auto media_session = globalMediaActivityCoordinator().beginSession(); + if (!media_session) { + return failResponse( + response, "Media activities are temporarily paused by StopAll"); + } string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (ResumeRecord): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); if (!dev) { return failResponse(response, "Microphone device not found: " + dev_id); } - if (!media_session.runIfCurrent([&] { dev->resume(); })) { + bool dispatch_allowed = false; + if (!media_session.runIfCurrent([&] { + dispatch_allowed = command.beginDispatch(); + if (dispatch_allowed) { + dev->resume(); + } + })) { return failResponse( response, "Microphone recording resume was canceled by StopAll"); } + if (!dispatch_allowed) { + return command.dispatchStatus(); + } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (ResumeRecord): success, id=" << dev_id; return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context, const api::StreamMicAudioCommand_Request* request, grpc::ServerWriter* writer) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.MicPhoneService/StreamAudio"); auto media_session = globalMediaActivityCoordinator().beginSession( [context] { context->TryCancel(); }); if (!media_session) { @@ -233,12 +259,16 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context return grpc::Status::OK; } - auto subscription = media_hub.subscribe( + auto source_dispatch = cmvr::media::beginMediaSourceStartDispatch( + dmgr_.safetyCoordinator(), dev_id); + auto subscription = source_dispatch.acquired() + ? media_hub.subscribe( track_id, cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED, [context, &media_session] { return context->IsCancelled() || media_session.cancelled(); - }); + }) + : cmvr::media::MediaSourceHub::Subscription{}; if (!subscription) { api::StreamMicAudioCommand_Feedback feedback; feedback.mutable_header()->set_success(false); @@ -325,30 +355,32 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context grpc::Status gRPCMicroPhoneServiceImpl::SetVolume(grpc::ServerContext* context, const api::SetMicPhoneVolumeCommand_Request* request, api::SetMicPhoneVolumeCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.MicPhoneService/SetVolume", request, response, + [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (SetVolume): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); if (!dev) { return failResponse(response, "Microphone device not found: " + dev_id); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } dev->setVolume(request->volume()); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (SetVolume): success, id=" << dev_id << ", volume=" << request->volume(); return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCMicroPhoneServiceImpl::GetVolume(grpc::ServerContext* context, const api::GetMicPhoneVolumeCommand_Request* request, api::GetMicPhoneVolumeCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.MicPhoneService/GetVolume"); try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (GetVolume): id=" << dev_id; diff --git a/cmvr-es/service/grpc/src/grpc_motor_service.cpp b/cmvr-es/service/grpc/src/grpc_motor_service.cpp index f4a88b0c..183937ca 100644 --- a/cmvr-es/service/grpc/src/grpc_motor_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_motor_service.cpp @@ -15,6 +15,8 @@ #include "common/base/logging/logger.h" #include "devices/motor/manager/include/motor_manager.h" #include "manager/device_manager/include/device_manager.h" +#include "service/grpc/include/grpc_command_transaction.h" +#include "service/grpc/include/grpc_security.h" #include "service/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { @@ -161,7 +163,8 @@ template grpc::Status runUnaryGuarded(Response* response, const char* rpc_name, Body&& body, - Cleanup&& cleanup) + Cleanup&& cleanup, + GrpcCommandTransaction* command = nullptr) { try { return body(); @@ -174,6 +177,9 @@ grpc::Status runUnaryGuarded(Response* response, } response->Clear(); fillFeedback(response->mutable_header(), false, error); + if (command) { + return command->finishException(error); + } return grpc::Status(grpc::StatusCode::INTERNAL, error); } catch (...) { const std::string error = @@ -184,6 +190,9 @@ grpc::Status runUnaryGuarded(Response* response, } response->Clear(); fillFeedback(response->mutable_header(), false, error); + if (command) { + return command->finishException(error); + } return grpc::Status(grpc::StatusCode::INTERNAL, error); } } @@ -215,7 +224,8 @@ grpc::Status runStreamingGuarded(const char* rpc_name, } template + typename IsPreempted, typename ValidateSafety, + typename SetLastError, typename Stop> grpc::Status runCyclicLoop( grpc::ServerContext* context, grpc::ServerReaderWriter* stream, @@ -224,6 +234,7 @@ grpc::Status runCyclicLoop( Apply&& apply, FillStatus&& fill_status, IsPreempted&& is_preempted, + ValidateSafety&& validate_safety, SetLastError&& set_last_error, Stop&& stop) { @@ -398,6 +409,15 @@ grpc::Status runCyclicLoop( grpc::StatusCode::INTERNAL, "failed to quick-stop preempted cyclic stream"); } + const auto safety_status = validate_safety(); + if (!safety_status.ok()) { + const std::string error = + "cyclic stream safety session was invalidated: " + + safety_status.error_message(); + return finishTerminal( + api::CYCLIC_STREAM_FAILED, false, error, + safety_status, true); + } std::optional request; bool ended = false; @@ -548,7 +568,16 @@ gRPCMotorServiceImpl::ControlLease::~ControlLease() } gRPCMotorServiceImpl::gRPCMotorServiceImpl() - : dmgr_(device::DeviceManager::getInstance()) + : gRPCMotorServiceImpl(makeDefaultGrpcSecurityGateway()) +{ +} + +gRPCMotorServiceImpl::gRPCMotorServiceImpl( + std::shared_ptr security_gateway) + : dmgr_(device::DeviceManager::getInstance()), + security_gateway_(security_gateway + ? std::move(security_gateway) + : makeDefaultGrpcSecurityGateway()) { } @@ -840,11 +869,20 @@ grpc::Status gRPCMotorServiceImpl::setZero( const api::SetMotorZeroRequest* request, api::MotorCommandResponse* response) { - return runUnaryGuarded( - response, "setZero", - [&]() { return setZeroImpl(context, request, response); }, - [&](const std::string& error) { - bestEffortQuickStop(request->target(), error); + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.MotorService/setZero", request, response, + [this, context, request, response](GrpcCommandTransaction& command) { + return runUnaryGuarded( + response, "setZero", + [&]() { + return setZeroImpl( + context, request, response, command); + }, + [&](const std::string& error) { + bestEffortQuickStop(request->target(), error); + }, + &command); }); } @@ -853,11 +891,20 @@ grpc::Status gRPCMotorServiceImpl::moveToZero( const api::MoveMotorToZeroRequest* request, api::MotorCommandResponse* response) { - return runUnaryGuarded( - response, "moveToZero", - [&]() { return moveToZeroImpl(context, request, response); }, - [&](const std::string& error) { - bestEffortQuickStop(request->target(), error); + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.MotorService/moveToZero", request, response, + [this, context, request, response](GrpcCommandTransaction& command) { + return runUnaryGuarded( + response, "moveToZero", + [&]() { + return moveToZeroImpl( + context, request, response, command); + }, + [&](const std::string& error) { + bestEffortQuickStop(request->target(), error); + }, + &command); }); } @@ -866,11 +913,20 @@ grpc::Status gRPCMotorServiceImpl::profilePosition( const api::ProfilePositionRequest* request, api::MotorCommandResponse* response) { - return runUnaryGuarded( - response, "profilePosition", - [&]() { return profilePositionImpl(context, request, response); }, - [&](const std::string& error) { - bestEffortQuickStop(request->target(), error); + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.MotorService/profilePosition", request, response, + [this, context, request, response](GrpcCommandTransaction& command) { + return runUnaryGuarded( + response, "profilePosition", + [&]() { + return profilePositionImpl( + context, request, response, command); + }, + [&](const std::string& error) { + bestEffortQuickStop(request->target(), error); + }, + &command); }); } @@ -879,11 +935,20 @@ grpc::Status gRPCMotorServiceImpl::profileVelocity( const api::ProfileVelocityRequest* request, api::MotorCommandResponse* response) { - return runUnaryGuarded( - response, "profileVelocity", - [&]() { return profileVelocityImpl(context, request, response); }, - [&](const std::string& error) { - bestEffortQuickStop(request->target(), error); + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.MotorService/profileVelocity", request, response, + [this, context, request, response](GrpcCommandTransaction& command) { + return runUnaryGuarded( + response, "profileVelocity", + [&]() { + return profileVelocityImpl( + context, request, response, command); + }, + [&](const std::string& error) { + bestEffortQuickStop(request->target(), error); + }, + &command); }); } @@ -892,12 +957,16 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicPosition( grpc::ServerReaderWriter* stream) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.MotorService/streamCyclicPosition"); std::optional cleanup_target; return runStreamingGuarded( "streamCyclicPosition", [&]() { return streamCyclicPositionImpl( - context, stream, cleanup_target); + context, stream, cleanup_target, + cmvr_grpc_call_guard.context()); }, [&](const std::string& error) { if (cleanup_target.has_value()) { @@ -911,12 +980,16 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicVelocity( grpc::ServerReaderWriter* stream) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.MotorService/streamCyclicVelocity"); std::optional cleanup_target; return runStreamingGuarded( "streamCyclicVelocity", [&]() { return streamCyclicVelocityImpl( - context, stream, cleanup_target); + context, stream, cleanup_target, + cmvr_grpc_call_guard.context()); }, [&](const std::string& error) { if (cleanup_target.has_value()) { @@ -930,11 +1003,20 @@ grpc::Status gRPCMotorServiceImpl::emergencyStop( const api::EmergencyStopRequest* request, api::MotorCommandResponse* response) { - return runUnaryGuarded( - response, "emergencyStop", - [&]() { return emergencyStopImpl(context, request, response); }, - [&](const std::string& error) { - bestEffortQuickStop(request->target(), error); + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.MotorService/emergencyStop", request, response, + [this, context, request, response](GrpcCommandTransaction& command) { + return runUnaryGuarded( + response, "emergencyStop", + [&]() { + return emergencyStopImpl( + context, request, response, command); + }, + [&](const std::string& error) { + bestEffortQuickStop(request->target(), error); + }, + &command); }); } @@ -943,6 +1025,8 @@ grpc::Status gRPCMotorServiceImpl::getStatus( const api::GetMotorStatusRequest* request, api::GetMotorStatusResponse* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.MotorService/getStatus"); return runUnaryGuarded( response, "getStatus", [&]() { return getStatusImpl(context, request, response); }, @@ -954,18 +1038,28 @@ grpc::Status gRPCMotorServiceImpl::setEnabled( const api::SetMotorEnabledRequest* request, api::MotorCommandResponse* response) { - return runUnaryGuarded( - response, "setEnabled", - [&]() { return setEnabledImpl(context, request, response); }, - [&](const std::string& error) { - bestEffortQuickStop(request->target(), error); + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.MotorService/setEnabled", request, response, + [this, context, request, response](GrpcCommandTransaction& command) { + return runUnaryGuarded( + response, "setEnabled", + [&]() { + return setEnabledImpl( + context, request, response, command); + }, + [&](const std::string& error) { + bestEffortQuickStop(request->target(), error); + }, + &command); }); } grpc::Status gRPCMotorServiceImpl::setZeroImpl( grpc::ServerContext* context, const api::SetMotorZeroRequest* request, - api::MotorCommandResponse* response) + api::MotorCommandResponse* response, + GrpcCommandTransaction& command) { const auto started = Clock::now(); ResolvedMotor resolved; @@ -1014,6 +1108,10 @@ grpc::Status gRPCMotorServiceImpl::setZeroImpl( response->set_elapsed_ms(elapsedMs(started)); return grpc::Status(grpc::StatusCode::ABORTED, error); } + if (!command.beginDispatch()) { + lease.reset(); + return command.dispatchStatus(); + } calibrated = resolved.motor->calibrateZeroQ(); { std::lock_guard state_lock(resolved.control->mutex); @@ -1074,7 +1172,8 @@ grpc::Status gRPCMotorServiceImpl::runProfilePosition( const double max_velocity_rad_s, const double acceleration_rad_s2, const api::MotorWaitOptions& wait, - api::MotorCommandResponse* response) + api::MotorCommandResponse* response, + GrpcCommandTransaction& command) { const auto started = Clock::now(); if (!isFinite(target_position_rad) || @@ -1131,6 +1230,10 @@ grpc::Status gRPCMotorServiceImpl::runProfilePosition( response->set_elapsed_ms(elapsedMs(started)); return grpc::Status(grpc::StatusCode::ABORTED, error); } + if (!command.beginDispatch()) { + lease.reset(); + return command.dispatchStatus(); + } resolved.motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); submitted = resolved.motor->commandProfilePosition( target_position_rad, max_velocity_rad_s, acceleration_rad_s2); @@ -1355,7 +1458,8 @@ grpc::Status gRPCMotorServiceImpl::waitForPosition( grpc::Status gRPCMotorServiceImpl::moveToZeroImpl( grpc::ServerContext* context, const api::MoveMotorToZeroRequest* request, - api::MotorCommandResponse* response) + api::MotorCommandResponse* response, + GrpcCommandTransaction& command) { ResolvedMotor resolved; auto status = resolveMotor(request->target(), resolved); @@ -1365,13 +1469,14 @@ grpc::Status gRPCMotorServiceImpl::moveToZeroImpl( } return runProfilePosition( context, resolved, 0.0, request->max_velocity_rad_s(), - request->acceleration_rad_s2(), request->wait(), response); + request->acceleration_rad_s2(), request->wait(), response, command); } grpc::Status gRPCMotorServiceImpl::profilePositionImpl( grpc::ServerContext* context, const api::ProfilePositionRequest* request, - api::MotorCommandResponse* response) + api::MotorCommandResponse* response, + GrpcCommandTransaction& command) { ResolvedMotor resolved; auto status = resolveMotor(request->target(), resolved); @@ -1382,7 +1487,7 @@ grpc::Status gRPCMotorServiceImpl::profilePositionImpl( return runProfilePosition( context, resolved, request->target_position_rad(), request->max_velocity_rad_s(), request->acceleration_rad_s2(), - request->wait(), response); + request->wait(), response, command); } grpc::Status gRPCMotorServiceImpl::waitForVelocity( @@ -1515,7 +1620,8 @@ grpc::Status gRPCMotorServiceImpl::waitForVelocity( grpc::Status gRPCMotorServiceImpl::profileVelocityImpl( grpc::ServerContext* context, const api::ProfileVelocityRequest* request, - api::MotorCommandResponse* response) + api::MotorCommandResponse* response, + GrpcCommandTransaction& command) { const auto started = Clock::now(); ResolvedMotor resolved; @@ -1578,6 +1684,10 @@ grpc::Status gRPCMotorServiceImpl::profileVelocityImpl( response->set_elapsed_ms(elapsedMs(started)); return grpc::Status(grpc::StatusCode::ABORTED, error); } + if (!command.beginDispatch()) { + lease.reset(); + return command.dispatchStatus(); + } resolved.motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY); submitted = resolved.motor->commandProfileVelocity( request->target_velocity_rad_s(), @@ -1672,7 +1782,8 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicPositionImpl( grpc::ServerContext* context, grpc::ServerReaderWriter* stream, - std::optional& cleanup_target) + std::optional& cleanup_target, + const GrpcRequestContext& request_context) { api::CyclicPositionRequest first; if (!stream->Read(&first) || !first.has_open()) { @@ -1693,6 +1804,28 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicPositionImpl( } const auto generation = lease->generation(); + const auto& header = first.open().target().header(); + GrpcStreamingSafetyOpen safety_open; + safety_open.full_method_name = + "/cmvr.api.MotorService/streamCyclicPosition"; + safety_open.device_id = header.device_id(); + safety_open.session_id = header.command_id().empty() + ? request_context.correlation_id + : header.command_id(); + safety_open.expected_service_instance_id = + header.expected_service_instance_id(); + if (header.has_expected_device_generation()) { + safety_open.expected_device_generation = + header.expected_device_generation(); + } + safety_open.authority_generation = generation; + safety_open.deadline = request_context.deadline; + GrpcStreamingSafetySession safety_session( + dmgr_.safetyCoordinator(), request_context, + std::move(safety_open)); + if (!safety_session.admitted()) { + return safety_session.status(); + } { std::lock_guard command_lock(resolved.control->command_mutex); if (context->IsCancelled() || @@ -1710,6 +1843,10 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicPositionImpl( grpc::StatusCode::ABORTED, "cyclic position open preempted by a stop request"); } + auto safety_dispatch = safety_session.beginDispatch(); + if (!safety_dispatch.acquired()) { + return safety_session.status(); + } resolved.motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); std::lock_guard state_lock(resolved.control->mutex); if (resolved.control->cancel_generation != generation) { @@ -1743,6 +1880,10 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicPositionImpl( grpc::StatusCode::INVALID_ARGUMENT, "cyclic position setpoint contains a non-finite value"); } + auto safety_dispatch = safety_session.beginDispatch(); + if (!safety_dispatch.acquired()) { + return safety_session.status(); + } const bool submitted = resolved.motor->commandCyclicPosition( setpoint.target_position_rad(), setpoint.has_target_velocity_rad_s() @@ -1770,6 +1911,11 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicPositionImpl( std::lock_guard lock(resolved.control->mutex); return resolved.control->cancel_generation != expected_generation; }, + [&]() { + return safety_session.revalidate() + ? grpc::Status::OK + : safety_session.status(); + }, [&](const std::string& error) { setLastError(resolved.control, error); }, @@ -1795,7 +1941,8 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicVelocityImpl( grpc::ServerContext* context, grpc::ServerReaderWriter* stream, - std::optional& cleanup_target) + std::optional& cleanup_target, + const GrpcRequestContext& request_context) { api::CyclicVelocityRequest first; if (!stream->Read(&first) || !first.has_open()) { @@ -1816,6 +1963,28 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicVelocityImpl( } const auto generation = lease->generation(); + const auto& header = first.open().target().header(); + GrpcStreamingSafetyOpen safety_open; + safety_open.full_method_name = + "/cmvr.api.MotorService/streamCyclicVelocity"; + safety_open.device_id = header.device_id(); + safety_open.session_id = header.command_id().empty() + ? request_context.correlation_id + : header.command_id(); + safety_open.expected_service_instance_id = + header.expected_service_instance_id(); + if (header.has_expected_device_generation()) { + safety_open.expected_device_generation = + header.expected_device_generation(); + } + safety_open.authority_generation = generation; + safety_open.deadline = request_context.deadline; + GrpcStreamingSafetySession safety_session( + dmgr_.safetyCoordinator(), request_context, + std::move(safety_open)); + if (!safety_session.admitted()) { + return safety_session.status(); + } { std::lock_guard command_lock(resolved.control->command_mutex); if (context->IsCancelled() || @@ -1833,6 +2002,10 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicVelocityImpl( grpc::StatusCode::ABORTED, "cyclic velocity open preempted by a stop request"); } + auto safety_dispatch = safety_session.beginDispatch(); + if (!safety_dispatch.acquired()) { + return safety_session.status(); + } resolved.motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); std::lock_guard state_lock(resolved.control->mutex); if (resolved.control->cancel_generation != generation) { @@ -1864,6 +2037,10 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicVelocityImpl( grpc::StatusCode::INVALID_ARGUMENT, "cyclic velocity setpoint contains a non-finite value"); } + auto safety_dispatch = safety_session.beginDispatch(); + if (!safety_dispatch.acquired()) { + return safety_session.status(); + } const bool submitted = resolved.motor->commandCyclicVelocity( setpoint.target_velocity_rad_s()); { @@ -1888,6 +2065,11 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicVelocityImpl( std::lock_guard lock(resolved.control->mutex); return resolved.control->cancel_generation != expected_generation; }, + [&]() { + return safety_session.revalidate() + ? grpc::Status::OK + : safety_session.status(); + }, [&](const std::string& error) { setLastError(resolved.control, error); }, @@ -1912,7 +2094,8 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicVelocityImpl( grpc::Status gRPCMotorServiceImpl::emergencyStopImpl( grpc::ServerContext*, const api::EmergencyStopRequest* request, - api::MotorCommandResponse* response) + api::MotorCommandResponse* response, + GrpcCommandTransaction& command) { const auto started = Clock::now(); ResolvedMotor resolved; @@ -1922,6 +2105,9 @@ grpc::Status gRPCMotorServiceImpl::emergencyStopImpl( return status; } std::lock_guard emergency_lock(resolved.control->emergency_mutex); + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } bool stopped = false; { { @@ -1993,7 +2179,8 @@ grpc::Status gRPCMotorServiceImpl::getStatusImpl( grpc::Status gRPCMotorServiceImpl::setEnabledImpl( grpc::ServerContext* context, const api::SetMotorEnabledRequest* request, - api::MotorCommandResponse* response) + api::MotorCommandResponse* response, + GrpcCommandTransaction& command) { const auto started = Clock::now(); ResolvedMotor resolved; @@ -2045,6 +2232,10 @@ grpc::Status gRPCMotorServiceImpl::setEnabledImpl( response->set_elapsed_ms(elapsedMs(started)); return grpc::Status(grpc::StatusCode::ABORTED, error); } + if (!command.beginDispatch()) { + lease.reset(); + return command.dispatchStatus(); + } success = request->enabled() ? resolved.motor->torqueOn() : resolved.motor->torqueOff(); diff --git a/cmvr-es/service/grpc/src/grpc_recovery_audit.cpp b/cmvr-es/service/grpc/src/grpc_recovery_audit.cpp new file mode 100644 index 00000000..e3bd03f0 --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_recovery_audit.cpp @@ -0,0 +1,164 @@ +#include "service/grpc/include/grpc_recovery_audit.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace cmvr::service { + +namespace { + +void setError(std::string* error, std::string message) noexcept +{ + if (!error) { + return; + } + try { + *error = std::move(message); + } catch (...) { + } +} + +class FileRecoveryAuditSink final : public RecoveryAuditSink { +public: + explicit FileRecoveryAuditSink(std::string path) + : path_(std::move(path)) + { + } + + bool append( + const RecoveryAuditRecord& record, + std::string* error) noexcept override + { + try { + google::protobuf::Struct event; + auto* fields = event.mutable_fields(); + (*fields)["schema_version"].set_string_value("1"); + (*fields)["occurred_at_unix_ms"].set_string_value( + std::to_string(record.occurred_at_unix_ms)); + (*fields)["stage"].set_string_value(record.stage); + (*fields)["correlation_id"].set_string_value( + record.correlation_id); + (*fields)["principal_id"].set_string_value(record.principal_id); + (*fields)["peer"].set_string_value(record.peer); + (*fields)["recovery_id"].set_string_value(record.recovery_id); + (*fields)["reason"].set_string_value(record.reason); + (*fields)["mode"].set_string_value(record.mode); + (*fields)["all_devices"].set_bool_value(record.all_devices); + (*fields)["expected_safety_epoch"].set_string_value( + std::to_string(record.expected_safety_epoch)); + (*fields)["previous_safety_epoch"].set_string_value( + std::to_string(record.previous_safety_epoch)); + (*fields)["current_safety_epoch"].set_string_value( + std::to_string(record.current_safety_epoch)); + (*fields)["result"].set_string_value(record.result); + auto* ids = (*fields)["device_ids"].mutable_list_value(); + for (const auto& id : record.device_ids) { + ids->add_values()->set_string_value(id); + } + + google::protobuf::util::JsonPrintOptions options; + options.preserve_proto_field_names = true; + std::string line; + const auto json_status = + google::protobuf::util::MessageToJsonString( + event, &line, options); + if (!json_status.ok()) { + setError(error, "failed to serialize recovery audit event"); + return false; + } + line.push_back('\n'); + + std::lock_guard lock(mutex_); + if (path_.empty()) { + setError(error, "recovery audit file is not configured"); + return false; + } + + int flags = O_WRONLY | O_CREAT | O_APPEND | O_CLOEXEC; +#ifdef O_NOFOLLOW + flags |= O_NOFOLLOW; +#endif + const int descriptor = ::open(path_.c_str(), flags, S_IRUSR | S_IWUSR); + if (descriptor < 0) { + setError( + error, + "failed to open recovery audit file: " + + std::string(std::strerror(errno))); + return false; + } + + const auto close_descriptor = [&] { (void)::close(descriptor); }; + struct stat metadata {}; + if (::fstat(descriptor, &metadata) != 0 || + !S_ISREG(metadata.st_mode) || metadata.st_uid != ::geteuid() || + (metadata.st_mode & (S_IRWXG | S_IRWXO)) != 0) { + close_descriptor(); + setError( + error, + "recovery audit file must be an owner-only regular file"); + return false; + } + + std::size_t written = 0; + while (written < line.size()) { + const auto count = ::write( + descriptor, + line.data() + written, + line.size() - written); + if (count < 0 && errno == EINTR) { + continue; + } + if (count <= 0) { + const auto message = std::string(std::strerror(errno)); + close_descriptor(); + setError( + error, + "failed to write recovery audit file: " + message); + return false; + } + written += static_cast(count); + } + if (::fdatasync(descriptor) != 0) { + const auto message = std::string(std::strerror(errno)); + close_descriptor(); + setError( + error, + "failed to sync recovery audit file: " + message); + return false; + } + close_descriptor(); + return true; + } catch (const std::exception& exception) { + setError( + error, + "recovery audit append threw: " + + std::string(exception.what())); + return false; + } catch (...) { + setError(error, "recovery audit append threw an unknown exception"); + return false; + } + } + +private: + std::string path_; + std::mutex mutex_; +}; + +} // namespace + +std::shared_ptr makeFileRecoveryAuditSink(std::string path) +{ + return std::make_shared(std::move(path)); +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/src/grpc_safety_participants.cpp b/cmvr-es/service/grpc/src/grpc_safety_participants.cpp new file mode 100644 index 00000000..e0f74fc7 --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_safety_participants.cpp @@ -0,0 +1,1139 @@ +#include "service/grpc/include/grpc_safety_participants.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "manager/media_source_hub/include/device_media_source_adapter.h" +#include "manager/safety/include/safety_coordinator.h" +#include "manager/task_manager/include/task_manager.h" +#include "service/action/include/action_queue_executor.h" +#include "service/grpc/include/camera_operational_activity_registry.h" +#include "service/grpc/include/camera_ptz_activity_registry.h" +#include "service/grpc/include/media_activity_coordinator.h" +#include "service/grpc/include/motor_activity_coordinator.h" +#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/stop_all/include/stop_operation_dispatcher.h" + +namespace cmvr::service { + +namespace { + +using safety::BarrierToken; +using safety::ParticipantDescriptor; +using safety::ParticipantPhase; +using safety::ParticipantResult; +using safety::RecoveryCheckResult; +using safety::RecoveryContext; +using safety::SafetyOperationContext; +using safety::SafetyParticipant; +using safety::SafetyReason; + +constexpr auto kParticipantTimeout = std::chrono::seconds(5); + +std::chrono::milliseconds remaining( + const safety::SafetyClock::time_point deadline) noexcept +{ + const auto now = safety::SafetyClock::now(); + if (now >= deadline) { + return std::chrono::milliseconds::zero(); + } + return std::chrono::duration_cast( + deadline - now); +} + +ParticipantResult success() +{ + return {true, SafetyReason::None, {}}; +} + +ParticipantResult failure( + const SafetyReason reason, + std::string detail) +{ + return {false, reason, std::move(detail)}; +} + +class TokenSequence { +protected: + BarrierToken tokenFor( + const std::string& participant_id, + const SafetyOperationContext& context) + { + return { + participant_id, + context.operation_id, + context.safety_epoch, + sequence_.fetch_add(1, std::memory_order_relaxed) + 1U}; + } + +private: + std::atomic sequence_{0}; +}; + +class IngressParticipant final : public SafetyParticipant, + private TokenSequence { +public: + ParticipantDescriptor descriptor() const override + { + return {"subsystem:ingress", ParticipantPhase::Ingress, true, + kParticipantTimeout}; + } + + BarrierToken beginBarrier( + const SafetyOperationContext& context) override + { + const auto token = tokenFor(descriptor().participant_id, context); + const auto ticket = globalStopAllAdmissionGate().beginStopAll(); + std::lock_guard lock(mutex_); + tickets_.emplace(token.generation, Entry{ticket, false}); + return token; + } + + ParticipantResult requestQuiesce( + const BarrierToken&, + const SafetyOperationContext&) override + { + return success(); + } + + ParticipantResult verifyQuiescent( + const BarrierToken&, + const SafetyOperationContext&) override + { + return globalStopAllAdmissionGate().lockAdmission().accepting() + ? failure( + SafetyReason::StopUnconfirmed, + "system ingress admission did not close") + : success(); + } + + RecoveryCheckResult recoverAdmission( + const BarrierToken& token, + const RecoveryContext&) override + { + bool rearm = false; + { + std::lock_guard lock(mutex_); + const auto found = tickets_.find(token.generation); + if (found == tickets_.end()) { + return { + false, + SafetyReason::StopUnconfirmed, + "system ingress recovery barrier is unavailable"}; + } + rearm = found->second.consumed; + } + if (rearm) { + const auto ticket = globalStopAllAdmissionGate().beginStopAll(); + if (!ticket.valid()) { + return { + false, + SafetyReason::StopUnconfirmed, + "system ingress could not begin a recovery round"}; + } + std::lock_guard lock(mutex_); + const auto found = tickets_.find(token.generation); + if (found == tickets_.end() || !found->second.consumed) { + (void)globalStopAllAdmissionGate().finishStopAll( + ticket, false); + return { + false, + SafetyReason::StopUnconfirmed, + "system ingress recovery barrier changed while rearming"}; + } + found->second.ticket = ticket; + found->second.consumed = false; + } + return {true, SafetyReason::None, {}}; + } + + ParticipantResult releaseBarrier( + const BarrierToken& token) noexcept override + { + try { + StopAllAdmissionGate::StopAllTicket ticket; + { + std::lock_guard lock(mutex_); + const auto found = tickets_.find(token.generation); + if (found == tickets_.end()) { + return failure( + SafetyReason::StopUnconfirmed, + "system ingress barrier is unavailable"); + } + ticket = found->second.ticket; + } + const auto committed = globalStopAllAdmissionGate() + .finishStopAllDetailed(ticket, true); + std::lock_guard lock(mutex_); + const auto found = tickets_.find(token.generation); + if (found != tickets_.end()) { + if (committed.admission_reopened) { + tickets_.erase(found); + } else if (committed.ticket_consumed) { + found->second.consumed = true; + } + } + return committed.admission_reopened + ? success() + : failure( + SafetyReason::StopUnconfirmed, + "could not safely resume system admission"); + } catch (...) { + return failure( + SafetyReason::InternalError, + "system ingress barrier release threw an exception"); + } + } + +private: + struct Entry { + StopAllAdmissionGate::StopAllTicket ticket; + bool consumed{false}; + }; + std::mutex mutex_; + std::unordered_map tickets_; +}; + +class ActionQueueParticipant final : public SafetyParticipant, + private TokenSequence { +public: + ActionQueueParticipant( + std::string participant_id, + std::shared_ptr executor) + : participant_id_(std::move(participant_id)), + executor_(std::move(executor)) + { + } + + ParticipantDescriptor descriptor() const override + { + return {participant_id_, ParticipantPhase::Scheduler, true, + kParticipantTimeout}; + } + + BarrierToken beginBarrier( + const SafetyOperationContext& context) override + { + const auto token = tokenFor(participant_id_, context); + const auto ticket = executor_->beginStopAll(true); + if (!ticket.valid()) { + return {}; + } + std::lock_guard lock(mutex_); + tickets_.emplace(token.generation, Entry{ticket, false}); + return token; + } + + ParticipantResult requestQuiesce( + const BarrierToken&, + const SafetyOperationContext&) override + { + return success(); + } + + ParticipantResult verifyQuiescent( + const BarrierToken&, + const SafetyOperationContext& context) override + { + return executor_->waitForIdle(remaining(context.deadline)) + ? success() + : failure( + SafetyReason::ParticipantTimeout, + "ActionQueue did not become idle before the deadline"); + } + + RecoveryCheckResult recoverAdmission( + const BarrierToken& token, + const RecoveryContext& context) override + { + bool rearm = false; + { + std::lock_guard lock(mutex_); + const auto found = tickets_.find(token.generation); + if (found == tickets_.end()) { + return { + false, + SafetyReason::StopUnconfirmed, + "ActionQueue recovery barrier is unavailable"}; + } + rearm = found->second.consumed; + } + if (rearm) { + const auto ticket = executor_->beginStopAll(true); + if (!ticket.valid()) { + return { + false, + SafetyReason::SystemStopping, + "ActionQueue is shutting down and cannot reopen"}; + } + std::lock_guard lock(mutex_); + const auto found = tickets_.find(token.generation); + if (found == tickets_.end() || !found->second.consumed) { + (void)executor_->finishStopAll(ticket, false); + return { + false, + SafetyReason::StopUnconfirmed, + "ActionQueue recovery barrier changed while rearming"}; + } + found->second.ticket = ticket; + found->second.consumed = false; + } + const bool idle = executor_->waitForIdle(remaining(context.deadline)); + return { + idle, + idle ? SafetyReason::None : SafetyReason::ParticipantTimeout, + idle ? std::string{} + : "ActionQueue remains active during recovery"}; + } + + ParticipantResult releaseBarrier( + const BarrierToken& token) noexcept override + { + try { + ActionQueueExecutor::StopAllTicket ticket; + { + std::lock_guard lock(mutex_); + const auto found = tickets_.find(token.generation); + if (found == tickets_.end()) { + return failure( + SafetyReason::StopUnconfirmed, + "ActionQueue barrier is unavailable"); + } + ticket = found->second.ticket; + } + const bool reopened = executor_->finishStopAll(ticket, true); + std::lock_guard lock(mutex_); + const auto found = tickets_.find(token.generation); + if (found != tickets_.end()) { + if (reopened) { + tickets_.erase(found); + } else { + found->second.consumed = true; + } + } + return reopened + ? success() + : failure( + SafetyReason::StopUnconfirmed, + "could not safely resume ActionQueue admission"); + } catch (...) { + return failure( + SafetyReason::InternalError, + "ActionQueue barrier release threw an exception"); + } + } + +private: + struct Entry { + ActionQueueExecutor::StopAllTicket ticket; + bool consumed{false}; + }; + std::string participant_id_; + std::shared_ptr executor_; + std::mutex mutex_; + std::unordered_map tickets_; +}; + +struct DispatchedBatch { + std::vector handles; +}; + +ParticipantResult waitForBatch( + const DispatchedBatch& batch, + const safety::SafetyClock::time_point deadline, + const std::string& description) +{ + for (const auto& handle : batch.handles) { + const auto result = handle.waitUntil(deadline); + if (!result.completed) { + return failure( + SafetyReason::ParticipantTimeout, + description + " did not complete before the deadline"); + } + if (!result.result) { + return failure( + SafetyReason::StopUnconfirmed, + result.detail.empty() + ? description + " did not confirm stop" + : result.detail); + } + } + return success(); +} + +StopOperationDispatcher::Handle submitDeferred( + StopOperationDispatcher& dispatcher, + std::string prefix, + DeferredStopOperation operation) +{ + auto callback = std::move(operation.operation); + return dispatcher.submit( + std::move(prefix) + operation.resource_key, + [callback = std::move(callback)]() mutable { + const auto result = callback(); + return StopOperationDispatcher::OperationResult{ + result.success, result.detail}; + }); +} + +class MediaSessionParticipant final : public SafetyParticipant, + private TokenSequence { +public: + explicit MediaSessionParticipant( + std::shared_ptr dispatcher) + : dispatcher_(std::move(dispatcher)) + { + } + + ParticipantDescriptor descriptor() const override + { + return {"subsystem:media-sessions", + ParticipantPhase::ControlSession, true, + kParticipantTimeout}; + } + + BarrierToken beginBarrier( + const SafetyOperationContext& context) override + { + const auto token = tokenFor(descriptor().participant_id, context); + Entry entry; + entry.ticket = globalMediaActivityCoordinator().beginStopAll(true); + if (!entry.ticket.valid()) { + return {}; + } + std::lock_guard lock(mutex_); + entries_.emplace(token.generation, std::move(entry)); + return token; + } + + ParticipantResult requestQuiesce( + const BarrierToken& token, + const SafetyOperationContext&) override + { + std::vector operations; + std::string error; + Entry* entry = nullptr; + { + std::lock_guard lock(mutex_); + const auto found = entries_.find(token.generation); + if (found == entries_.end()) { + return failure( + SafetyReason::StopUnconfirmed, + "media session barrier is unavailable"); + } + if (!globalMediaActivityCoordinator() + .collectCancellationOperations( + found->second.ticket, operations, &error)) { + return failure( + SafetyReason::StopUnconfirmed, + error.empty() + ? "could not collect media session cancellations" + : error); + } + entry = &found->second; + entry->batch.handles.reserve(operations.size()); + for (auto& operation : operations) { + entry->batch.handles.push_back(submitDeferred( + *dispatcher_, "safety:media-session:", + std::move(operation))); + } + } + (void)entry; + return success(); + } + + ParticipantResult verifyQuiescent( + const BarrierToken& token, + const SafetyOperationContext& context) override + { + Entry entry; + { + std::lock_guard lock(mutex_); + const auto found = entries_.find(token.generation); + if (found == entries_.end()) { + return failure( + SafetyReason::StopUnconfirmed, + "media session barrier is unavailable"); + } + entry = found->second; + } + const auto batch = waitForBatch( + entry.batch, context.deadline, "media cancellation"); + if (!batch.success) { + return batch; + } + return globalMediaActivityCoordinator().waitForStopped( + entry.ticket, remaining(context.deadline)) + ? success() + : failure( + SafetyReason::ParticipantTimeout, + "media sessions remained active before the deadline"); + } + + RecoveryCheckResult recoverAdmission( + const BarrierToken& token, + const RecoveryContext& context) override + { + bool rearm = false; + MediaActivityCoordinator::StopAllTicket ticket; + { + std::lock_guard lock(mutex_); + const auto found = entries_.find(token.generation); + if (found == entries_.end()) { + return { + false, SafetyReason::StopUnconfirmed, + "media recovery barrier is unavailable"}; + } + ticket = found->second.ticket; + rearm = found->second.consumed; + } + if (rearm) { + auto replacement = + globalMediaActivityCoordinator().beginStopAll(true); + if (!replacement.valid()) { + return { + false, SafetyReason::StopUnconfirmed, + "could not rearm media admission during recovery"}; + } + { + std::lock_guard lock(mutex_); + const auto found = entries_.find(token.generation); + if (found == entries_.end() || !found->second.consumed) { + (void)globalMediaActivityCoordinator() + .finishStopAllDetailed(replacement, false); + return { + false, SafetyReason::StopUnconfirmed, + "media recovery barrier changed while rearming"}; + } + found->second.ticket = replacement; + found->second.batch = {}; + found->second.consumed = false; + ticket = replacement; + } + + const SafetyOperationContext operation_context{ + context.recovery_id, + context.safety_epoch, + context.deadline}; + const auto requested = requestQuiesce(token, operation_context); + if (!requested.success) { + return { + false, requested.reason, std::move(requested.detail)}; + } + const auto verified = verifyQuiescent(token, operation_context); + return { + verified.success, + verified.reason, + std::move(verified.detail)}; + } + const bool stopped = globalMediaActivityCoordinator().waitForStopped( + ticket, remaining(context.deadline)); + return { + stopped, + stopped ? SafetyReason::None : SafetyReason::ParticipantTimeout, + stopped ? std::string{} + : "media sessions remain active during recovery"}; + } + + ParticipantResult releaseBarrier( + const BarrierToken& token) noexcept override + { + try { + MediaActivityCoordinator::StopAllTicket ticket; + { + std::lock_guard lock(mutex_); + const auto found = entries_.find(token.generation); + if (found == entries_.end()) { + return failure( + SafetyReason::StopUnconfirmed, + "media session barrier is unavailable"); + } + ticket = found->second.ticket; + } + const auto committed = globalMediaActivityCoordinator() + .finishStopAllDetailed(ticket, true); + { + std::lock_guard lock(mutex_); + const auto found = entries_.find(token.generation); + if (found != entries_.end() && + found->second.ticket.generation == ticket.generation && + found->second.ticket.ticket_id == ticket.ticket_id) { + if (committed.admission_resumed) { + entries_.erase(found); + } else if (committed.ticket_consumed) { + found->second.consumed = true; + } + } + } + return committed.participant_stopped && + committed.admission_resumed + ? success() + : failure( + SafetyReason::StopUnconfirmed, + committed.ticket_consumed + ? "media stopped but admission remained latched" + : "could not safely resume media session admission"); + } catch (...) { + return failure( + SafetyReason::InternalError, + "media session barrier release threw an exception"); + } + } + +private: + struct Entry { + MediaActivityCoordinator::StopAllTicket ticket; + DispatchedBatch batch; + bool consumed{false}; + }; + std::shared_ptr dispatcher_; + std::mutex mutex_; + std::unordered_map entries_; +}; + +class MotorSessionParticipant final : public SafetyParticipant, + private TokenSequence { +public: + explicit MotorSessionParticipant( + std::shared_ptr dispatcher) + : dispatcher_(std::move(dispatcher)) + { + } + + ParticipantDescriptor descriptor() const override + { + return {"subsystem:motor-sessions", + ParticipantPhase::ControlSession, true, + kParticipantTimeout}; + } + + BarrierToken beginBarrier( + const SafetyOperationContext& context) override + { + const auto token = tokenFor(descriptor().participant_id, context); + Entry entry; + entry.ticket = globalMotorActivityCoordinator().beginStopAll(true); + if (!entry.ticket.valid()) { + return {}; + } + std::lock_guard lock(mutex_); + entries_.emplace(token.generation, std::move(entry)); + return token; + } + + ParticipantResult requestQuiesce( + const BarrierToken& token, + const SafetyOperationContext&) override + { + std::vector operations; + std::string error; + std::lock_guard lock(mutex_); + const auto found = entries_.find(token.generation); + if (found == entries_.end()) { + return failure( + SafetyReason::StopUnconfirmed, + "motor session barrier is unavailable"); + } + if (!globalMotorActivityCoordinator().collectStopOperations( + found->second.ticket, operations, &error)) { + return failure( + SafetyReason::StopUnconfirmed, + error.empty() ? "could not collect motor stop operations" + : error); + } + found->second.batch.handles.reserve(operations.size()); + for (auto& operation : operations) { + found->second.batch.handles.push_back(submitDeferred( + *dispatcher_, "safety:motor-session:", + std::move(operation))); + } + return success(); + } + + ParticipantResult verifyQuiescent( + const BarrierToken& token, + const SafetyOperationContext& context) override + { + Entry entry; + { + std::lock_guard lock(mutex_); + const auto found = entries_.find(token.generation); + if (found == entries_.end()) { + return failure( + SafetyReason::StopUnconfirmed, + "motor session barrier is unavailable"); + } + entry = found->second; + } + const auto batch = waitForBatch( + entry.batch, context.deadline, "motor quick-stop"); + if (!batch.success) { + return batch; + } + std::string error; + if (!globalMotorActivityCoordinator().waitForStopped( + entry.ticket, remaining(context.deadline), &error)) { + return failure( + SafetyReason::ParticipantTimeout, + error.empty() + ? "motor sessions remained active before the deadline" + : error); + } + return success(); + } + + RecoveryCheckResult recoverAdmission( + const BarrierToken& token, + const RecoveryContext& context) override + { + bool rearm = false; + MotorActivityCoordinator::StopAllTicket ticket; + { + std::lock_guard lock(mutex_); + const auto found = entries_.find(token.generation); + if (found == entries_.end()) { + return { + false, SafetyReason::StopUnconfirmed, + "motor recovery barrier is unavailable"}; + } + ticket = found->second.ticket; + rearm = found->second.consumed; + } + if (rearm) { + auto replacement = + globalMotorActivityCoordinator().beginStopAll(true); + if (!replacement.valid()) { + return { + false, SafetyReason::StopUnconfirmed, + "could not rearm motor admission during recovery"}; + } + { + std::lock_guard lock(mutex_); + const auto found = entries_.find(token.generation); + if (found == entries_.end() || !found->second.consumed) { + (void)globalMotorActivityCoordinator() + .finishStopAllDetailed(replacement, false); + return { + false, SafetyReason::StopUnconfirmed, + "motor recovery barrier changed while rearming"}; + } + found->second.ticket = replacement; + found->second.batch = {}; + found->second.consumed = false; + ticket = replacement; + } + + const SafetyOperationContext operation_context{ + context.recovery_id, + context.safety_epoch, + context.deadline}; + const auto requested = requestQuiesce(token, operation_context); + if (!requested.success) { + return { + false, requested.reason, std::move(requested.detail)}; + } + const auto verified = verifyQuiescent(token, operation_context); + return { + verified.success, + verified.reason, + std::move(verified.detail)}; + } + std::string error; + const bool stopped = globalMotorActivityCoordinator().waitForStopped( + ticket, remaining(context.deadline), &error); + return { + stopped, + stopped ? SafetyReason::None : SafetyReason::ParticipantTimeout, + stopped ? std::string{} : error}; + } + + ParticipantResult releaseBarrier( + const BarrierToken& token) noexcept override + { + try { + MotorActivityCoordinator::StopAllTicket ticket; + { + std::lock_guard lock(mutex_); + const auto found = entries_.find(token.generation); + if (found == entries_.end()) { + return failure( + SafetyReason::StopUnconfirmed, + "motor session barrier is unavailable"); + } + ticket = found->second.ticket; + } + const auto committed = globalMotorActivityCoordinator() + .finishStopAllDetailed(ticket, true); + { + std::lock_guard lock(mutex_); + const auto found = entries_.find(token.generation); + if (found != entries_.end() && + found->second.ticket.generation == ticket.generation && + found->second.ticket.ticket_id == ticket.ticket_id) { + if (committed.admission_resumed) { + entries_.erase(found); + } else if (committed.ticket_consumed) { + found->second.consumed = true; + } + } + } + return committed.participant_stopped && + committed.admission_resumed + ? success() + : failure( + SafetyReason::StopUnconfirmed, + committed.ticket_consumed + ? "motors stopped but admission remained latched" + : "could not safely resume motor session admission"); + } catch (...) { + return failure( + SafetyReason::InternalError, + "motor session barrier release threw an exception"); + } + } + +private: + struct Entry { + MotorActivityCoordinator::StopAllTicket ticket; + DispatchedBatch batch; + bool consumed{false}; + }; + std::shared_ptr dispatcher_; + std::mutex mutex_; + std::unordered_map entries_; +}; + +using StopFactory = std::function()>; + +class DeferredBatchParticipant final : public SafetyParticipant, + private TokenSequence { +public: + DeferredBatchParticipant( + ParticipantDescriptor descriptor, + std::shared_ptr dispatcher, + StopFactory factory) + : descriptor_(std::move(descriptor)), + dispatcher_(std::move(dispatcher)), + factory_(std::move(factory)) + { + } + + ParticipantDescriptor descriptor() const override { return descriptor_; } + + BarrierToken beginBarrier( + const SafetyOperationContext& context) override + { + const auto token = tokenFor(descriptor_.participant_id, context); + std::lock_guard lock(mutex_); + batches_.try_emplace(token.generation); + return token; + } + + ParticipantResult requestQuiesce( + const BarrierToken& token, + const SafetyOperationContext&) override + { + auto operations = factory_(); + std::lock_guard lock(mutex_); + const auto found = batches_.find(token.generation); + if (found == batches_.end()) { + return failure( + SafetyReason::StopUnconfirmed, + descriptor_.participant_id + " barrier is unavailable"); + } + found->second.handles.clear(); + found->second.handles.reserve(operations.size()); + for (auto& operation : operations) { + found->second.handles.push_back(submitDeferred( + *dispatcher_, + "safety:" + descriptor_.participant_id + ':', + std::move(operation))); + } + return success(); + } + + ParticipantResult verifyQuiescent( + const BarrierToken& token, + const SafetyOperationContext& context) override + { + DispatchedBatch batch; + { + std::lock_guard lock(mutex_); + const auto found = batches_.find(token.generation); + if (found == batches_.end()) { + return failure( + SafetyReason::StopUnconfirmed, + descriptor_.participant_id + " barrier is unavailable"); + } + batch = found->second; + } + return waitForBatch( + batch, context.deadline, descriptor_.participant_id); + } + + RecoveryCheckResult recoverAdmission( + const BarrierToken& token, + const RecoveryContext& context) override + { + SafetyOperationContext operation{ + context.recovery_id, context.safety_epoch, context.deadline}; + const auto requested = requestQuiesce(token, operation); + if (!requested.success) { + return {false, requested.reason, requested.detail}; + } + const auto verified = verifyQuiescent(token, operation); + return {verified.success, verified.reason, verified.detail}; + } + + ParticipantResult releaseBarrier( + const BarrierToken& token) noexcept override + { + try { + std::lock_guard lock(mutex_); + batches_.erase(token.generation); + return success(); + } catch (...) { + return failure( + SafetyReason::InternalError, + descriptor_.participant_id + + " barrier release threw an exception"); + } + } + +private: + ParticipantDescriptor descriptor_; + std::shared_ptr dispatcher_; + StopFactory factory_; + std::mutex mutex_; + std::unordered_map batches_; +}; + +std::vector taskStopOperations() +{ + std::vector operations; + const auto tasks = task::TaskManager::activitySnapshotIfInitialized(); + operations.reserve(tasks.size()); + for (const auto& task : tasks) { + const auto id = task ? task->id() : std::string{"unknown"}; + operations.push_back({ + "task:" + id, + [task] { + if (!task) { + return DeferredStopResult{ + false, "task activity target is null"}; + } + return DeferredStopResult{ + task->stopActivity(), + "task " + task->id() + + " operational stop was not confirmed"}; + }}); + } + return operations; +} + +std::vector cameraPtzStopOperations() +{ + std::vector operations; + const auto ids = globalCameraPtzActivityRegistry().trackedDeviceIds(); + operations.reserve(ids.size()); + for (const auto& id : ids) { + operations.push_back({ + "camera-ptz:" + id, + [id] { + std::vector failures; + const bool stopped = globalCameraPtzActivityRegistry() + .stopActivitiesForDevice(id, &failures); + return DeferredStopResult{ + stopped, + failures.empty() ? std::string{} : failures.front()}; + }}); + } + return operations; +} + +std::vector mediaSourceStopOperations() +{ + std::unordered_set unique_ids; + const auto merge = [&unique_ids](const std::vector& ids) { + unique_ids.insert(ids.begin(), ids.end()); + }; + merge(media::globalMediaSourceHub().trackedSourceIds()); + merge(globalCameraOperationalActivityRegistry().trackedDeviceIds()); + + std::vector operations; + operations.reserve(unique_ids.size()); + for (const auto& id : unique_ids) { + operations.push_back({ + "media-source:" + id, + [id] { + std::vector failures; + bool stopped = media::globalMediaSourceHub() + .stopSourcesForDevice(id, &failures); + if (!globalCameraOperationalActivityRegistry() + .stopActivitiesForDevice(id, &failures)) { + stopped = false; + } + return DeferredStopResult{ + stopped, + failures.empty() ? std::string{} : failures.front()}; + }}); + } + return operations; +} + +struct SharedParticipants { + std::size_t owners{0}; + std::vector ids; +}; + +std::mutex shared_participants_mutex; +std::unordered_map + shared_participants; + +void acquireSharedParticipants( + safety::SafetyCoordinator& coordinator, + const std::shared_ptr& dispatcher) +{ + std::lock_guard lock(shared_participants_mutex); + auto found = shared_participants.find(&coordinator); + if (found != shared_participants.end()) { + ++found->second.owners; + return; + } + + std::vector> participants; + participants.push_back(std::make_shared()); + participants.push_back( + std::make_shared(dispatcher)); + participants.push_back( + std::make_shared(dispatcher)); + participants.push_back(std::make_shared( + ParticipantDescriptor{ + "subsystem:tasks", ParticipantPhase::Scheduler, true, + kParticipantTimeout}, + dispatcher, + taskStopOperations)); + participants.push_back(std::make_shared( + ParticipantDescriptor{ + "subsystem:camera-ptz", ParticipantPhase::Actuator, true, + kParticipantTimeout}, + dispatcher, + cameraPtzStopOperations)); + participants.push_back(std::make_shared( + ParticipantDescriptor{ + "subsystem:media-sources", + ParticipantPhase::PeripheralActivity, + true, + kParticipantTimeout}, + dispatcher, + mediaSourceStopOperations)); + + SharedParticipants entry; + entry.owners = 1; + try { + for (const auto& participant : participants) { + const auto id = participant->descriptor().participant_id; + if (!coordinator.registerParticipant(participant)) { + throw std::runtime_error( + "failed to register safety participant: " + id); + } + entry.ids.push_back(id); + } + } catch (...) { + for (const auto& id : entry.ids) { + (void)coordinator.unregisterParticipant(id); + } + throw; + } + shared_participants.emplace(&coordinator, std::move(entry)); +} + +void releaseSharedParticipants(safety::SafetyCoordinator& coordinator) noexcept +{ + try { + std::lock_guard lock(shared_participants_mutex); + const auto found = shared_participants.find(&coordinator); + if (found == shared_participants.end() || --found->second.owners != 0) { + return; + } + for (const auto& id : found->second.ids) { + (void)coordinator.unregisterParticipant(id); + } + // A retained barrier owns its participant independently. Never keep a + // raw coordinator key after the final service owner has gone. + shared_participants.erase(found); + } catch (...) { + } +} + +} // namespace + +struct GrpcSafetyParticipantRegistration::Impl { + safety::SafetyCoordinator* coordinator{nullptr}; + std::string action_participant_id; + bool shared_acquired{false}; + + ~Impl() + { + if (!coordinator) { + return; + } + if (!action_participant_id.empty()) { + (void)coordinator->unregisterParticipant(action_participant_id); + } + if (shared_acquired) { + releaseSharedParticipants(*coordinator); + } + } +}; + +GrpcSafetyParticipantRegistration::GrpcSafetyParticipantRegistration( + std::unique_ptr impl) + : impl_(std::move(impl)) +{ +} + +GrpcSafetyParticipantRegistration::~GrpcSafetyParticipantRegistration() = + default; +GrpcSafetyParticipantRegistration::GrpcSafetyParticipantRegistration( + GrpcSafetyParticipantRegistration&&) noexcept = default; +GrpcSafetyParticipantRegistration& +GrpcSafetyParticipantRegistration::operator=( + GrpcSafetyParticipantRegistration&&) noexcept = default; + +std::unique_ptr +registerGrpcSafetyParticipants( + safety::SafetyCoordinator& coordinator, + std::shared_ptr action_queue, + std::shared_ptr stop_dispatcher) +{ + if (!action_queue || !stop_dispatcher) { + throw std::invalid_argument( + "safety participant registration requires live executors"); + } + auto impl = std::make_unique(); + impl->coordinator = &coordinator; + acquireSharedParticipants(coordinator, stop_dispatcher); + impl->shared_acquired = true; + impl->action_participant_id = + "subsystem:action-queue:" + action_queue->instanceId(); + auto action_participant = std::make_shared( + impl->action_participant_id, std::move(action_queue)); + if (!coordinator.registerParticipant(std::move(action_participant))) { + throw std::runtime_error( + "failed to register ActionQueue safety participant"); + } + return std::unique_ptr( + new GrpcSafetyParticipantRegistration(std::move(impl))); +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/src/grpc_safety_proto.cpp b/cmvr-es/service/grpc/src/grpc_safety_proto.cpp new file mode 100644 index 00000000..4f270c4f --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_safety_proto.cpp @@ -0,0 +1,397 @@ +#include "service/grpc/include/grpc_safety_proto.h" + +#include + +namespace cmvr::service { + +namespace { + +const char* participantPhaseName( + const safety::ParticipantPhase phase) noexcept +{ + switch (phase) { + case safety::ParticipantPhase::Ingress: return "INGRESS"; + case safety::ParticipantPhase::Scheduler: return "SCHEDULER"; + case safety::ParticipantPhase::ControlSession: + return "CONTROL_SESSION"; + case safety::ParticipantPhase::Actuator: return "ACTUATOR"; + case safety::ParticipantPhase::PeripheralActivity: + return "PERIPHERAL_ACTIVITY"; + case safety::ParticipantPhase::Verification: return "VERIFICATION"; + } + return "UNKNOWN"; +} + +void populateParticipantResult( + const safety::ParticipantResultView& source, + api::SafetyParticipantResultInfo& destination) +{ + destination.set_recorded(source.recorded); + destination.set_success(source.success); + destination.set_reason_code(toApiSafetyReason(source.reason)); + destination.set_detail(source.detail); +} + +} // namespace + +api::CommandReasonCode toApiSafetyReason( + const safety::SafetyReason value) noexcept +{ + using safety::SafetyReason; + switch (value) { + case SafetyReason::None: return api::COMMAND_REASON_CODE_NONE; + case SafetyReason::InvalidArgument: + return api::COMMAND_REASON_CODE_INVALID_ARGUMENT; + case SafetyReason::Unauthenticated: + return api::COMMAND_REASON_CODE_UNAUTHENTICATED; + case SafetyReason::PermissionDenied: + return api::COMMAND_REASON_CODE_PERMISSION_DENIED; + case SafetyReason::RecoveryRpcDisabled: + return api::COMMAND_REASON_CODE_RECOVERY_RPC_DISABLED; + case SafetyReason::DeviceNotFound: + return api::COMMAND_REASON_CODE_DEVICE_NOT_FOUND; + case SafetyReason::DeviceUnavailable: + return api::COMMAND_REASON_CODE_DEVICE_UNAVAILABLE; + case SafetyReason::UnsupportedCommand: + return api::COMMAND_REASON_CODE_UNSUPPORTED_COMMAND; + case SafetyReason::SystemStarting: + return api::COMMAND_REASON_CODE_SYSTEM_STARTING; + case SafetyReason::SystemStopping: + return api::COMMAND_REASON_CODE_SYSTEM_STOPPING; + case SafetyReason::SafetyLatched: + return api::COMMAND_REASON_CODE_SAFETY_LATCHED; + case SafetyReason::SafetyStateMissing: + return api::COMMAND_REASON_CODE_SAFETY_STATE_MISSING; + case SafetyReason::SafetyStateStale: + return api::COMMAND_REASON_CODE_SAFETY_STATE_STALE; + case SafetyReason::HardwareUnsafe: + return api::COMMAND_REASON_CODE_HARDWARE_UNSAFE; + case SafetyReason::EmergencyStopActive: + return api::COMMAND_REASON_CODE_EMERGENCY_STOP_ACTIVE; + case SafetyReason::ProtectiveStopActive: + return api::COMMAND_REASON_CODE_PROTECTIVE_STOP_ACTIVE; + case SafetyReason::DeviceDisconnected: + return api::COMMAND_REASON_CODE_DEVICE_DISCONNECTED; + case SafetyReason::DeviceFault: + return api::COMMAND_REASON_CODE_DEVICE_FAULT; + case SafetyReason::DeviceNotReady: + return api::COMMAND_REASON_CODE_DEVICE_NOT_READY; + case SafetyReason::DeviceStillMoving: + return api::COMMAND_REASON_CODE_DEVICE_STILL_MOVING; + case SafetyReason::ControlBusy: + return api::COMMAND_REASON_CODE_CONTROL_BUSY; + case SafetyReason::GenerationMismatch: + return api::COMMAND_REASON_CODE_GENERATION_MISMATCH; + case SafetyReason::CommandIdRequired: + return api::COMMAND_REASON_CODE_COMMAND_ID_REQUIRED; + case SafetyReason::CommandIdConflict: + return api::COMMAND_REASON_CODE_COMMAND_ID_CONFLICT; + case SafetyReason::ResultEvicted: + return api::COMMAND_REASON_CODE_RESULT_EVICTED; + case SafetyReason::LedgerExhausted: + return api::COMMAND_REASON_CODE_LEDGER_EXHAUSTED; + case SafetyReason::Backpressure: + return api::COMMAND_REASON_CODE_BACKPRESSURE; + case SafetyReason::DeadlineExceededBeforeDispatch: + return api::COMMAND_REASON_CODE_DEADLINE_EXCEEDED_BEFORE_DISPATCH; + case SafetyReason::OutcomeUnknown: + return api::COMMAND_REASON_CODE_OUTCOME_UNKNOWN; + case SafetyReason::ParticipantTimeout: + return api::COMMAND_REASON_CODE_PARTICIPANT_TIMEOUT; + case SafetyReason::StopUnconfirmed: + return api::COMMAND_REASON_CODE_STOP_UNCONFIRMED; + case SafetyReason::RecoveryEpochMismatch: + return api::COMMAND_REASON_CODE_RECOVERY_EPOCH_MISMATCH; + case SafetyReason::RecoveryReasonRequired: + return api::COMMAND_REASON_CODE_RECOVERY_REASON_REQUIRED; + case SafetyReason::RecoveryAuditFailed: + return api::COMMAND_REASON_CODE_RECOVERY_AUDIT_FAILED; + case SafetyReason::InternalError: + return api::COMMAND_REASON_CODE_INTERNAL_ERROR; + } + return api::COMMAND_REASON_CODE_INTERNAL_ERROR; +} + +safety::SafetyReason fromApiSafetyReason( + const api::CommandReasonCode value) noexcept +{ + using safety::SafetyReason; + switch (value) { + case api::COMMAND_REASON_CODE_NONE: return SafetyReason::None; + case api::COMMAND_REASON_CODE_INVALID_ARGUMENT: + return SafetyReason::InvalidArgument; + case api::COMMAND_REASON_CODE_UNAUTHENTICATED: + return SafetyReason::Unauthenticated; + case api::COMMAND_REASON_CODE_PERMISSION_DENIED: + return SafetyReason::PermissionDenied; + case api::COMMAND_REASON_CODE_RECOVERY_RPC_DISABLED: + return SafetyReason::RecoveryRpcDisabled; + case api::COMMAND_REASON_CODE_DEVICE_NOT_FOUND: + return SafetyReason::DeviceNotFound; + case api::COMMAND_REASON_CODE_DEVICE_UNAVAILABLE: + return SafetyReason::DeviceUnavailable; + case api::COMMAND_REASON_CODE_UNSUPPORTED_COMMAND: + return SafetyReason::UnsupportedCommand; + case api::COMMAND_REASON_CODE_SYSTEM_STARTING: + return SafetyReason::SystemStarting; + case api::COMMAND_REASON_CODE_SYSTEM_STOPPING: + return SafetyReason::SystemStopping; + case api::COMMAND_REASON_CODE_SAFETY_LATCHED: + return SafetyReason::SafetyLatched; + case api::COMMAND_REASON_CODE_SAFETY_STATE_MISSING: + return SafetyReason::SafetyStateMissing; + case api::COMMAND_REASON_CODE_SAFETY_STATE_STALE: + return SafetyReason::SafetyStateStale; + case api::COMMAND_REASON_CODE_HARDWARE_UNSAFE: + return SafetyReason::HardwareUnsafe; + case api::COMMAND_REASON_CODE_EMERGENCY_STOP_ACTIVE: + return SafetyReason::EmergencyStopActive; + case api::COMMAND_REASON_CODE_PROTECTIVE_STOP_ACTIVE: + return SafetyReason::ProtectiveStopActive; + case api::COMMAND_REASON_CODE_DEVICE_DISCONNECTED: + return SafetyReason::DeviceDisconnected; + case api::COMMAND_REASON_CODE_DEVICE_FAULT: + return SafetyReason::DeviceFault; + case api::COMMAND_REASON_CODE_DEVICE_NOT_READY: + return SafetyReason::DeviceNotReady; + case api::COMMAND_REASON_CODE_DEVICE_STILL_MOVING: + return SafetyReason::DeviceStillMoving; + case api::COMMAND_REASON_CODE_CONTROL_BUSY: + return SafetyReason::ControlBusy; + case api::COMMAND_REASON_CODE_GENERATION_MISMATCH: + return SafetyReason::GenerationMismatch; + case api::COMMAND_REASON_CODE_COMMAND_ID_REQUIRED: + return SafetyReason::CommandIdRequired; + case api::COMMAND_REASON_CODE_COMMAND_ID_CONFLICT: + return SafetyReason::CommandIdConflict; + case api::COMMAND_REASON_CODE_RESULT_EVICTED: + return SafetyReason::ResultEvicted; + case api::COMMAND_REASON_CODE_LEDGER_EXHAUSTED: + return SafetyReason::LedgerExhausted; + case api::COMMAND_REASON_CODE_BACKPRESSURE: + return SafetyReason::Backpressure; + case api::COMMAND_REASON_CODE_DEADLINE_EXCEEDED_BEFORE_DISPATCH: + return SafetyReason::DeadlineExceededBeforeDispatch; + case api::COMMAND_REASON_CODE_OUTCOME_UNKNOWN: + return SafetyReason::OutcomeUnknown; + case api::COMMAND_REASON_CODE_PARTICIPANT_TIMEOUT: + return SafetyReason::ParticipantTimeout; + case api::COMMAND_REASON_CODE_STOP_UNCONFIRMED: + return SafetyReason::StopUnconfirmed; + case api::COMMAND_REASON_CODE_RECOVERY_EPOCH_MISMATCH: + return SafetyReason::RecoveryEpochMismatch; + case api::COMMAND_REASON_CODE_RECOVERY_REASON_REQUIRED: + return SafetyReason::RecoveryReasonRequired; + case api::COMMAND_REASON_CODE_RECOVERY_AUDIT_FAILED: + return SafetyReason::RecoveryAuditFailed; + case api::COMMAND_REASON_CODE_INTERNAL_ERROR: + case api::COMMAND_REASON_CODE_UNSPECIFIED: + return SafetyReason::InternalError; + } + return SafetyReason::InternalError; +} + +api::SafetyTriState toApiSafetyTriState( + const safety::TriState value) noexcept +{ + switch (value) { + case safety::TriState::False: return api::SAFETY_TRI_STATE_FALSE; + case safety::TriState::True: return api::SAFETY_TRI_STATE_TRUE; + case safety::TriState::Unknown: break; + } + return api::SAFETY_TRI_STATE_UNKNOWN; +} + +api::SafetyCondition toApiSafetyCondition( + const safety::SafetyCondition value) noexcept +{ + switch (value) { + case safety::SafetyCondition::Nominal: + return api::SAFETY_CONDITION_NOMINAL; + case safety::SafetyCondition::Restricted: + return api::SAFETY_CONDITION_RESTRICTED; + case safety::SafetyCondition::Unsafe: + return api::SAFETY_CONDITION_UNSAFE; + case safety::SafetyCondition::Unknown: break; + } + return api::SAFETY_CONDITION_UNKNOWN; +} + +api::SystemAdmissionState toApiSystemAdmissionState( + const safety::SystemAdmissionState value) noexcept +{ + switch (value) { + case safety::SystemAdmissionState::Starting: + return api::SYSTEM_ADMISSION_STATE_STARTING; + case safety::SystemAdmissionState::Open: + return api::SYSTEM_ADMISSION_STATE_OPEN; + case safety::SystemAdmissionState::Stopping: + return api::SYSTEM_ADMISSION_STATE_STOPPING; + case safety::SystemAdmissionState::Latched: + return api::SYSTEM_ADMISSION_STATE_LATCHED; + case safety::SystemAdmissionState::Recovering: + return api::SYSTEM_ADMISSION_STATE_RECOVERING; + case safety::SystemAdmissionState::ShuttingDown: + return api::SYSTEM_ADMISSION_STATE_SHUTTING_DOWN; + } + return api::SYSTEM_ADMISSION_STATE_UNSPECIFIED; +} + +api::DeviceAdmissionState toApiDeviceAdmissionState( + const safety::DeviceAdmissionState value) noexcept +{ + switch (value) { + case safety::DeviceAdmissionState::Observing: + return api::DEVICE_ADMISSION_STATE_OBSERVING; + case safety::DeviceAdmissionState::Open: + return api::DEVICE_ADMISSION_STATE_OPEN; + case safety::DeviceAdmissionState::Blocked: + return api::DEVICE_ADMISSION_STATE_BLOCKED; + case safety::DeviceAdmissionState::Quarantined: + return api::DEVICE_ADMISSION_STATE_QUARANTINED; + case safety::DeviceAdmissionState::Recovering: + return api::DEVICE_ADMISSION_STATE_RECOVERING; + case safety::DeviceAdmissionState::Removed: + return api::DEVICE_ADMISSION_STATE_REMOVED; + } + return api::DEVICE_ADMISSION_STATE_UNSPECIFIED; +} + +api::SafetyBlockerScope toApiSafetyBlockerScope( + const safety::BlockerScope value) noexcept +{ + return value == safety::BlockerScope::System + ? api::SAFETY_BLOCKER_SCOPE_SYSTEM + : api::SAFETY_BLOCKER_SCOPE_DEVICE; +} + +api::SafetyRecoveryRequirement toApiRecoveryRequirement( + const safety::RecoveryRequirement value) noexcept +{ + switch (value) { + case safety::RecoveryRequirement::RefreshOnly: + return api::SAFETY_RECOVERY_REQUIREMENT_REFRESH_ONLY; + case safety::RecoveryRequirement::ClearSoftwareLatch: + return api::SAFETY_RECOVERY_REQUIREMENT_CLEAR_SOFTWARE_LATCH; + case safety::RecoveryRequirement::HardwareReleaseRequired: + return api::SAFETY_RECOVERY_REQUIREMENT_HARDWARE_RELEASE_REQUIRED; + case safety::RecoveryRequirement::ManualInspectionRequired: + return api::SAFETY_RECOVERY_REQUIREMENT_MANUAL_INSPECTION_REQUIRED; + } + return api::SAFETY_RECOVERY_REQUIREMENT_UNSPECIFIED; +} + +api::SafetyOperationResult toApiRecoveryResult( + const safety::RecoveryResultCode value) noexcept +{ + switch (value) { + case safety::RecoveryResultCode::Recovered: + return api::SAFETY_OPERATION_RESULT_RECOVERED; + case safety::RecoveryResultCode::VerifiedButStillBlocked: + return api::SAFETY_OPERATION_RESULT_VERIFIED_BUT_STILL_BLOCKED; + case safety::RecoveryResultCode::BlockerRemains: + return api::SAFETY_OPERATION_RESULT_BLOCKER_REMAINS; + case safety::RecoveryResultCode::EpochMismatch: + return api::SAFETY_OPERATION_RESULT_EPOCH_MISMATCH; + case safety::RecoveryResultCode::NothingToRecover: + return api::SAFETY_OPERATION_RESULT_NOTHING_TO_RECOVER; + case safety::RecoveryResultCode::TimedOut: + return api::SAFETY_OPERATION_RESULT_TIMED_OUT; + case safety::RecoveryResultCode::Failed: + return api::SAFETY_OPERATION_RESULT_FAILED; + } + return api::SAFETY_OPERATION_RESULT_FAILED; +} + +void populateDeviceSafetyState( + const safety::DeviceSafetyStateView& source, + api::DeviceSafetyStateInfo& destination) +{ + const auto& snapshot = source.safety.snapshot; + destination.set_device_id(source.descriptor.device_id); + destination.set_device_kind(device::toString(source.descriptor.kind)); + destination.set_policy_family( + safety::toString(source.descriptor.default_policy)); + destination.set_lifecycle_state(device::toString(source.lifecycle)); + destination.set_health_state(device::toString(source.health.state)); + destination.set_admission_state( + toApiDeviceAdmissionState(source.admission_state)); + destination.set_condition(toApiSafetyCondition(snapshot.condition)); + destination.set_has_sample(source.safety.has_sample); + destination.set_snapshot_fresh(source.safety.fresh); + if (source.safety.has_sample) { + destination.set_sample_age_ms(static_cast( + source.safety.sample_age.count() < 0 + ? 0 + : source.safety.sample_age.count())); + } + destination.set_sample_sequence(snapshot.sample_sequence); + destination.set_observed_at_unix_ms(snapshot.observed_at_unix_ms); + destination.set_device_generation(snapshot.device_generation); + destination.set_connected(toApiSafetyTriState(snapshot.connected)); + destination.set_operational_ready( + toApiSafetyTriState(snapshot.operational_ready)); + destination.set_quiescent(toApiSafetyTriState(snapshot.quiescent)); + destination.set_motion_active( + toApiSafetyTriState(snapshot.motion_active)); + destination.set_actuator_enabled( + toApiSafetyTriState(snapshot.actuator_enabled)); + destination.set_emergency_stop_active( + toApiSafetyTriState(snapshot.emergency_stop_active)); + destination.set_protective_stop_active( + toApiSafetyTriState(snapshot.protective_stop_active)); + destination.set_fault_active( + toApiSafetyTriState(snapshot.fault_active)); + + for (const auto& blocker : source.blockers) { + auto* target = destination.add_blockers(); + target->set_reason_code(toApiSafetyReason(blocker.reason)); + target->set_scope(toApiSafetyBlockerScope(blocker.scope)); + target->set_recovery_requirement( + toApiRecoveryRequirement(blocker.recovery_requirement)); + target->set_source_id(blocker.source_id); + target->set_operation_id(blocker.operation_id); + target->set_first_observed_at_unix_ms( + blocker.first_observed_at_unix_ms); + target->set_last_observed_at_unix_ms( + blocker.last_observed_at_unix_ms); + } +} + +void populateSafetyTargetResult( + const safety::SafetyTargetResult& source, + api::SafetyOperationTargetResult& destination) +{ + destination.set_target_id(source.target_id); + destination.set_result( + source.success ? api::SAFETY_OPERATION_RESULT_SUCCEEDED + : api::SAFETY_OPERATION_RESULT_FAILED); + destination.set_reason_code(toApiSafetyReason(source.reason)); + destination.set_detail(source.detail); + destination.set_before_state( + toApiDeviceAdmissionState(source.before_state)); + destination.set_after_state( + toApiDeviceAdmissionState(source.after_state)); +} + +void populateSafetyParticipantState( + const safety::ParticipantSafetyStateView& source, + api::SafetyParticipantStateInfo& destination) +{ + destination.set_participant_id(source.descriptor.participant_id); + destination.set_phase(participantPhaseName(source.descriptor.phase)); + destination.set_required(source.descriptor.required); + destination.set_registered(source.registered); + destination.set_barrier_active(source.barrier_active); + destination.set_barrier_retained(source.barrier_retained); + destination.set_operation_id(source.operation_id); + destination.set_safety_epoch(source.safety_epoch); + populateParticipantResult( + source.last_request, *destination.mutable_last_request()); + populateParticipantResult( + source.last_verify, *destination.mutable_last_verify()); + populateParticipantResult( + source.last_release, *destination.mutable_last_release()); +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/src/grpc_security.cpp b/cmvr-es/service/grpc/src/grpc_security.cpp new file mode 100644 index 00000000..c802037a --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_security.cpp @@ -0,0 +1,688 @@ +#include "service/grpc/include/grpc_security.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cmvr::service { + +namespace { + +constexpr std::size_t kMaxCorrelationIdLength = 64; +std::atomic next_correlation_id{1}; + +bool isSafeCorrelationId(const std::string& value) +{ + if (value.empty() || value.size() > kMaxCorrelationIdLength) { + return false; + } + return std::all_of( + value.begin(), value.end(), [](const unsigned char character) { + return std::isalnum(character) || character == '-' || + character == '_' || character == '.' || character == ':'; + }); +} + +std::string makeCorrelationId() +{ + const auto sequence = + next_correlation_id.fetch_add(1, std::memory_order_relaxed); + const auto now = std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()).count(); + std::ostringstream output; + output << "grpc-" << std::hex << now << '-' << sequence; + return output.str(); +} + +std::string correlationIdFrom( + const std::multimap& metadata) +{ + const auto range = metadata.equal_range("x-correlation-id"); + if (range.first != range.second && + std::next(range.first) == range.second && + isSafeCorrelationId(range.first->second)) { + return range.first->second; + } + return makeCorrelationId(); +} + +bool roleAllows(const GrpcPrincipal& principal, const GrpcRole required) +{ + const auto rank = [](const GrpcRole role) { + switch (role) { + case GrpcRole::Anonymous: return 0; + case GrpcRole::Observer: return 1; + case GrpcRole::Operator: return 2; + case GrpcRole::SafetyAdmin: return 3; + } + return -1; + }; + return std::any_of( + principal.roles.begin(), principal.roles.end(), + [&](const GrpcRole role) { return rank(role) >= rank(required); }); +} + +GrpcCallFacts factsFrom( + grpc::ServerContext* context, + const GrpcMethodPolicy& method, + const bool encrypted) +{ + GrpcCallFacts facts; + facts.full_method_name = method.full_method_name; + facts.transport_encrypted = encrypted; + facts.received_at = std::chrono::steady_clock::now(); + facts.deadline = std::chrono::steady_clock::time_point::max(); + + if (context) { + facts.peer = context->peer(); + for (const auto& [key, value] : context->client_metadata()) { + facts.metadata.emplace( + std::string(key.data(), key.size()), + std::string(value.data(), value.size())); + } + + const auto deadline = context->deadline(); + if (deadline != std::chrono::system_clock::time_point::max()) { + const auto remaining = deadline - std::chrono::system_clock::now(); + facts.deadline = remaining <= decltype(remaining)::zero() + ? facts.received_at + : facts.received_at + + std::chrono::duration_cast< + std::chrono::steady_clock::duration>( + remaining); + } + } + + facts.local_peer = isLocalGrpcPeer(facts.peer); + facts.correlation_id = correlationIdFrom(facts.metadata); + return facts; +} + +} // namespace + +bool isLoopbackGrpcHost(const std::string& host) noexcept +{ + return host == "127.0.0.1" || host == "localhost" || host == "::1" || + host == "[::1]" || host.rfind("unix:", 0) == 0; +} + +bool isLocalGrpcPeer(const std::string& peer) noexcept +{ + return peer.rfind("unix:", 0) == 0 || + peer.rfind("ipv4:127.", 0) == 0 || + peer == "ipv6:[::1]" || peer == "ipv6:::1"; +} + +const char* toString(const GrpcTransportSecurity value) noexcept +{ + switch (value) { + case GrpcTransportSecurity::Insecure: return "insecure"; + case GrpcTransportSecurity::ServerTls: return "server_tls"; + case GrpcTransportSecurity::MutualTls: return "mutual_tls"; + } + return "unknown"; +} + +const char* toString(const GrpcAuthenticationMethod value) noexcept +{ + switch (value) { + case GrpcAuthenticationMethod::Disabled: return "disabled"; + case GrpcAuthenticationMethod::StaticToken: return "static_token"; + case GrpcAuthenticationMethod::Jwt: return "jwt"; + case GrpcAuthenticationMethod::TlsClientCertificate: + return "tls_client_certificate"; + } + return "unknown"; +} + +const char* toString(const GrpcRecoveryExposure value) noexcept +{ + switch (value) { + case GrpcRecoveryExposure::Disabled: return "disabled"; + case GrpcRecoveryExposure::LocalOnly: return "local_only"; + case GrpcRecoveryExposure::Authorized: return "authorized"; + } + return "unknown"; +} + +const char* toString(const GrpcRole value) noexcept +{ + switch (value) { + case GrpcRole::Anonymous: return "anonymous"; + case GrpcRole::Observer: return "observer"; + case GrpcRole::Operator: return "operator"; + case GrpcRole::SafetyAdmin: return "safety_admin"; + } + return "unknown"; +} + +GrpcSecurityConfigResult resolveGrpcSecurityConfig( + const config::GRPCServerConfig& server_config, + const std::string& effective_host) +{ + GrpcSecurityConfigResult result; + const bool loopback = isLoopbackGrpcHost(effective_host); + + if (!server_config.has_security()) { + result.valid = true; + result.config.transport = GrpcTransportSecurity::Insecure; + result.config.authentication = GrpcAuthenticationMethod::Disabled; + result.config.recovery_exposure = GrpcRecoveryExposure::Disabled; + result.config.allow_insecure_non_loopback = !loopback; + result.config.insecure_non_loopback = !loopback; + result.config.legacy_compatibility = true; + result.warnings.emplace_back( + "missing grpc security config: using one-release legacy " + "INSECURE/DISABLED compatibility with recovery disabled"); + return result; + } + + const auto& security = server_config.security(); + if (security.transport_mode() != config::GRPCSecurityConfig::INSECURE) { + result.error = + "only INSECURE gRPC transport is compiled in this release"; + return result; + } + result.config.transport = GrpcTransportSecurity::Insecure; + + if (security.authentication_mode() != + config::GRPCSecurityConfig::DISABLED) { + result.error = + "only DISABLED gRPC authentication is compiled in this release"; + return result; + } + result.config.authentication = GrpcAuthenticationMethod::Disabled; + + switch (security.recovery_exposure()) { + case config::GRPCSecurityConfig::RECOVERY_DISABLED: + result.config.recovery_exposure = GrpcRecoveryExposure::Disabled; + break; + case config::GRPCSecurityConfig::RECOVERY_LOCAL_ONLY: + result.config.recovery_exposure = GrpcRecoveryExposure::LocalOnly; + break; + case config::GRPCSecurityConfig::RECOVERY_AUTHORIZED: + result.error = + "RECOVERY_AUTHORIZED requires an implemented authentication " + "provider"; + return result; + case config::GRPCSecurityConfig::RECOVERY_EXPOSURE_UNSPECIFIED: + default: + result.error = "grpc recovery exposure must be explicit"; + return result; + } + result.config.recovery_audit_file = security.audit_file(); + if (result.config.recovery_exposure != + GrpcRecoveryExposure::Disabled && + result.config.recovery_audit_file.empty()) { + result.error = + "an enabled recovery RPC requires a persistent audit_file"; + return result; + } + + result.config.allow_insecure_non_loopback = + security.allow_insecure_non_loopback(); + result.config.insecure_non_loopback = !loopback; + if (!loopback && !security.allow_insecure_non_loopback()) { + result.error = + "INSECURE/DISABLED gRPC on a non-loopback host requires " + "allow_insecure_non_loopback=true"; + return result; + } + if (!loopback) { + result.warnings.emplace_back( + "gRPC is listening without transport encryption or client " + "authentication on a non-loopback host"); + } + + result.valid = true; + return result; +} + +GrpcAuthenticationResult DisabledGrpcAuthenticationProvider::authenticate( + const GrpcCallFacts&) const +{ + GrpcAuthenticationResult result; + result.principal.id = "anonymous"; + result.principal.method = GrpcAuthenticationMethod::Disabled; + result.principal.authenticated = false; + result.principal.roles = {GrpcRole::Anonymous}; + result.status = grpc::Status::OK; + return result; +} + +CompatibilityGrpcAuthorizationPolicy::CompatibilityGrpcAuthorizationPolicy( + const GrpcRecoveryExposure recovery_exposure) + : recovery_exposure_(recovery_exposure) +{ +} + +GrpcAuthorizationDecision CompatibilityGrpcAuthorizationPolicy::authorize( + const GrpcRequestContext& context, + const GrpcMethodPolicy& method) const +{ + if (method.access == GrpcAccessClass::Recover) { + switch (recovery_exposure_) { + case GrpcRecoveryExposure::Disabled: + return { + false, + grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, + "RECOVERY_RPC_DISABLED")}; + case GrpcRecoveryExposure::LocalOnly: + if (!context.local_peer) { + return { + false, + grpc::Status( + grpc::StatusCode::PERMISSION_DENIED, + "RecoverSafetyState is restricted to a local peer")}; + } + return {true, grpc::Status::OK}; + case GrpcRecoveryExposure::Authorized: + if (!context.principal.authenticated || + !roleAllows(context.principal, GrpcRole::SafetyAdmin)) { + return { + false, + grpc::Status( + context.principal.authenticated + ? grpc::StatusCode::PERMISSION_DENIED + : grpc::StatusCode::UNAUTHENTICATED, + "RecoverSafetyState requires SafetyAdmin")}; + } + return {true, grpc::Status::OK}; + } + } + + if (!context.principal.authenticated && + context.principal.method == GrpcAuthenticationMethod::Disabled) { + return {true, grpc::Status::OK}; + } + if (!context.principal.authenticated) { + return { + false, + grpc::Status( + grpc::StatusCode::UNAUTHENTICATED, + "gRPC caller authentication failed")}; + } + if (!roleAllows(context.principal, method.minimum_role)) { + return { + false, + grpc::Status( + grpc::StatusCode::PERMISSION_DENIED, + "gRPC caller does not have the required role")}; + } + return {true, grpc::Status::OK}; +} + +bool GrpcMethodPolicyRegistry::registerPolicy(GrpcMethodPolicy policy) +{ + if (policy.full_method_name.empty()) { + return false; + } + return policies_.emplace(policy.full_method_name, std::move(policy)).second; +} + +std::optional GrpcMethodPolicyRegistry::find( + const std::string& full_method_name) const +{ + const auto found = policies_.find(full_method_name); + if (found == policies_.end()) { + return std::nullopt; + } + return found->second; +} + +std::vector GrpcMethodPolicyRegistry::snapshot() const +{ + std::vector result; + result.reserve(policies_.size()); + for (const auto& [name, policy] : policies_) { + (void)name; + result.push_back(policy); + } + std::sort( + result.begin(), result.end(), + [](const auto& lhs, const auto& rhs) { + return lhs.full_method_name < rhs.full_method_name; + }); + return result; +} + +const GrpcMethodPolicyRegistry& defaultGrpcMethodPolicyRegistry() +{ + static const GrpcMethodPolicyRegistry registry = [] { + GrpcMethodPolicyRegistry result; + const auto add = [&result]( + const char* service, + const char* method, + const GrpcAccessClass access, + const safety::CommandIntent intent, + const safety::SafetyPolicyFamily family, + const bool mutating, + const bool safety_lane = false) { + GrpcRole role = GrpcRole::Observer; + if (access == GrpcAccessClass::Mutate || + access == GrpcAccessClass::Stop) { + role = GrpcRole::Operator; + } else if (access == GrpcAccessClass::Recover) { + role = GrpcRole::SafetyAdmin; + } + GrpcMethodPolicy policy; + policy.full_method_name = + std::string("/cmvr.api.") + service + '/' + method; + policy.access = access; + policy.minimum_role = role; + policy.command_intent = intent; + policy.policy_family = family; + policy.mutating = mutating; + policy.safety_lane = safety_lane; + if (!result.registerPolicy(std::move(policy))) { + throw std::logic_error( + std::string("duplicate gRPC method policy: ") + + service + '/' + method); + } + }; + const auto add_many = [&add]( + const char* service, + const std::initializer_list methods, + const GrpcAccessClass access, + const safety::CommandIntent intent, + const safety::SafetyPolicyFamily family, + const bool mutating, + const bool safety_lane = false) { + for (const auto* method : methods) { + add(service, method, access, intent, family, mutating, + safety_lane); + } + }; + + using safety::CommandIntent; + using safety::SafetyPolicyFamily; + add_many("SystemService", + {"GetSystemInfo", "GetSystemStatus", "GetDeviceList", + "GetSafetyState"}, + GrpcAccessClass::Read, CommandIntent::Observe, + SafetyPolicyFamily::Sensor, false); + add("SystemService", "UpdateParams", GrpcAccessClass::Mutate, + CommandIntent::Configure, SafetyPolicyFamily::Sensor, true); + add("SystemService", "StopAll", GrpcAccessClass::Stop, + CommandIntent::Stop, SafetyPolicyFamily::Control, true, true); + add("SystemService", "ExecuteActionQueue", GrpcAccessClass::Mutate, + CommandIntent::Actuate, SafetyPolicyFamily::Control, true); + add("SystemService", "RecoverSafetyState", GrpcAccessClass::Recover, + CommandIntent::RecoverAdmission, SafetyPolicyFamily::Control, + true, true); + + add_many("ArmService", + {"getJointState", "getPose", "getPoseMatrix", + "computeForwardKinematics"}, + GrpcAccessClass::Read, CommandIntent::Observe, + SafetyPolicyFamily::Control, false); + add_many("ArmService", {"torqueOff", "stopMotion"}, + GrpcAccessClass::Stop, CommandIntent::Stop, + SafetyPolicyFamily::Control, true, true); + add("ArmService", "clearFault", GrpcAccessClass::Mutate, + CommandIntent::ResetFault, SafetyPolicyFamily::Control, true); + add("ArmService", "torqueOn", GrpcAccessClass::Mutate, + CommandIntent::StartActivity, SafetyPolicyFamily::Control, true); + add_many("ArmService", + {"moveJ", "moveL", "speedJ", "speedL", "servoJ"}, + GrpcAccessClass::Mutate, CommandIntent::Actuate, + SafetyPolicyFamily::Control, true); + add_many("ArmService", {"calibrateZeroQ", "ExecuteJsonCommand"}, + GrpcAccessClass::Mutate, CommandIntent::Configure, + SafetyPolicyFamily::Control, true); + add("armteleop.v1.ArmTeleopService", "Teleoperate", + GrpcAccessClass::Mutate, + CommandIntent::Actuate, SafetyPolicyFamily::Control, true); + + add_many("AgvService", + {"getRuntimeState", "getNavigationStatus", "listMaps", + "listStations", "downloadMap", "streamMap"}, + GrpcAccessClass::Read, CommandIntent::Observe, + SafetyPolicyFamily::Control, false); + add_many("AgvService", + {"emergencyStop", "pauseNavigation", "cancelNavigation", + "stopVelocityControl", "stopMapping"}, + GrpcAccessClass::Stop, CommandIntent::Stop, + SafetyPolicyFamily::Control, true, true); + add("AgvService", "clearFault", GrpcAccessClass::Mutate, + CommandIntent::ResetFault, SafetyPolicyFamily::Control, true); + add("AgvService", "resumeNavigation", GrpcAccessClass::Mutate, + CommandIntent::StartActivity, SafetyPolicyFamily::Control, true); + add_many("AgvService", + {"navigateToPose", "navigateToStation", "followPath", + "setVelocity", "translate"}, + GrpcAccessClass::Mutate, CommandIntent::Actuate, + SafetyPolicyFamily::Control, true); + add_many("AgvService", + {"switchMap", "uploadMap", "startMapping"}, + GrpcAccessClass::Mutate, CommandIntent::Configure, + SafetyPolicyFamily::Control, true); + + add("MotorService", "getStatus", GrpcAccessClass::Read, + CommandIntent::Observe, SafetyPolicyFamily::Control, false); + add("MotorService", "emergencyStop", GrpcAccessClass::Stop, + CommandIntent::Stop, SafetyPolicyFamily::Control, true, true); + add_many("MotorService", + {"moveToZero", "profilePosition", "profileVelocity", + "streamCyclicPosition", "streamCyclicVelocity"}, + GrpcAccessClass::Mutate, CommandIntent::Actuate, + SafetyPolicyFamily::Control, true); + add_many("MotorService", {"setZero", "setEnabled"}, + GrpcAccessClass::Mutate, CommandIntent::Configure, + SafetyPolicyFamily::Control, true); + + add_many("DexHandService", + {"GetStatus", "GetSensorData", "GetSensorDataStream"}, + GrpcAccessClass::Read, CommandIntent::Observe, + SafetyPolicyFamily::Control, false); + add_many("DexHandService", + {"SetDexHandPos", "SetDexHandAngle", "SetDexHandForce", + "SetDexHandSpeed", "SetDexHandPresetAct"}, + GrpcAccessClass::Mutate, CommandIntent::Actuate, + SafetyPolicyFamily::Control, true); + + add_many("CameraService", + {"GetStatus", "GetRGBImage", "GetDepthImage", + "GetRGBDImages", "GetRGBImageStream", + "GetDepthImageStream", "GetRGBDImagesStream"}, + GrpcAccessClass::Read, CommandIntent::Observe, + SafetyPolicyFamily::Sensor, false); + add_many("CameraService", {"StartCamera", "StartRecording"}, + GrpcAccessClass::Mutate, CommandIntent::StartActivity, + SafetyPolicyFamily::Sensor, true); + add_many("CameraService", {"StopCamera", "StopRecording"}, + GrpcAccessClass::Stop, CommandIntent::Stop, + SafetyPolicyFamily::Sensor, true, true); + add("CameraService", "ControlPtz", GrpcAccessClass::Mutate, + CommandIntent::Actuate, SafetyPolicyFamily::Control, true); + + add_many("MicPhoneService", {"GetStatus", "StreamAudio", "GetVolume"}, + GrpcAccessClass::Read, CommandIntent::Observe, + SafetyPolicyFamily::Sensor, false); + add_many("MicPhoneService", {"StartRecord", "ResumeRecord"}, + GrpcAccessClass::Mutate, CommandIntent::StartActivity, + SafetyPolicyFamily::Sensor, true); + add_many("MicPhoneService", {"StopRecord", "PauseRecord"}, + GrpcAccessClass::Stop, CommandIntent::Stop, + SafetyPolicyFamily::Sensor, true, true); + add("MicPhoneService", "SetVolume", GrpcAccessClass::Mutate, + CommandIntent::Configure, SafetyPolicyFamily::Sensor, true); + + add_many("SpeakerService", {"GetStatus", "GetVolume"}, + GrpcAccessClass::Read, CommandIntent::Observe, + SafetyPolicyFamily::Sensor, false); + add_many("SpeakerService", + {"PlayAudio", "StreamAudio", "ResumePlayback"}, + GrpcAccessClass::Mutate, CommandIntent::StartActivity, + SafetyPolicyFamily::Sensor, true); + add_many("SpeakerService", {"StopPlayback", "PausePlayback"}, + GrpcAccessClass::Stop, CommandIntent::Stop, + SafetyPolicyFamily::Sensor, true, true); + add("SpeakerService", "SetVolume", GrpcAccessClass::Mutate, + CommandIntent::Configure, SafetyPolicyFamily::Sensor, true); + + add("BioHeadService", "GetSystemStatus", GrpcAccessClass::Read, + CommandIntent::Observe, SafetyPolicyFamily::Control, false); + add_many("BioHeadService", {"EmergencyStop", "SpeakStop"}, + GrpcAccessClass::Stop, CommandIntent::Stop, + SafetyPolicyFamily::Control, true, true); + add_many("BioHeadService", + {"SetExpression", "StreamExpression", "SpeakStart", + "Happy", "Surprise", "ExpressionTired", + "ExpressionAngry", "ExpressionSadness", + "ExpressionYawn"}, + GrpcAccessClass::Mutate, CommandIntent::Actuate, + SafetyPolicyFamily::Control, true); + + add("HlcService", "touch", GrpcAccessClass::Mutate, + CommandIntent::Actuate, SafetyPolicyFamily::Control, true); + add("TestService", "Call", GrpcAccessClass::Read, + CommandIntent::Observe, SafetyPolicyFamily::Sensor, false); + return result; + }(); + return registry; +} + +GrpcCallGuard::GrpcCallGuard( + GrpcRequestContext context, + GrpcAuthorizationDecision decision) + : context_(std::move(context)), decision_(std::move(decision)) +{ +} + +GrpcSecurityGateway::GrpcSecurityGateway( + GrpcSecurityRuntimeConfig config, + std::shared_ptr authentication, + std::shared_ptr authorization, + GrpcSecurityAuditSink audit_sink) + : config_(std::move(config)), + authentication_(std::move(authentication)), + authorization_(std::move(authorization)), + audit_sink_(std::move(audit_sink)) +{ +} + +GrpcCallGuard GrpcSecurityGateway::beginCall( + grpc::ServerContext* server_context, + const GrpcMethodPolicy& method) const +{ + return beginCall( + factsFrom( + server_context, method, + config_.transport != GrpcTransportSecurity::Insecure), + method); +} + +GrpcCallGuard GrpcSecurityGateway::beginCall( + GrpcCallFacts facts, + const GrpcMethodPolicy& method) const +{ + if (facts.full_method_name.empty()) { + facts.full_method_name = method.full_method_name; + } + if (facts.received_at == std::chrono::steady_clock::time_point{}) { + facts.received_at = std::chrono::steady_clock::now(); + } + if (facts.deadline == std::chrono::steady_clock::time_point{}) { + facts.deadline = std::chrono::steady_clock::time_point::max(); + } + if (facts.correlation_id.empty()) { + facts.correlation_id = correlationIdFrom(facts.metadata); + } + if (!facts.local_peer) { + facts.local_peer = isLocalGrpcPeer(facts.peer); + } + + const auto authentication = authentication_->authenticate(facts); + GrpcRequestContext context{ + facts.correlation_id, + facts.full_method_name, + facts.peer, + authentication.principal, + facts.transport_encrypted, + facts.local_peer, + facts.received_at, + facts.deadline}; + + GrpcAuthorizationDecision decision; + if (!authentication.ok()) { + decision = {false, authentication.status}; + } else { + decision = authorization_->authorize(context, method); + } + + if (audit_sink_) { + audit_sink_(GrpcSecurityAuditRecord{ + context.correlation_id, + context.full_method_name, + context.principal.id, + context.peer, + context.principal.method, + method.access, + context.principal.authenticated, + decision.allowed, + decision.status.error_code()}); + } + return GrpcCallGuard(std::move(context), std::move(decision)); +} + +std::shared_ptr makeGrpcSecurityGateway( + const GrpcSecurityRuntimeConfig& config, + GrpcSecurityAuditSink audit_sink) +{ + return std::make_shared( + config, + std::make_shared(), + std::make_shared( + config.recovery_exposure), + std::move(audit_sink)); +} + +std::shared_ptr makeDefaultGrpcSecurityGateway() +{ + GrpcSecurityRuntimeConfig config; + config.transport = GrpcTransportSecurity::Insecure; + config.authentication = GrpcAuthenticationMethod::Disabled; + config.recovery_exposure = GrpcRecoveryExposure::Disabled; + config.legacy_compatibility = true; + return makeGrpcSecurityGateway(config); +} + +GrpcCallGuard beginRegisteredGrpcCall( + const std::shared_ptr& gateway, + grpc::ServerContext* server_context, + const std::string& full_method_name) +{ + const auto policy = + defaultGrpcMethodPolicyRegistry().find(full_method_name); + if (!policy.has_value()) { + GrpcRequestContext context; + context.full_method_name = full_method_name; + context.received_at = std::chrono::steady_clock::now(); + context.deadline = std::chrono::steady_clock::time_point::max(); + if (server_context) { + context.peer = server_context->peer(); + context.local_peer = isLocalGrpcPeer(context.peer); + } + return GrpcCallGuard( + std::move(context), + {false, + grpc::Status( + grpc::StatusCode::INTERNAL, + "gRPC method has no registered security policy")}); + } + const auto active_gateway = + gateway ? gateway : makeDefaultGrpcSecurityGateway(); + return active_gateway->beginCall(server_context, *policy); +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/src/grpc_speaker_service.cpp b/cmvr-es/service/grpc/src/grpc_speaker_service.cpp index f80a257f..eb113ab4 100644 --- a/cmvr-es/service/grpc/src/grpc_speaker_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_speaker_service.cpp @@ -1,6 +1,10 @@ #include "common/base/logging/logger.h" +#include "service/grpc/include/grpc_command_transaction.h" #include "service/grpc/include/media_activity_coordinator.h" +#include "service/grpc/include/grpc_security.h" #include +#include +#include // // Created by xtkuang on 2025/6/10. // @@ -94,10 +98,20 @@ private: }; } -gRPCSpeakerServiceImpl::gRPCSpeakerServiceImpl(): dmgr_(DeviceManager::getInstance()) {} +gRPCSpeakerServiceImpl::gRPCSpeakerServiceImpl() + : gRPCSpeakerServiceImpl(makeDefaultGrpcSecurityGateway()) {} + +gRPCSpeakerServiceImpl::gRPCSpeakerServiceImpl( + std::shared_ptr security_gateway) + : dmgr_(DeviceManager::getInstance()), + security_gateway_(security_gateway + ? std::move(security_gateway) + : makeDefaultGrpcSecurityGateway()) {} grpc::Status gRPCSpeakerServiceImpl::GetStatus(grpc::ServerContext* context, const api::GetSpeakerStateCommand_Request* request, api::GetSpeakerStateCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.SpeakerService/GetStatus"); try { string dev_id = request->header().device_id(); //CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (GetStatus): id=" << dev_id; @@ -132,12 +146,15 @@ grpc::Status gRPCSpeakerServiceImpl::GetStatus(grpc::ServerContext* context, grpc::Status gRPCSpeakerServiceImpl::PlayAudio(grpc::ServerContext* context, const api::PlayAudioCommand_Request* request, api::PlayAudioCommand_Feedback* response) { - auto media_session = globalMediaActivityCoordinator().beginSession(); - if (!media_session) { - return failResponse( - response, "Media activities are temporarily paused by StopAll"); - } - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.SpeakerService/PlayAudio", request, response, + [this, request, response](GrpcCommandTransaction& command) { + auto media_session = globalMediaActivityCoordinator().beginSession(); + if (!media_session) { + return failResponse( + response, "Media activities are temporarily paused by StopAll"); + } string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (PlayAudio): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); @@ -148,29 +165,32 @@ grpc::Status gRPCSpeakerServiceImpl::PlayAudio(grpc::ServerContext* context, return failResponse( response, "Speaker is already controlled by another media session: " + dev_id); } + bool dispatch_allowed = false; if (!media_session.runIfCurrent([&] { - dev->play(request->audio_path()); + dispatch_allowed = command.beginDispatch(); + if (dispatch_allowed) { + dev->play(request->audio_path()); + } })) { return failResponse( response, "Speaker playback start was canceled by StopAll"); } + if (!dispatch_allowed) { + return command.dispatchStatus(); + } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (PlayAudio): success, id=" << dev_id << ", path=" << request->audio_path(); return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCSpeakerServiceImpl::StreamAudio(grpc::ServerContext* context, grpc::ServerReader* reader, api::StreamSpeakerAudioCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.SpeakerService/StreamAudio"); auto media_session = globalMediaActivityCoordinator().beginSession( [context] { context->TryCancel(); }); if (!media_session) { @@ -181,6 +201,7 @@ grpc::Status gRPCSpeakerServiceImpl::StreamAudio(grpc::ServerContext* context, api::StreamSpeakerAudioCommand_Request request; std::shared_ptr dev; std::unique_ptr stream_lease; + std::optional safety_session; std::string dev_id; while (reader->Read(&request)) { @@ -201,11 +222,54 @@ grpc::Status gRPCSpeakerServiceImpl::StreamAudio(grpc::ServerContext* context, "Speaker is already controlled by another media session: " + dev_id); } stream_lease = std::make_unique(dev); + + const auto& header = request.header(); + GrpcStreamingSafetyOpen safety_open; + safety_open.full_method_name = + "/cmvr.api.SpeakerService/StreamAudio"; + safety_open.device_id = dev_id; + safety_open.session_id = header.command_id().empty() + ? cmvr_grpc_call_guard.context().correlation_id + : header.command_id(); + safety_open.expected_service_instance_id = + header.expected_service_instance_id(); + if (header.has_expected_device_generation()) { + safety_open.expected_device_generation = + header.expected_device_generation(); + } + safety_open.deadline = + cmvr_grpc_call_guard.context().deadline; + safety_session.emplace( + dmgr_.safetyCoordinator(), + cmvr_grpc_call_guard.context(), + std::move(safety_open)); + if (!safety_session->admitted()) { + return safety_session->status(); + } + } else if (!request.header().device_id().empty() && + request.header().device_id() != dev_id) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "speaker stream cannot change device_id after its first frame"); + } + + if (!safety_session || !safety_session->revalidate()) { + return safety_session + ? safety_session->status() + : grpc::Status( + grpc::StatusCode::INTERNAL, + "speaker stream safety session was not initialized"); } const auto frame = fromProtoAudioData(request.audio()); bool pushed = false; + grpc::Status dispatch_status = grpc::Status::OK; const bool push_allowed = media_session.runIfCurrent([&] { + auto dispatch = safety_session->beginDispatch(); + if (!dispatch.acquired()) { + dispatch_status = safety_session->status(); + return; + } if (!frame.data.empty()) { stream_lease->arm(); } @@ -214,6 +278,9 @@ grpc::Status gRPCSpeakerServiceImpl::StreamAudio(grpc::ServerContext* context, if (!push_allowed || media_session.cancelled()) { return mediaStoppedStatus(); } + if (!dispatch_status.ok()) { + return dispatch_status; + } if (!pushed) { return failResponse(response, "Failed to push speaker audio frame: " + dev_id); } @@ -239,13 +306,19 @@ grpc::Status gRPCSpeakerServiceImpl::StreamAudio(grpc::ServerContext* context, grpc::Status gRPCSpeakerServiceImpl::StopPlayback(grpc::ServerContext* context, const api::StopSpeakerCommand_Request* request, api::StopSpeakerCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.SpeakerService/StopPlayback", request, response, + [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (StopPlayback): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); if (!dev) { return failResponse(response, "Speaker device not found: " + dev_id); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } if (!dev->stopPlayback()) { return failResponse(response, "Failed to stop speaker: " + dev_id); } @@ -253,46 +326,43 @@ grpc::Status gRPCSpeakerServiceImpl::StopPlayback(grpc::ServerContext* context, setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (StopPlayback): success, id=" << dev_id; return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCSpeakerServiceImpl::PausePlayback(grpc::ServerContext* context, const api::PauseSpeakerCommand_Request* request, api::PauseSpeakerCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.SpeakerService/PausePlayback", request, response, + [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (PausePlayback): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); if (!dev) { return failResponse(response, "Speaker device not found: " + dev_id); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } dev->pause(); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (PausePlayback): success, id=" << dev_id; return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCSpeakerServiceImpl::ResumePlayback(grpc::ServerContext* context, const api::ResumeSpeakerCommand_Request* request, api::ResumeSpeakerCommand_Feedback* response) { - auto media_session = globalMediaActivityCoordinator().beginSession(); - if (!media_session) { - return failResponse( - response, "Media activities are temporarily paused by StopAll"); - } - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.SpeakerService/ResumePlayback", request, response, + [this, request, response](GrpcCommandTransaction& command) { + auto media_session = globalMediaActivityCoordinator().beginSession(); + if (!media_session) { + return failResponse( + response, "Media activities are temporarily paused by StopAll"); + } string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (ResumePlayback): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); @@ -303,49 +373,54 @@ grpc::Status gRPCSpeakerServiceImpl::ResumePlayback(grpc::ServerContext* context return failResponse( response, "Speaker is already controlled by another media session: " + dev_id); } - if (!media_session.runIfCurrent([&] { dev->resume(); })) { + bool dispatch_allowed = false; + if (!media_session.runIfCurrent([&] { + dispatch_allowed = command.beginDispatch(); + if (dispatch_allowed) { + dev->resume(); + } + })) { return failResponse( response, "Speaker playback resume was canceled by StopAll"); } + if (!dispatch_allowed) { + return command.dispatchStatus(); + } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (ResumePlayback): success, id=" << dev_id; return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCSpeakerServiceImpl::SetVolume(grpc::ServerContext* context, const api::SetSpeakerVolumeCommand_Request* request, api::SetSpeakerVolumeCommand_Feedback* response) { - try { + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyCoordinator(), + "/cmvr.api.SpeakerService/SetVolume", request, response, + [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (SetVolume): id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); if (!dev) { return failResponse(response, "Speaker device not found: " + dev_id); } + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } dev->setVolume(request->volume()); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (SetVolume): success, id=" << dev_id << ", volume=" << request->volume(); return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } + }); } grpc::Status gRPCSpeakerServiceImpl::GetVolume(grpc::ServerContext* context, const api::GetSpeakerVolumeCommand_Request* request, api::GetSpeakerVolumeCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.SpeakerService/GetVolume"); try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (GetVolume): id=" << dev_id; diff --git a/cmvr-es/service/grpc/src/grpc_system_service.cpp b/cmvr-es/service/grpc/src/grpc_system_service.cpp index d39563fa..d3c57513 100644 --- a/cmvr-es/service/grpc/src/grpc_system_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_system_service.cpp @@ -33,6 +33,10 @@ #include "service/action/include/action_queue_executor.h" #include "service/grpc/include/camera_operational_activity_registry.h" #include "service/grpc/include/camera_ptz_activity_registry.h" +#include "service/grpc/include/grpc_recovery_audit.h" +#include "service/grpc/include/grpc_safety_proto.h" +#include "service/grpc/include/grpc_safety_participants.h" +#include "service/grpc/include/grpc_security.h" #include "service/grpc/include/media_activity_coordinator.h" #include "service/grpc/include/motor_activity_coordinator.h" #include "service/stop_all/include/stop_all_admission_gate.h" @@ -1035,31 +1039,141 @@ cmvr::api::SystemDeviceHealth toApiDeviceHealth( return cmvr::api::SYSTEM_DEVICE_HEALTH_UNSPECIFIED; } +struct ParsedSafetyScope final { + bool valid{false}; + bool all_devices{false}; + std::vector device_ids; + std::string error; +}; + +ParsedSafetyScope parseSafetyScope( + const cmvr::api::SafetyScope& scope, + const bool default_to_all) +{ + ParsedSafetyScope result; + switch (scope.target_case()) { + case cmvr::api::SafetyScope::kAllDevices: + if (!scope.all_devices()) { + result.error = "all_devices must be explicitly true"; + return result; + } + result.valid = true; + result.all_devices = true; + return result; + case cmvr::api::SafetyScope::kDevices: { + if (scope.devices().device_ids().empty()) { + result.error = "device scope must contain at least one device ID"; + return result; + } + std::unordered_set unique; + result.device_ids.reserve(scope.devices().device_ids_size()); + for (const auto& id : scope.devices().device_ids()) { + if (id.empty() || !unique.insert(id).second) { + result.error = + "device scope IDs must be non-empty and unique"; + return result; + } + result.device_ids.push_back(id); + } + result.valid = true; + return result; + } + case cmvr::api::SafetyScope::TARGET_NOT_SET: + if (default_to_all) { + result.valid = true; + result.all_devices = true; + } else { + result.error = "an explicit recovery scope is required"; + } + return result; + } + result.error = "invalid safety scope"; + return result; +} + +void setSafetyHeaderFailure( + cmvr::api::CommandHeader_Feedback* header, + const cmvr::safety::SafetyReason reason, + const std::string& detail) +{ + header->set_success(false); + header->set_reason_code(toApiSafetyReason(reason)); + header->set_error_message(detail); + header->set_execution_state( + cmvr::api::COMMAND_EXECUTION_STATE_REJECTED_BEFORE_DISPATCH); + setCurrentTimestamp(header->mutable_timestamp()); +} + +bool recoveryCompletedAsRequested( + const cmvr::safety::RecoveryResultCode result) noexcept +{ + return result == cmvr::safety::RecoveryResultCode::Recovered || + result == + cmvr::safety::RecoveryResultCode::VerifiedButStillBlocked || + result == cmvr::safety::RecoveryResultCode::NothingToRecover; +} + } // namespace gRPCSystemServiceImpl::gRPCSystemServiceImpl() - : gRPCSystemServiceImpl(std::chrono::seconds(15)) + : gRPCSystemServiceImpl( + std::chrono::seconds(15), makeDefaultGrpcSecurityGateway()) { } gRPCSystemServiceImpl::gRPCSystemServiceImpl( const std::chrono::milliseconds stop_timeout) + : gRPCSystemServiceImpl(stop_timeout, makeDefaultGrpcSecurityGateway()) +{ +} + +gRPCSystemServiceImpl::gRPCSystemServiceImpl( + std::shared_ptr security_gateway) + : gRPCSystemServiceImpl( + std::chrono::seconds(15), std::move(security_gateway)) +{ +} + +gRPCSystemServiceImpl::gRPCSystemServiceImpl( + const std::chrono::milliseconds stop_timeout, + std::shared_ptr security_gateway) + : gRPCSystemServiceImpl( + stop_timeout, std::move(security_gateway), nullptr) +{ +} + +gRPCSystemServiceImpl::gRPCSystemServiceImpl( + const std::chrono::milliseconds stop_timeout, + std::shared_ptr security_gateway, + std::shared_ptr recovery_audit_sink) : dmgr_(DeviceManager::getInstance()), stop_timeout_( stop_timeout > std::chrono::milliseconds::zero() ? stop_timeout : std::chrono::seconds(15)), - action_queue_(std::make_unique(dmgr_)) + security_gateway_( + security_gateway ? std::move(security_gateway) + : makeDefaultGrpcSecurityGateway()), + recovery_audit_sink_(std::move(recovery_audit_sink)), + action_queue_(std::make_shared(dmgr_)) { // Acquire only after ActionQueue construction succeeds. This ensures an // exception cannot release the last dispatcher outside the registry. acquireProcessStopDispatcher(stop_dispatcher_); + try { + safety_participant_registration_ = registerGrpcSafetyParticipants( + dmgr_.safetyCoordinator(), action_queue_, stop_dispatcher_); + } catch (...) { + releaseProcessStopDispatcher(stop_dispatcher_); + throw; + } } gRPCSystemServiceImpl::~gRPCSystemServiceImpl() { // The dispatcher intentionally outlives ActionQueue, then joins any // deadline-overrunning stop workers before the last service disappears. + safety_participant_registration_.reset(); action_queue_.reset(); releaseProcessStopDispatcher(stop_dispatcher_); } @@ -1088,11 +1202,27 @@ void gRPCSystemServiceImpl::prepareForShutdown() grpc::Status gRPCSystemServiceImpl::GetSystemInfo(grpc::ServerContext* context, const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.SystemService/GetSystemInfo"); try { response->set_version(dmgr_.version()); response->set_system_name(dmgr_.name()); response->set_action_service_instance_id( action_queue_->instanceId()); + const auto& security = security_gateway_->config(); + response->set_grpc_transport_security(toString(security.transport)); + response->set_grpc_authentication(toString(security.authentication)); + response->set_grpc_recovery_exposure( + toString(security.recovery_exposure)); + response->set_grpc_insecure_non_loopback( + security.insecure_non_loopback); + const auto safety = dmgr_.safetyCoordinator().snapshot(); + response->set_control_service_instance_id( + safety.service_instance_id); + response->set_safety_enforcement_mode( + cmvr::safety::toString(safety.enforcement_mode)); + response->set_safety_schema_version(1); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (GetSystemInfo): success, name=" @@ -1110,6 +1240,9 @@ grpc::Status gRPCSystemServiceImpl::GetSystemInfo(grpc::ServerContext* context, grpc::Status gRPCSystemServiceImpl::GetSystemStatus(grpc::ServerContext* context, const api::GetSystemStatusCommand_Request* request, api::GetSystemStatusCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.SystemService/GetSystemStatus"); try { std::list> dev_list; dmgr_.getDeviceList(dev_list); @@ -1163,7 +1296,9 @@ grpc::Status gRPCSystemServiceImpl::GetDeviceList( const api::GetDeviceListCommand_Request* request, api::GetDeviceListCommand_Feedback* response) { - (void)context; + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.SystemService/GetDeviceList"); (void)request; try { const auto snapshot = dmgr_.snapshot(); @@ -1207,8 +1342,244 @@ grpc::Status gRPCSystemServiceImpl::GetDeviceList( } } +grpc::Status gRPCSystemServiceImpl::GetSafetyState( + grpc::ServerContext* context, + const cmvr::api::GetSafetyStateCommand_Request* request, + cmvr::api::GetSafetyStateCommand_Feedback* response) +{ + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.SystemService/GetSafetyState"); + if (!request || !response) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "GetSafetyState request and response are required"); + } + + const auto scope = parseSafetyScope(request->scope(), true); + if (!scope.valid) { + setSafetyHeaderFailure( + response->mutable_header(), + cmvr::safety::SafetyReason::InvalidArgument, + scope.error); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, scope.error); + } + + const auto snapshot = dmgr_.safetyCoordinator().snapshot(); + response->set_system_state( + toApiSystemAdmissionState(snapshot.system_state)); + response->set_safety_epoch(snapshot.safety_epoch); + response->set_control_service_instance_id(snapshot.service_instance_id); + response->set_enforcement_mode( + cmvr::safety::toString(snapshot.enforcement_mode)); + response->set_active_operation_id(snapshot.active_operation_id); + response->set_active_operation_phase(snapshot.active_operation_phase); + response->set_sampled_at_unix_ms(unixTimeMs()); + + std::unordered_set requested_ids( + scope.device_ids.begin(), scope.device_ids.end()); + for (const auto& device : snapshot.devices) { + if (!scope.all_devices && + requested_ids.erase(device.descriptor.device_id) == 0U) { + continue; + } + populateDeviceSafetyState(device, *response->add_devices()); + } + for (const auto& participant : snapshot.participants) { + populateSafetyParticipantState( + participant, *response->add_participants()); + } + if (!requested_ids.empty()) { + const auto detail = + "safety device is not registered: " + *requested_ids.begin(); + response->Clear(); + setSafetyHeaderFailure( + response->mutable_header(), + cmvr::safety::SafetyReason::DeviceNotFound, + detail); + return grpc::Status(grpc::StatusCode::NOT_FOUND, detail); + } + + response->mutable_header()->set_success(true); + response->mutable_header()->set_reason_code( + cmvr::api::COMMAND_REASON_CODE_NONE); + response->mutable_header()->set_service_instance_id( + snapshot.service_instance_id); + response->mutable_header()->set_safety_epoch(snapshot.safety_epoch); + response->mutable_header()->set_execution_state( + cmvr::api::COMMAND_EXECUTION_STATE_COMPLETED); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; +} + +grpc::Status gRPCSystemServiceImpl::RecoverSafetyState( + grpc::ServerContext* context, + const cmvr::api::RecoverSafetyStateCommand_Request* request, + cmvr::api::RecoverSafetyStateCommand_Feedback* response) +{ + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.SystemService/RecoverSafetyState"); + if (!request || !response) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "RecoverSafetyState request and response are required"); + } + + const auto scope = parseSafetyScope(request->scope(), false); + if (!scope.valid || request->recovery_id().empty() || + request->reason().empty() || request->expected_safety_epoch() == 0 || + request->mode() == + cmvr::api::RecoverSafetyStateCommand::MODE_UNSPECIFIED) { + std::string detail = scope.valid + ? "recovery_id, reason, expected_safety_epoch, and mode are required" + : scope.error; + 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 bool verify_only = request->mode() == + cmvr::api::RecoverSafetyStateCommand::VERIFY_ONLY; + 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 = verify_only ? "verify_only" : "clear_software_latch"; + audit.all_devices = scope.all_devices; + audit.device_ids = scope.device_ids; + audit.expected_safety_epoch = request->expected_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_.safetyCoordinator().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())); + } + + cmvr::safety::RecoveryRequest coordinator_request; + coordinator_request.recovery_id = request->recovery_id(); + coordinator_request.device_ids = scope.device_ids; + coordinator_request.all_devices = scope.all_devices; + coordinator_request.expected_safety_epoch = + request->expected_safety_epoch(); + coordinator_request.verify_only = verify_only; + coordinator_request.reason = request->reason(); + coordinator_request.deadline = deadline; + if (!verify_only) { + auto commit_audit = audit; + commit_audit.stage = "clear_commit"; + commit_audit.result = "authorized"; + const auto sink = recovery_audit_sink_; + 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] Recovery clear audit " + "failed: " + << error; + } + return persisted; + }; + } + + const auto result = + dmgr_.safetyCoordinator().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_.safetyCoordinator().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 if (!result.targets.empty()) { + header->set_reason_code( + toApiSafetyReason(result.targets.front().reason)); + header->set_error_message(result.targets.front().detail); + } else { + header->set_reason_code( + cmvr::api::COMMAND_REASON_CODE_INTERNAL_ERROR); + header->set_error_message("recovery 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] Recovery 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( + security_gateway_, context, + "/cmvr.api.SystemService/UpdateParams"); response->mutable_header()->set_success(false); response->mutable_header()->set_error_message("UpdateParams is no longer supported. Use typed device commands or reload configuration."); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -1218,6 +1589,75 @@ grpc::Status gRPCSystemServiceImpl::UpdateParams(grpc::ServerContext* context, c grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, "/cmvr.api.SystemService/StopAll"); + if (!request || !response) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "StopAll request and response are required"); + } + if (dmgr_.safetyCoordinator().config().enforcement_mode != + cmvr::safety::EnforcementMode::Legacy) { + auto deadline = cmvr_grpc_call_guard.context().deadline; + const auto configured_deadline = + cmvr::safety::SafetyClock::now() + stop_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())); + } + + std::string operation_id = request->operation_id(); + if (operation_id.empty() && request->has_header()) { + operation_id = request->header().command_id(); + } + const auto result = dmgr_.safetyCoordinator().stopAll( + std::move(operation_id), deadline); + response->set_operation_id(result.operation_id); + 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()); + } + + auto* header = response->mutable_header(); + header->set_success(result.success); + header->set_command_id(result.operation_id); + header->set_service_instance_id( + dmgr_.safetyCoordinator().serviceInstanceId()); + header->set_safety_epoch(result.current_safety_epoch); + header->set_execution_state( + result.success + ? cmvr::api::COMMAND_EXECUTION_STATE_COMPLETED + : cmvr::api::COMMAND_EXECUTION_STATE_FAILED); + if (result.success) { + header->set_reason_code(cmvr::api::COMMAND_REASON_CODE_NONE); + } else if (!result.targets.empty()) { + 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_STOP_UNCONFIRMED); + header->set_error_message( + "StopAll did not produce a participant result"); + } + setCurrentTimestamp(header->mutable_timestamp()); + return grpc::Status::OK; + } (void)request; const auto stop_deadline = std::chrono::steady_clock::now() + stop_timeout_; @@ -1694,12 +2134,24 @@ grpc::Status gRPCSystemServiceImpl::ExecuteActionQueue( const cmvr::api::ActionQueueCommand_Request* request, cmvr::api::ActionQueueCommand_Feedback* response) { + CMVR_GRPC_REQUIRE_REGISTERED_CALL( + security_gateway_, context, + "/cmvr.api.SystemService/ExecuteActionQueue"); if (!request || !response) { return grpc::Status( grpc::StatusCode::INVALID_ARGUMENT, "ActionQueue request and response are required"); } try { + cmvr::safety::CommandActor actor; + actor.principal_id = + cmvr_grpc_call_guard.context().principal.id; + actor.authenticated = + cmvr_grpc_call_guard.context().principal.authenticated; + for (const auto role : + cmvr_grpc_call_guard.context().principal.roles) { + actor.roles.emplace_back(cmvr::service::toString(role)); + } // The callback is consumed only on this synchronous handler stack. It // is never retained by the worker-owned action Record, so returning the // RPC cannot leave a dangling ServerContext reference. @@ -1708,7 +2160,8 @@ grpc::Status gRPCSystemServiceImpl::ExecuteActionQueue( *response, [context]() { return context && context->IsCancelled(); - }); + }, + std::move(actor)); if (wait_result == ActionQueueExecutor::WaitResult::CanceledBeforeAdmission) { return grpc::Status( diff --git a/cmvr-es/service/grpc/src/media_activity_coordinator.cpp b/cmvr-es/service/grpc/src/media_activity_coordinator.cpp index 1650cad1..8bd99123 100644 --- a/cmvr-es/service/grpc/src/media_activity_coordinator.cpp +++ b/cmvr-es/service/grpc/src/media_activity_coordinator.cpp @@ -408,12 +408,21 @@ bool MediaActivityCoordinator::waitForStopped( bool MediaActivityCoordinator::finishStopAll( const StopAllTicket& ticket, const bool all_media_stopped) +{ + return finishStopAllDetailed(ticket, all_media_stopped) + .participant_stopped; +} + +MediaActivityCoordinator::FinishStopAllResult +MediaActivityCoordinator::finishStopAllDetailed( + const StopAllTicket& ticket, + const bool all_media_stopped) { std::lock_guard lock(impl_->mutex); if (!ticket.valid() || impl_->accepting || ticket.generation != impl_->generation || impl_->stop_all_tickets.erase(ticket.ticket_id) == 0U) { - return false; + return {}; } const bool caller_succeeded = @@ -425,7 +434,20 @@ bool MediaActivityCoordinator::finishStopAll( !impl_->hasSessionsBefore(ticket.generation)) { impl_->accepting = true; } - return caller_succeeded; + return {true, caller_succeeded, impl_->accepting}; +} + +void MediaActivityCoordinator::clearForTesting() noexcept +{ + try { + std::lock_guard lock(impl_->mutex); + impl_->accepting = true; + impl_->stop_all_failed = false; + ++impl_->generation; + impl_->stop_all_tickets.clear(); + } catch (...) { + } + impl_->condition.notify_all(); } MediaActivityCoordinator& globalMediaActivityCoordinator() diff --git a/cmvr-es/service/grpc/src/motor_activity_coordinator.cpp b/cmvr-es/service/grpc/src/motor_activity_coordinator.cpp index 9f46a48e..aef287ae 100644 --- a/cmvr-es/service/grpc/src/motor_activity_coordinator.cpp +++ b/cmvr-es/service/grpc/src/motor_activity_coordinator.cpp @@ -436,12 +436,21 @@ bool MotorActivityCoordinator::stopAndWait( bool MotorActivityCoordinator::finishStopAll( const StopAllTicket& ticket, const bool all_motors_stopped) +{ + return finishStopAllDetailed(ticket, all_motors_stopped) + .participant_stopped; +} + +MotorActivityCoordinator::FinishStopAllResult +MotorActivityCoordinator::finishStopAllDetailed( + const StopAllTicket& ticket, + const bool all_motors_stopped) { std::lock_guard lock(impl_->mutex); if (!ticket.valid() || impl_->accepting || ticket.generation != impl_->generation || impl_->stop_all_tickets.erase(ticket.ticket_id) == 0U) { - return false; + return {}; } const bool caller_succeeded = @@ -454,7 +463,7 @@ bool MotorActivityCoordinator::finishStopAll( impl_->accepting = true; impl_->round_targets.clear(); } - return caller_succeeded; + return {true, caller_succeeded, impl_->accepting}; } void MotorActivityCoordinator::notifyStateChanged() noexcept diff --git a/cmvr-es/service/grpc/tests/camera_ptz_activity_registry_test.cpp b/cmvr-es/service/grpc/tests/camera_ptz_activity_registry_test.cpp index a4de10b9..272e3ecf 100644 --- a/cmvr-es/service/grpc/tests/camera_ptz_activity_registry_test.cpp +++ b/cmvr-es/service/grpc/tests/camera_ptz_activity_registry_test.cpp @@ -166,6 +166,35 @@ TEST_F(CameraPtzActivityRegistryTest, CameraPtzActivityRegistry::DispatchResult::Success); } +TEST_F(CameraPtzActivityRegistryTest, + ExplicitStopRemainsAvailableWhileStartAdmissionIsLatched) +{ + CameraPtzActivityRegistry registry; + auto camera = std::make_shared("camera"); + ASSERT_EQ( + registry.control( + camera->id(), camera, device::PtzCommand::PanLeft, false, 4), + CameraPtzActivityRegistry::DispatchResult::Success); + + const auto ticket = globalStopAllAdmissionGate().beginStopAll(); + ASSERT_TRUE(ticket.valid()); + EXPECT_FALSE(globalStopAllAdmissionGate().finishStopAll(ticket, false)); + EXPECT_EQ( + registry.control( + camera->id(), camera, device::PtzCommand::ZoomIn, false, 4), + CameraPtzActivityRegistry::DispatchResult::RejectedByStopAll); + EXPECT_EQ( + registry.control( + camera->id(), camera, device::PtzCommand::PanLeft, true, 4), + CameraPtzActivityRegistry::DispatchResult::Success); + EXPECT_EQ(registry.activeCommandCount(), 0U); + + const auto calls = camera->calls(); + ASSERT_EQ(calls.size(), 2U); + EXPECT_FALSE(calls.front().stop); + EXPECT_TRUE(calls.back().stop); +} + TEST_F(CameraPtzActivityRegistryTest, StopForDeviceIsSelectiveAndActiveIdsReflectFailures) { @@ -260,5 +289,61 @@ TEST_F(CameraPtzActivityRegistryTest, EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); } +TEST_F(CameraPtzActivityRegistryTest, + DispatchFenceRunsInsidePerDeviceQueueBeforeHardwareMutation) +{ + CameraPtzActivityRegistry registry; + auto camera = std::make_shared("camera"); + std::mutex mutex; + std::condition_variable condition; + bool first_fence_entered = false; + bool release_first_fence = false; + std::atomic second_fence_entered{false}; + + std::thread first([&] { + EXPECT_EQ( + registry.control( + camera->id(), camera, device::PtzCommand::PanLeft, false, 4, + [&] { + std::unique_lock lock(mutex); + first_fence_entered = true; + condition.notify_all(); + condition.wait(lock, [&] { return release_first_fence; }); + return true; + }), + CameraPtzActivityRegistry::DispatchResult::Success); + }); + { + std::unique_lock lock(mutex); + condition.wait(lock, [&] { return first_fence_entered; }); + } + + std::thread second([&] { + EXPECT_EQ( + registry.control( + camera->id(), camera, device::PtzCommand::ZoomIn, false, 7, + [&] { + second_fence_entered = true; + return false; + }), + CameraPtzActivityRegistry::DispatchResult::RejectedByDispatchFence); + }); + std::this_thread::yield(); + EXPECT_FALSE(second_fence_entered.load()); + + { + std::lock_guard lock(mutex); + release_first_fence = true; + } + condition.notify_all(); + first.join(); + second.join(); + + EXPECT_TRUE(second_fence_entered.load()); + const auto calls = camera->calls(); + ASSERT_EQ(calls.size(), 1U); + EXPECT_EQ(calls.front().command, device::PtzCommand::PanLeft); +} + } // namespace } // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp index 19108a25..3c923938 100644 --- a/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp @@ -588,6 +588,25 @@ protected: response.header().error_message()}; } + grpc::Status identifiedMoveJ( + const std::string& command_id, + const double position, + api::MoveJ_Response& response) + { + api::MoveJ_Request request; + auto* header = request.mutable_header(); + header->set_device_id("aubo_arm"); + header->set_command_id(command_id); + header->set_expected_service_instance_id( + device::DeviceManager::getInstance() + .safetyCoordinator() + .serviceInstanceId()); + header->set_valid_for_ms(1000); + request.mutable_target()->add_position(position); + grpc::ServerContext context; + return service_->moveJ(&context, &request, &response); + } + MoveOutcome moveL(const std::string& device_id) { api::MoveL_Request request; @@ -833,6 +852,49 @@ TEST_F(GrpcArmServiceTest, MoveBindsLeaseRevocationCancellation) EXPECT_FALSE(aubo_arm_->lastMotionCancellationRequested()); } +TEST_F(GrpcArmServiceTest, + IdenticalCommandIdReplaysCachedResultWithoutRedispatch) +{ + api::MoveJ_Response first; + api::MoveJ_Response retry; + + const auto first_status = identifiedMoveJ( + "arm-movej-idempotency-1", 0.1, first); + const auto retry_status = identifiedMoveJ( + "arm-movej-idempotency-1", 0.1, retry); + + ASSERT_TRUE(first_status.ok()) << first_status.error_message(); + ASSERT_TRUE(retry_status.ok()) << retry_status.error_message(); + EXPECT_TRUE(first.header().success()); + EXPECT_TRUE(retry.header().success()); + EXPECT_EQ( + retry.header().execution_state(), + api::COMMAND_EXECUTION_STATE_COMPLETED); + EXPECT_EQ(retry.header().command_id(), "arm-movej-idempotency-1"); + EXPECT_EQ(aubo_arm_->moveJCalls(), 1); +} + +TEST_F(GrpcArmServiceTest, + ReusedCommandIdWithDifferentPayloadIsRejectedWithoutRedispatch) +{ + api::MoveJ_Response first; + api::MoveJ_Response conflict; + + const auto first_status = identifiedMoveJ( + "arm-movej-conflict-1", 0.1, first); + const auto conflict_status = identifiedMoveJ( + "arm-movej-conflict-1", 0.2, conflict); + + ASSERT_TRUE(first_status.ok()) << first_status.error_message(); + EXPECT_EQ( + conflict_status.error_code(), grpc::StatusCode::ALREADY_EXISTS); + EXPECT_FALSE(conflict.header().success()); + EXPECT_EQ( + conflict.header().reason_code(), + api::COMMAND_REASON_CODE_COMMAND_ID_CONFLICT); + EXPECT_EQ(aubo_arm_->moveJCalls(), 1); +} + TEST_F(GrpcArmServiceTest, StopAllCancelsInFlightTorqueOnWithoutReportingSuccess) { @@ -1109,6 +1171,9 @@ TEST_F(GrpcArmServiceTest, StopMotionFailureRetainsSafetyBarrier) EXPECT_EQ(stop_status.error_code(), grpc::StatusCode::INTERNAL); EXPECT_FALSE(stop_response.success()); + EXPECT_EQ( + stop_response.reason_code(), + api::COMMAND_REASON_CODE_STOP_UNCONFIRMED); EXPECT_FALSE(authority.validate(action_lease.token)); EXPECT_TRUE(authority.isLeased("aubo_arm")); EXPECT_EQ( @@ -1143,8 +1208,12 @@ TEST_F(GrpcArmServiceTest, StopMotionExceptionRetainsSafetyBarrier) const auto stop_status = stopMotion("aubo_arm", stop_response); const auto rejected_move = moveL("aubo_arm"); - EXPECT_EQ(stop_status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_EQ( + stop_status.error_code(), grpc::StatusCode::FAILED_PRECONDITION); EXPECT_FALSE(stop_response.success()); + EXPECT_EQ( + stop_response.reason_code(), + api::COMMAND_REASON_CODE_STOP_UNCONFIRMED); EXPECT_FALSE(authority.validate(action_lease.token)); EXPECT_TRUE(authority.isLeased("aubo_arm")); EXPECT_EQ( diff --git a/cmvr-es/service/grpc/tests/grpc_arm_teleop_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_arm_teleop_service_test.cpp index 5e5bf2f5..a8394d5a 100644 --- a/cmvr-es/service/grpc/tests/grpc_arm_teleop_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_arm_teleop_service_test.cpp @@ -17,6 +17,8 @@ #include #include +#include "manager/safety/include/device_safety_endpoint.h" +#include "manager/safety/include/safety_coordinator.h" #include "service/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { @@ -253,11 +255,93 @@ private: mutable std::set backend_threads_; }; +class FakeTeleopSafetyEndpoint final + : public safety::DeviceSafetyEndpoint { +public: + FakeTeleopSafetyEndpoint() + { + descriptor_.device_id = makeManifest().robot_id(); + descriptor_.kind = device::DeviceKind::Arm; + descriptor_.default_policy = + safety::SafetyPolicyFamily::Control; + descriptor_.maximum_snapshot_age = 1s; + descriptor_.supports_active_refresh = true; + } + + safety::DeviceSafetyDescriptor descriptor() const override + { + return descriptor_; + } + + void bindPublisher( + safety::SafetySnapshotPublisher publisher) override + { + publisher_ = std::move(publisher); + } + + void requestSafetyRefresh() noexcept override + { + if (!publisher_) { + return; + } + safety::DeviceSafetySnapshot snapshot; + snapshot.device_id = descriptor_.device_id; + snapshot.condition = safety::SafetyCondition::Nominal; + snapshot.device_generation = generation_; + snapshot.sample_sequence = ++sequence_; + snapshot.observed_at = safety::SafetyClock::now(); + snapshot.connected = safety::TriState::True; + snapshot.operational_ready = safety::TriState::True; + snapshot.quiescent = safety::TriState::True; + snapshot.motion_active = safety::TriState::False; + snapshot.actuator_enabled = safety::TriState::True; + snapshot.emergency_stop_active = safety::TriState::False; + snapshot.protective_stop_active = safety::TriState::False; + snapshot.fault_active = safety::TriState::False; + (void)publisher_(std::move(snapshot)); + } + + void onDeviceGenerationChanged( + const std::uint64_t generation) noexcept override + { + generation_ = generation; + requestSafetyRefresh(); + } + + safety::HardwareCheckResult validateBeforeDispatch( + const safety::AdmissionPermit&) override + { + ++hardware_checks_; + return {true, safety::SafetyReason::None, {}}; + } + + safety::RecoveryCheckResult reconcileAdmissionState( + const safety::RecoveryContext&) override + { + return {true, safety::SafetyReason::None, {}}; + } + + int hardwareChecks() const noexcept + { + return hardware_checks_.load(); + } + +private: + safety::DeviceSafetyDescriptor descriptor_; + safety::SafetySnapshotPublisher publisher_; + std::uint64_t generation_{1}; + std::uint64_t sequence_{0}; + std::atomic hardware_checks_{0}; +}; + class TeleopServerHarness final { public: explicit TeleopServerHarness( - std::shared_ptr backend) - : service_(std::move(backend)) + std::shared_ptr backend, + safety::SafetyCoordinator* safety_coordinator = nullptr) + : service_( + std::move(backend), nullptr, nullptr, + safety_coordinator) { socket_path_ = "/tmp/cmvr_arm_teleop_service_test_" + @@ -775,5 +859,62 @@ TEST(ArmTeleopServiceTest, admission.clearForTesting(); } +TEST(ArmTeleopServiceTest, + CoordinatorInvalidationStopsExistingSessionBeforeAnotherSetpoint) +{ + safety::SafetyCoordinatorConfig config; + config.enforcement_mode = safety::EnforcementMode::EnforceAll; + safety::SafetyCoordinator coordinator(config); + auto endpoint = std::make_shared(); + ASSERT_TRUE(coordinator.registerDevice( + {endpoint->descriptor(), endpoint, {}})); + coordinator.updateDeviceRuntimeState( + makeManifest().robot_id(), + device::ManagedDeviceState::Running, + {device::DeviceHealthState::Healthy, {}}); + coordinator.markStartupComplete(); + + auto backend = std::make_shared(); + TeleopServerHarness harness(backend, &coordinator); + grpc::ClientContext context; + context.set_deadline(std::chrono::system_clock::now() + 2s); + auto stream = harness.stub().Teleoperate(&context); + + ASSERT_TRUE(stream->Write(makeOpenFrame(makeManifest(), 500, 1500))); + expectOpeningFrames(*stream); + EXPECT_EQ(endpoint->hardwareChecks(), 1); + + ASSERT_TRUE(stream->Write(makeSetpoint(1))); + arm_teleop::ServerFrame response; + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ(response.status().phase(), arm_teleop::SESSION_PHASE_ACTIVE); + EXPECT_EQ(response.status().applied_sequence(), 1U); + EXPECT_EQ(endpoint->hardwareChecks(), 2); + + coordinator.quarantineDevice( + makeManifest().robot_id(), + safety::SafetyReason::OutcomeUnknown, + "test-quarantine"); + + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_LEASE_LOST); + EXPECT_EQ( + response.status().stop_reason(), + arm_teleop::STOP_REASON_EMERGENCY_STOP); + const auto status = stream->Finish(); + EXPECT_TRUE( + status.error_code() == grpc::StatusCode::FAILED_PRECONDITION || + status.error_code() == grpc::StatusCode::CANCELLED) + << status.error_message(); + EXPECT_EQ(backend->appliedSequences(), + (std::vector{1U})); + ASSERT_FALSE(backend->stopReasons().empty()); + EXPECT_EQ( + backend->stopReasons().back(), + arm_teleop::STOP_REASON_EMERGENCY_STOP); +} + } // namespace } // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_command_transaction_test.cpp b/cmvr-es/service/grpc/tests/grpc_command_transaction_test.cpp new file mode 100644 index 00000000..c50d7178 --- /dev/null +++ b/cmvr-es/service/grpc/tests/grpc_command_transaction_test.cpp @@ -0,0 +1,454 @@ +#include "service/grpc/include/grpc_command_transaction.h" + +#include +#include +#include +#include +#include + +#include + +#include "cmvr/api/arm_command.pb.h" +#include "manager/safety/include/device_safety_endpoint.h" + +namespace cmvr::service { +namespace { + +class FakeEndpoint final : public safety::DeviceSafetyEndpoint { +public: + explicit FakeEndpoint(std::string device_id) + { + descriptor_.device_id = std::move(device_id); + descriptor_.kind = device::DeviceKind::Arm; + descriptor_.default_policy = safety::SafetyPolicyFamily::Control; + descriptor_.maximum_snapshot_age = std::chrono::seconds(1); + descriptor_.supports_active_refresh = true; + } + + safety::DeviceSafetyDescriptor descriptor() const override + { + return descriptor_; + } + + void bindPublisher(safety::SafetySnapshotPublisher publisher) override + { + publisher_ = std::move(publisher); + } + + void requestSafetyRefresh() noexcept override + { + if (!publisher_) { + return; + } + safety::DeviceSafetySnapshot snapshot; + snapshot.device_id = descriptor_.device_id; + snapshot.condition = safety::SafetyCondition::Nominal; + snapshot.device_generation = 1; + snapshot.sample_sequence = ++sequence_; + snapshot.observed_at = safety::SafetyClock::now(); + snapshot.connected = safety::TriState::True; + snapshot.operational_ready = safety::TriState::True; + snapshot.quiescent = safety::TriState::True; + snapshot.motion_active = safety::TriState::False; + snapshot.actuator_enabled = safety::TriState::True; + snapshot.emergency_stop_active = safety::TriState::False; + snapshot.protective_stop_active = safety::TriState::False; + snapshot.fault_active = safety::TriState::False; + (void)publisher_(std::move(snapshot)); + } + + safety::HardwareCheckResult validateBeforeDispatch( + const safety::AdmissionPermit&) override + { + ++hardware_checks; + return final_check; + } + + safety::RecoveryCheckResult reconcileAdmissionState( + const safety::RecoveryContext&) override + { + return {true, safety::SafetyReason::None, {}}; + } + + safety::DeviceSafetyDescriptor descriptor_; + safety::SafetySnapshotPublisher publisher_; + safety::HardwareCheckResult final_check{ + true, safety::SafetyReason::None, {}}; + std::atomic sequence_{0}; + std::atomic hardware_checks{0}; +}; + +api::MoveJ_Request moveRequest( + const safety::SafetyCoordinator& coordinator, + const std::string& command_id, + const double target = 0.25) +{ + api::MoveJ_Request request; + request.mutable_header()->set_device_id("arm"); + request.mutable_header()->set_command_id(command_id); + request.mutable_header()->set_expected_service_instance_id( + coordinator.serviceInstanceId()); + request.mutable_header()->set_valid_for_ms(1000); + request.mutable_target()->add_position(target); + return request; +} + +grpc::Status executeMove( + safety::SafetyCoordinator& coordinator, + const api::MoveJ_Request& request, + api::MoveJ_Response& response, + int& dispatches, + const bool throw_after_dispatch = false) +{ + grpc::ServerContext context; + return executeRegisteredGrpcCommand( + makeDefaultGrpcSecurityGateway(), + &context, + coordinator, + "/cmvr.api.ArmService/moveJ", + &request, + &response, + [&](GrpcCommandTransaction& transaction) { + if (!transaction.beginDispatch()) { + return transaction.dispatchStatus(); + } + ++dispatches; + if (throw_after_dispatch) { + throw std::runtime_error("simulated lost driver acknowledgement"); + } + response.mutable_header()->set_success(true); + return grpc::Status::OK; + }); +} + +TEST(GrpcCommandTransactionTest, + DeterministicHashIgnoresRetryIdentityButIncludesPayload) +{ + safety::SafetyCoordinator coordinator; + auto first = moveRequest(coordinator, "command-1", 0.25); + auto retry = first; + retry.mutable_header()->set_command_id("command-2"); + retry.mutable_header()->set_valid_for_ms(2000); + retry.mutable_header()->mutable_timestamp()->set_seconds(1234); + auto changed = retry; + changed.mutable_target()->set_position(0, 0.5); + + const auto first_hash = deterministicGrpcPayloadHash( + "/cmvr.api.ArmService/moveJ", first); + EXPECT_FALSE(first_hash.empty()); + EXPECT_EQ( + first_hash, + deterministicGrpcPayloadHash( + "/cmvr.api.ArmService/moveJ", retry)); + EXPECT_NE( + first_hash, + deterministicGrpcPayloadHash( + "/cmvr.api.ArmService/moveJ", changed)); + EXPECT_NE( + first_hash, + deterministicGrpcPayloadHash( + "/cmvr.api.ArmService/moveL", first)); +} + +TEST(GrpcCommandTransactionTest, SameIdReturnsCachedResponseWithoutRedispatch) +{ + safety::SafetyCoordinator coordinator; + auto endpoint = std::make_shared("arm"); + ASSERT_TRUE(coordinator.registerDevice( + {endpoint->descriptor(), endpoint, {}})); + coordinator.markStartupComplete(); + + const auto request = moveRequest(coordinator, "command-cache"); + api::MoveJ_Response first_response; + int dispatches = 0; + ASSERT_TRUE(executeMove( + coordinator, request, first_response, dispatches).ok()); + ASSERT_TRUE(first_response.header().success()); + EXPECT_EQ(dispatches, 1); + + auto retry = request; + retry.mutable_header()->set_valid_for_ms(2500); + retry.mutable_header()->mutable_timestamp()->set_seconds(42); + api::MoveJ_Response cached_response; + ASSERT_TRUE(executeMove( + coordinator, retry, cached_response, dispatches).ok()); + EXPECT_EQ(dispatches, 1); + EXPECT_TRUE(cached_response.header().success()); + EXPECT_EQ( + cached_response.header().execution_state(), + api::COMMAND_EXECUTION_STATE_COMPLETED); + EXPECT_EQ(cached_response.header().command_id(), "command-cache"); + + auto conflict = request; + conflict.mutable_target()->set_position(0, 0.75); + api::MoveJ_Response conflict_response; + const auto conflict_status = executeMove( + coordinator, conflict, conflict_response, dispatches); + EXPECT_EQ(conflict_status.error_code(), grpc::StatusCode::ALREADY_EXISTS); + EXPECT_EQ(dispatches, 1); + EXPECT_EQ( + conflict_response.header().reason_code(), + api::COMMAND_REASON_CODE_COMMAND_ID_CONFLICT); +} + +TEST(GrpcCommandTransactionTest, + EnforcedCommandRequiresIdentityAndRunsFinalHardwareCheck) +{ + safety::SafetyCoordinatorConfig config; + config.enforcement_mode = safety::EnforcementMode::EnforceAll; + safety::SafetyCoordinator coordinator(config); + auto endpoint = std::make_shared("arm"); + ASSERT_TRUE(coordinator.registerDevice( + {endpoint->descriptor(), endpoint, {}})); + coordinator.updateDeviceRuntimeState( + "arm", + device::ManagedDeviceState::Running, + {device::DeviceHealthState::Healthy, {}}); + coordinator.markStartupComplete(); + + auto missing_identity = moveRequest(coordinator, ""); + api::MoveJ_Response rejected; + int dispatches = 0; + const auto rejected_status = executeMove( + coordinator, missing_identity, rejected, dispatches); + EXPECT_EQ( + rejected_status.error_code(), grpc::StatusCode::INVALID_ARGUMENT); + EXPECT_EQ(dispatches, 0); + EXPECT_EQ( + rejected.header().reason_code(), + api::COMMAND_REASON_CODE_COMMAND_ID_REQUIRED); + + auto accepted = moveRequest(coordinator, "command-enforced"); + api::MoveJ_Response accepted_response; + ASSERT_TRUE(executeMove( + coordinator, accepted, accepted_response, dispatches).ok()); + EXPECT_EQ(dispatches, 1); + EXPECT_EQ(endpoint->hardware_checks.load(), 1); + EXPECT_EQ( + accepted_response.header().device_generation(), 1U); +} + +TEST(GrpcCommandTransactionTest, + ExceptionAfterDispatchIsQuarantinedAndNeverRedispatched) +{ + safety::SafetyCoordinatorConfig config; + config.enforcement_mode = safety::EnforcementMode::EnforceAll; + safety::SafetyCoordinator coordinator(config); + auto endpoint = std::make_shared("arm"); + ASSERT_TRUE(coordinator.registerDevice( + {endpoint->descriptor(), endpoint, {}})); + coordinator.updateDeviceRuntimeState( + "arm", + device::ManagedDeviceState::Running, + {device::DeviceHealthState::Healthy, {}}); + coordinator.markStartupComplete(); + + const auto request = moveRequest(coordinator, "command-unknown"); + api::MoveJ_Response response; + int dispatches = 0; + ASSERT_TRUE(executeMove( + coordinator, request, response, dispatches, true).ok()); + EXPECT_EQ(dispatches, 1); + EXPECT_EQ( + response.header().reason_code(), + api::COMMAND_REASON_CODE_OUTCOME_UNKNOWN); + ASSERT_EQ(coordinator.snapshot().devices.size(), 1U); + EXPECT_EQ( + coordinator.snapshot().devices.front().admission_state, + safety::DeviceAdmissionState::Quarantined); + + api::MoveJ_Response retry_response; + ASSERT_TRUE(executeMove( + coordinator, request, retry_response, dispatches).ok()); + EXPECT_EQ(dispatches, 1); + EXPECT_EQ( + retry_response.header().execution_state(), + api::COMMAND_EXECUTION_STATE_OUTCOME_UNKNOWN); +} + +TEST(GrpcCommandTransactionTest, + ScopedDispatchChecksHardwareForEverySubmission) +{ + safety::SafetyCoordinatorConfig config; + config.enforcement_mode = safety::EnforcementMode::EnforceAll; + safety::SafetyCoordinator coordinator(config); + auto endpoint = std::make_shared("arm"); + ASSERT_TRUE(coordinator.registerDevice( + {endpoint->descriptor(), endpoint, {}})); + coordinator.updateDeviceRuntimeState( + "arm", + device::ManagedDeviceState::Running, + {device::DeviceHealthState::Healthy, {}}); + coordinator.markStartupComplete(); + + const auto request = moveRequest(coordinator, "command-scoped"); + api::MoveJ_Response response; + grpc::ServerContext context; + int dispatches = 0; + const auto status = executeRegisteredGrpcCommand( + makeDefaultGrpcSecurityGateway(), + &context, + coordinator, + "/cmvr.api.ArmService/moveJ", + &request, + &response, + [&](GrpcCommandTransaction& transaction) { + for (int index = 0; index < 2; ++index) { + auto dispatch = transaction.beginScopedDispatch(); + if (!dispatch.acquired()) { + return transaction.dispatchStatus(); + } + ++dispatches; + } + response.mutable_header()->set_success(true); + return grpc::Status::OK; + }); + + EXPECT_TRUE(status.ok()); + EXPECT_TRUE(response.header().success()); + EXPECT_EQ(dispatches, 2); + EXPECT_EQ(endpoint->hardware_checks.load(), 2); +} + +TEST(GrpcCommandTransactionTest, + RevokedActuationPermitCannotSuppressInternalSafetyStop) +{ + safety::SafetyCoordinatorConfig config; + config.enforcement_mode = safety::EnforcementMode::EnforceAll; + safety::SafetyCoordinator coordinator(config); + auto endpoint = std::make_shared("arm"); + ASSERT_TRUE(coordinator.registerDevice( + {endpoint->descriptor(), endpoint, {}})); + coordinator.updateDeviceRuntimeState( + "arm", + device::ManagedDeviceState::Running, + {device::DeviceHealthState::Healthy, {}}); + coordinator.markStartupComplete(); + + const auto request = moveRequest(coordinator, "command-revoked"); + api::MoveJ_Response response; + grpc::ServerContext context; + int actuations = 0; + int safety_stops = 0; + const auto status = executeRegisteredGrpcCommand( + makeDefaultGrpcSecurityGateway(), + &context, + coordinator, + "/cmvr.api.ArmService/moveJ", + &request, + &response, + [&](GrpcCommandTransaction& transaction) { + { + auto dispatch = transaction.beginScopedDispatch(); + if (!dispatch.acquired()) { + return transaction.dispatchStatus(); + } + ++actuations; + } + + const auto stop_all = coordinator.stopAll("revoke-scoped-permit"); + EXPECT_TRUE(stop_all.success); + EXPECT_FALSE(transaction.revalidate()); + const auto revoked_status = transaction.dispatchStatus(); + EXPECT_FALSE(transaction.beginScopedDispatch().acquired()); + + auto stop_dispatch = transaction.beginSafetyStopDispatch(); + if (stop_dispatch.acquired()) { + ++safety_stops; + } + response.mutable_header()->set_success(false); + return revoked_status; + }); + + // Ledger-backed commands report their terminal outcome in Feedback. + EXPECT_TRUE(status.ok()); + EXPECT_FALSE(response.header().success()); + EXPECT_EQ( + response.header().reason_code(), + api::COMMAND_REASON_CODE_SAFETY_LATCHED); + EXPECT_EQ(actuations, 1); + EXPECT_EQ(safety_stops, 1); + EXPECT_EQ(endpoint->hardware_checks.load(), 2); +} + +TEST(GrpcCommandTransactionTest, + ServerDerivedStopRemainsDispatchableWhenActuationIsQuarantined) +{ + safety::SafetyCoordinatorConfig config; + config.enforcement_mode = safety::EnforcementMode::EnforceAll; + safety::SafetyCoordinator coordinator(config); + auto endpoint = std::make_shared("arm"); + ASSERT_TRUE(coordinator.registerDevice( + {endpoint->descriptor(), endpoint, {}})); + coordinator.updateDeviceRuntimeState( + "arm", + device::ManagedDeviceState::Running, + {device::DeviceHealthState::Healthy, {}}); + coordinator.markStartupComplete(); + coordinator.quarantineDevice( + "arm", safety::SafetyReason::OutcomeUnknown, + "uncertain-ptz-start"); + + const auto registered = defaultGrpcMethodPolicyRegistry().find( + "/cmvr.api.CameraService/ControlPtz"); + ASSERT_TRUE(registered.has_value()); + + auto start_request = moveRequest(coordinator, "ptz-start"); + api::MoveJ_Response start_response; + grpc::ServerContext start_context; + int start_dispatches = 0; + const auto start_status = executeServerDerivedGrpcCommand( + makeDefaultGrpcSecurityGateway(), + &start_context, + coordinator, + "/cmvr.api.CameraService/ControlPtz", + *registered, + &start_request, + &start_response, + [&](GrpcCommandTransaction& transaction) { + if (!transaction.beginDispatch()) { + return transaction.dispatchStatus(); + } + ++start_dispatches; + start_response.mutable_header()->set_success(true); + return grpc::Status::OK; + }); + EXPECT_TRUE(start_status.ok()); + EXPECT_EQ(start_dispatches, 0); + EXPECT_FALSE(start_response.header().success()); + EXPECT_EQ( + start_response.header().reason_code(), + api::COMMAND_REASON_CODE_SAFETY_LATCHED); + + auto stop_request = moveRequest(coordinator, "ptz-stop"); + api::MoveJ_Response stop_response; + grpc::ServerContext stop_context; + auto stop_policy = *registered; + stop_policy.access = GrpcAccessClass::Stop; + stop_policy.command_intent = safety::CommandIntent::Stop; + stop_policy.safety_lane = true; + int stop_dispatches = 0; + const auto stop_status = executeServerDerivedGrpcCommand( + makeDefaultGrpcSecurityGateway(), + &stop_context, + coordinator, + "/cmvr.api.CameraService/ControlPtz", + std::move(stop_policy), + &stop_request, + &stop_response, + [&](GrpcCommandTransaction& transaction) { + if (!transaction.beginDispatch()) { + return transaction.dispatchStatus(); + } + ++stop_dispatches; + stop_response.mutable_header()->set_success(true); + return grpc::Status::OK; + }); + EXPECT_TRUE(stop_status.ok()); + EXPECT_TRUE(stop_response.header().success()); + EXPECT_EQ(stop_dispatches, 1); + EXPECT_EQ(endpoint->hardware_checks.load(), 1); +} + +} // namespace +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp index dae08370..da8898fa 100644 --- a/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp @@ -668,7 +668,8 @@ TEST_F(MotorServiceTest, EXPECT_TRUE(blocked_response.status().emergency_stopped()); } -TEST_F(MotorServiceTest, ProfileBackendExceptionReturnsInternalAndQuickStops) +TEST_F(MotorServiceTest, + ProfileBackendExceptionReportsUnknownOutcomeAndQuickStops) { protocol_->throw_profile_position_ = true; @@ -682,12 +683,22 @@ TEST_F(MotorServiceTest, ProfileBackendExceptionReturnsInternalAndQuickStops) api::MotorCommandResponse response; const auto status = service_->profilePosition(&context, &request, &response); - EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_EQ(status.error_code(), grpc::StatusCode::ABORTED); EXPECT_FALSE(response.header().success()); + EXPECT_EQ( + response.header().reason_code(), + api::COMMAND_REASON_CODE_OUTCOME_UNKNOWN); EXPECT_NE(response.header().error_message().find( "injected profile position exception"), std::string::npos); EXPECT_GE(protocol_->quick_stop_count_.load(), 1); + const auto safety = device::DeviceManager::getInstance() + .safetyCoordinator() + .snapshot(); + ASSERT_EQ(safety.devices.size(), 1U); + EXPECT_EQ( + safety.devices.front().admission_state, + safety::DeviceAdmissionState::Quarantined); } TEST_F(MotorServiceTest, ExceptionCleanupKeepsMotorReservedUntilQuickStopFinishes) @@ -732,7 +743,10 @@ TEST_F(MotorServiceTest, ExceptionCleanupKeepsMotorReservedUntilQuickStopFinishe &competing_context, &competing_request, &competing_response); failing.join(); - EXPECT_EQ(failing_status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_EQ(failing_status.error_code(), grpc::StatusCode::ABORTED); + EXPECT_EQ( + failing_response.header().reason_code(), + api::COMMAND_REASON_CODE_OUTCOME_UNKNOWN); EXPECT_EQ(competing_status.error_code(), grpc::StatusCode::RESOURCE_EXHAUSTED); } diff --git a/cmvr-es/service/grpc/tests/grpc_security_test.cpp b/cmvr-es/service/grpc/tests/grpc_security_test.cpp new file mode 100644 index 00000000..1a21dfe6 --- /dev/null +++ b/cmvr-es/service/grpc/tests/grpc_security_test.cpp @@ -0,0 +1,294 @@ +#include "service/grpc/include/grpc_security.h" + +#include +#include +#include +#include + +#include +#include + +#include "cmvr/api/agv_service.pb.h" +#include "cmvr/api/arm_service.pb.h" +#include "cmvr/api/arm_teleop_v1.pb.h" +#include "cmvr/api/biohead_service.pb.h" +#include "cmvr/api/camera_service.pb.h" +#include "cmvr/api/dexhand_service.pb.h" +#include "cmvr/api/hlc_service.pb.h" +#include "cmvr/api/microphone_service.pb.h" +#include "cmvr/api/motor_service.pb.h" +#include "cmvr/api/speaker_service.pb.h" +#include "cmvr/api/system_service.pb.h" +#include "cmvr/api/test_service.pb.h" + +namespace cmvr::service { +namespace { + +GrpcMethodPolicy readPolicy() +{ + return { + "/cmvr.api.SystemService/GetSystemInfo", + GrpcAccessClass::Read, + GrpcRole::Observer}; +} + +GrpcMethodPolicy recoveryPolicy() +{ + return { + "/cmvr.api.SystemService/RecoverSafetyState", + GrpcAccessClass::Recover, + GrpcRole::SafetyAdmin}; +} + +class SafetyAdminProvider final : public GrpcAuthenticationProvider { +public: + GrpcAuthenticationResult authenticate(const GrpcCallFacts&) const override + { + GrpcPrincipal principal; + principal.id = "maintenance"; + principal.method = GrpcAuthenticationMethod::StaticToken; + principal.authenticated = true; + principal.roles = {GrpcRole::SafetyAdmin}; + return {std::move(principal), grpc::Status::OK}; + } +}; + +TEST(GrpcSecurityConfigTest, + LegacyConfigPreservesAnonymousAccessButDisablesRecovery) +{ + config::GRPCServerConfig config; + const auto result = resolveGrpcSecurityConfig(config, "0.0.0.0"); + + ASSERT_TRUE(result.valid) << result.error; + EXPECT_TRUE(result.config.legacy_compatibility); + EXPECT_TRUE(result.config.insecure_non_loopback); + EXPECT_EQ( + result.config.authentication, GrpcAuthenticationMethod::Disabled); + EXPECT_EQ( + result.config.recovery_exposure, GrpcRecoveryExposure::Disabled); + EXPECT_FALSE(result.warnings.empty()); +} + +TEST(GrpcSecurityConfigTest, + ExplicitNonLoopbackInsecureRequiresAcknowledgement) +{ + config::GRPCServerConfig config; + auto* security = config.mutable_security(); + security->set_transport_mode(config::GRPCSecurityConfig::INSECURE); + security->set_authentication_mode(config::GRPCSecurityConfig::DISABLED); + security->set_recovery_exposure( + config::GRPCSecurityConfig::RECOVERY_DISABLED); + + const auto rejected = resolveGrpcSecurityConfig(config, "0.0.0.0"); + EXPECT_FALSE(rejected.valid); + + security->set_allow_insecure_non_loopback(true); + const auto accepted = resolveGrpcSecurityConfig(config, "0.0.0.0"); + ASSERT_TRUE(accepted.valid) << accepted.error; + EXPECT_TRUE(accepted.config.insecure_non_loopback); +} + +TEST(GrpcSecurityConfigTest, + UnsupportedAuthenticationNeverFallsBackToDisabled) +{ + config::GRPCServerConfig config; + auto* security = config.mutable_security(); + security->set_transport_mode(config::GRPCSecurityConfig::INSECURE); + security->set_authentication_mode( + config::GRPCSecurityConfig::STATIC_TOKEN); + security->set_recovery_exposure( + config::GRPCSecurityConfig::RECOVERY_AUTHORIZED); + + const auto result = resolveGrpcSecurityConfig(config, "127.0.0.1"); + EXPECT_FALSE(result.valid); + EXPECT_NE(result.error.find("DISABLED"), std::string::npos); +} + +TEST(GrpcSecurityConfigTest, EnabledRecoveryRequiresPersistentAuditFile) +{ + config::GRPCServerConfig config; + auto* security = config.mutable_security(); + security->set_transport_mode(config::GRPCSecurityConfig::INSECURE); + security->set_authentication_mode(config::GRPCSecurityConfig::DISABLED); + security->set_recovery_exposure( + config::GRPCSecurityConfig::RECOVERY_LOCAL_ONLY); + + const auto rejected = + resolveGrpcSecurityConfig(config, "127.0.0.1"); + EXPECT_FALSE(rejected.valid); + EXPECT_NE(rejected.error.find("audit_file"), std::string::npos); + + security->set_audit_file("/var/lib/cmvr-es/recovery-audit.jsonl"); + const auto accepted = + resolveGrpcSecurityConfig(config, "127.0.0.1"); + ASSERT_TRUE(accepted.valid) << accepted.error; + EXPECT_EQ( + accepted.config.recovery_audit_file, + "/var/lib/cmvr-es/recovery-audit.jsonl"); +} + +TEST(GrpcSecurityGatewayTest, DisabledProviderDoesNotTrustIdentityMetadata) +{ + auto gateway = makeDefaultGrpcSecurityGateway(); + GrpcCallFacts facts; + facts.peer = "ipv4:10.0.0.5:12345"; + facts.metadata.emplace("principal", "safety-admin"); + facts.metadata.emplace("role", "SafetyAdmin"); + + const auto call = gateway->beginCall(std::move(facts), readPolicy()); + + ASSERT_TRUE(call.allowed()) << call.status().error_message(); + EXPECT_EQ(call.context().principal.id, "anonymous"); + EXPECT_FALSE(call.context().principal.authenticated); + ASSERT_EQ(call.context().principal.roles.size(), 1U); + EXPECT_EQ(call.context().principal.roles.front(), GrpcRole::Anonymous); +} + +TEST(GrpcSecurityGatewayTest, RecoveryIsDisabledBeforeLedgerAdmission) +{ + auto gateway = makeDefaultGrpcSecurityGateway(); + const auto call = gateway->beginCall(GrpcCallFacts{}, recoveryPolicy()); + + EXPECT_FALSE(call.allowed()); + EXPECT_EQ( + call.status().error_code(), grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_EQ(call.status().error_message(), "RECOVERY_RPC_DISABLED"); +} + +TEST(GrpcSecurityGatewayTest, LocalOnlyRecoveryUsesActualPeerClassification) +{ + GrpcSecurityRuntimeConfig config; + config.recovery_exposure = GrpcRecoveryExposure::LocalOnly; + auto gateway = makeGrpcSecurityGateway(config); + + GrpcCallFacts remote; + remote.peer = "ipv4:192.168.1.20:42000"; + EXPECT_FALSE(gateway->beginCall(remote, recoveryPolicy()).allowed()); + + GrpcCallFacts local; + local.peer = "ipv4:127.0.0.1:42000"; + EXPECT_TRUE(gateway->beginCall(local, recoveryPolicy()).allowed()); + + EXPECT_TRUE(isLocalGrpcPeer("ipv6:[::1]")); + EXPECT_TRUE(isLocalGrpcPeer("unix:/run/cmvr-es.sock")); + EXPECT_FALSE(isLocalGrpcPeer("ipv4:10.0.0.1:50051")); +} + +TEST(GrpcSecurityGatewayTest, AuthorizedRecoveryAcceptsSafetyAdminProvider) +{ + GrpcSecurityRuntimeConfig config; + config.authentication = GrpcAuthenticationMethod::StaticToken; + config.recovery_exposure = GrpcRecoveryExposure::Authorized; + auto gateway = std::make_shared( + config, + std::make_shared(), + std::make_shared( + GrpcRecoveryExposure::Authorized)); + + const auto call = gateway->beginCall(GrpcCallFacts{}, recoveryPolicy()); + ASSERT_TRUE(call.allowed()) << call.status().error_message(); + EXPECT_EQ(call.context().principal.id, "maintenance"); +} + +TEST(GrpcSecurityGatewayTest, AuditUsesEffectiveServerPrincipal) +{ + std::vector records; + GrpcSecurityRuntimeConfig config; + auto gateway = makeGrpcSecurityGateway( + config, + [&records](const GrpcSecurityAuditRecord& record) { + records.push_back(record); + }); + + GrpcCallFacts facts; + facts.metadata.emplace("x-correlation-id", "request-42"); + ASSERT_TRUE(gateway->beginCall(std::move(facts), readPolicy()).allowed()); + + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records.front().correlation_id, "request-42"); + EXPECT_EQ(records.front().principal_id, "anonymous"); + EXPECT_FALSE(records.front().authenticated); +} + +TEST(GrpcMethodPolicyRegistryTest, RejectsDuplicatesAndSortsSnapshot) +{ + GrpcMethodPolicyRegistry registry; + EXPECT_TRUE(registry.registerPolicy(recoveryPolicy())); + EXPECT_TRUE(registry.registerPolicy(readPolicy())); + EXPECT_FALSE(registry.registerPolicy(readPolicy())); + + const auto policies = registry.snapshot(); + ASSERT_EQ(policies.size(), 2U); + EXPECT_LT(policies[0].full_method_name, policies[1].full_method_name); + EXPECT_TRUE(registry.find(readPolicy().full_method_name).has_value()); + EXPECT_FALSE(registry.find("/unknown/method").has_value()); +} + +TEST(GrpcMethodPolicyRegistryTest, + DefaultRegistryCoversEveryCompiledApiMethod) +{ + const auto& registry = defaultGrpcMethodPolicyRegistry(); + const auto* pool = google::protobuf::DescriptorPool::generated_pool(); + const std::vector services{ + "cmvr.api.AgvService", + "cmvr.api.ArmService", + "cmvr.api.armteleop.v1.ArmTeleopService", + "cmvr.api.BioHeadService", + "cmvr.api.CameraService", + "cmvr.api.DexHandService", + "cmvr.api.HlcService", + "cmvr.api.MicPhoneService", + "cmvr.api.MotorService", + "cmvr.api.SpeakerService", + "cmvr.api.SystemService", + "cmvr.api.TestService"}; + + std::size_t method_count = 0; + for (const auto& service_name : services) { + const auto* service = pool->FindServiceByName(service_name); + ASSERT_NE(service, nullptr) << service_name; + for (int index = 0; index < service->method_count(); ++index) { + const auto* method = service->method(index); + const std::string full_name = + "/" + service_name + "/" + + std::string(method->name()); + const auto policy = registry.find(full_name); + ASSERT_TRUE(policy.has_value()) << full_name; + EXPECT_EQ( + policy->mutating, + policy->access != GrpcAccessClass::Read) << full_name; + if (policy->mutating) { + EXPECT_NE( + policy->command_intent, + safety::CommandIntent::Observe) << full_name; + } + ++method_count; + } + } + EXPECT_EQ(registry.snapshot().size(), method_count); +} + +TEST(GrpcMethodPolicyRegistryTest, + TouchIsAControlActuationRatherThanAReadOnlySensorCall) +{ + const auto policy = defaultGrpcMethodPolicyRegistry().find( + "/cmvr.api.HlcService/touch"); + ASSERT_TRUE(policy.has_value()); + EXPECT_EQ(policy->access, GrpcAccessClass::Mutate); + EXPECT_EQ(policy->command_intent, safety::CommandIntent::Actuate); + EXPECT_EQ(policy->policy_family, safety::SafetyPolicyFamily::Control); + EXPECT_TRUE(policy->mutating); + EXPECT_FALSE(policy->safety_lane); +} + +TEST(GrpcMethodPolicyRegistryTest, UnregisteredMethodFailsClosed) +{ + const auto call = beginRegisteredGrpcCall( + makeDefaultGrpcSecurityGateway(), nullptr, + "/cmvr.api.UnknownService/Mutate"); + EXPECT_FALSE(call.allowed()); + EXPECT_EQ(call.status().error_code(), grpc::StatusCode::INTERNAL); +} + +} // namespace +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp index ab7adbff..7181aafd 100644 --- a/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp @@ -34,6 +34,8 @@ #include "service/grpc/include/camera_operational_activity_registry.h" #include "service/grpc/include/camera_ptz_activity_registry.h" #include "service/grpc/include/grpc_camera_service.h" +#include "service/grpc/include/grpc_recovery_audit.h" +#include "service/grpc/include/grpc_security.h" #include "service/grpc/include/media_activity_coordinator.h" #include "service/grpc/include/motor_activity_coordinator.h" #include "service/stop_all/include/stop_all_admission_gate.h" @@ -42,6 +44,46 @@ namespace cmvr::service { namespace { +class AllowAllAuthorizationPolicy final : public GrpcAuthorizationPolicy { +public: + GrpcAuthorizationDecision authorize( + const GrpcRequestContext&, + const GrpcMethodPolicy&) const override + { + return {true, grpc::Status::OK}; + } +}; + +class MemoryRecoveryAuditSink final : public RecoveryAuditSink { +public: + bool append( + const RecoveryAuditRecord& record, + std::string* error) noexcept override + { + if (fail) { + if (error) { + *error = "injected audit failure"; + } + return false; + } + records.push_back(record); + return true; + } + + bool fail{false}; + std::vector records; +}; + +std::shared_ptr makeAllowAllRecoveryGateway() +{ + GrpcSecurityRuntimeConfig config; + config.recovery_exposure = GrpcRecoveryExposure::LocalOnly; + return std::make_shared( + config, + std::make_shared(), + std::make_shared()); +} + class SnapshotDevice final : public device::AbstractDevice { public: SnapshotDevice(std::string id, @@ -1288,6 +1330,18 @@ const api::SystemDeviceInfo* findDevice( return nullptr; } +const api::SafetyOperationTargetResult* findSafetyTarget( + const api::StopAllCommand_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 @@ -1299,6 +1353,7 @@ protected: globalStopAllAdmissionGate().clearForTesting(); globalCameraOperationalActivityRegistry().clearForTesting(); globalCameraPtzActivityRegistry().clearForTesting(); + globalMediaActivityCoordinator().clearForTesting(); globalMotorActivityCoordinator().clearForTesting(); device::DeviceManager::destroyInstance(); } @@ -1320,6 +1375,7 @@ protected: globalStopAllAdmissionGate().clearForTesting(); globalCameraOperationalActivityRegistry().clearForTesting(); globalCameraPtzActivityRegistry().clearForTesting(); + globalMediaActivityCoordinator().clearForTesting(); globalMotorActivityCoordinator().clearForTesting(); (void)media::globalMediaSourceHub().stopAllSources(); } @@ -1510,6 +1566,122 @@ TEST_F(GrpcSystemServiceTest, EmptyListReturnsMetadataAndTimestamps) EXPECT_LE(response.sampled_at_unix_ms(), after_ms); } +TEST_F(GrpcSystemServiceTest, SafetyStateUsesCoordinatorMemorySnapshot) +{ + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + registerDevice( + manager, + std::make_shared( + "safety-camera", + device::DeviceKind::Camera, + "SafetyCamera", + device::DeviceHealthSnapshot{ + device::DeviceHealthState::Healthy, {}})); + service_ = std::make_unique(); + + api::GetSafetyStateCommand_Request request; + api::GetSafetyStateCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->GetSafetyState( + &context, &request, &response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_FALSE(response.control_service_instance_id().empty()); + EXPECT_EQ( + response.header().service_instance_id(), + response.control_service_instance_id()); + EXPECT_GT(response.safety_epoch(), 0U); + ASSERT_EQ(response.devices_size(), 1); + EXPECT_EQ(response.devices(0).device_id(), "safety-camera"); + EXPECT_EQ(response.devices(0).policy_family(), "Sensor"); + ASSERT_GT(response.participants_size(), 0); + EXPECT_FALSE(response.participants(0).participant_id().empty()); + EXPECT_FALSE(response.participants(0).phase().empty()); + EXPECT_TRUE(response.participants(0).registered()); + + api::GetSystemInfoCommand_Request info_request; + api::GetSystemInfoCommand_Feedback info_response; + grpc::ServerContext info_context; + ASSERT_TRUE(service_->GetSystemInfo( + &info_context, &info_request, &info_response).ok()); + EXPECT_EQ( + info_response.control_service_instance_id(), + response.control_service_instance_id()); + EXPECT_EQ(info_response.safety_schema_version(), 1U); +} + +TEST_F(GrpcSystemServiceTest, RecoveryIsDisabledByDefault) +{ + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + service_ = std::make_unique(); + + api::RecoverSafetyStateCommand_Request request; + request.set_recovery_id("recovery-disabled"); + request.mutable_scope()->set_all_devices(true); + request.set_expected_safety_epoch( + manager.safetyCoordinator().snapshot().safety_epoch); + request.set_mode(api::RecoverSafetyStateCommand::VERIFY_ONLY); + request.set_reason("diagnostic verification"); + api::RecoverSafetyStateCommand_Feedback response; + grpc::ServerContext context; + + const auto status = service_->RecoverSafetyState( + &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; + auto& manager = device::DeviceManager::getInstance(config); + manager.registerDevice(std::make_shared( + "audit-camera", device::DeviceKind::Camera, "AuditCamera")); + auto audit = std::make_shared(); + service_ = std::make_unique( + std::chrono::seconds(1), makeAllowAllRecoveryGateway(), audit); + + api::RecoverSafetyStateCommand_Request request; + request.set_recovery_id("recovery-audited"); + request.mutable_scope()->set_all_devices(true); + request.set_expected_safety_epoch( + manager.safetyCoordinator().snapshot().safety_epoch); + request.set_mode(api::RecoverSafetyStateCommand::VERIFY_ONLY); + request.set_reason("verify the local work cell"); + request.set_timeout_ms(500); + api::RecoverSafetyStateCommand_Feedback response; + grpc::ServerContext context; + + const auto status = service_->RecoverSafetyState( + &context, &request, &response); + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_EQ(audit->records.size(), 2U); + EXPECT_EQ(audit->records.front().stage, "accepted"); + EXPECT_EQ(audit->records.back().stage, "completed"); + EXPECT_EQ(response.recovery_id(), "recovery-audited"); + + const auto epoch_before_failure = + manager.safetyCoordinator().snapshot().safety_epoch; + audit->fail = true; + request.set_recovery_id("recovery-audit-fails"); + request.set_expected_safety_epoch(epoch_before_failure); + response.Clear(); + grpc::ServerContext failed_context; + const auto failed = service_->RecoverSafetyState( + &failed_context, &request, &response); + EXPECT_EQ(failed.error_code(), grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_EQ( + response.header().reason_code(), + api::COMMAND_REASON_CODE_RECOVERY_AUDIT_FAILED); + EXPECT_EQ( + manager.safetyCoordinator().snapshot().safety_epoch, + epoch_before_failure); +} + TEST_F(GrpcSystemServiceTest, MapsEveryKnownDeviceKind) { struct ExpectedMapping { @@ -1605,6 +1777,77 @@ TEST_F(GrpcSystemServiceTest, ActionQueueExecutesFourMoveLStepsSerially) "arm:L:1", "arm:L:2", "arm:L:3", "arm:L:4"})); } +TEST_F(GrpcSystemServiceTest, + ActionQueueRejectsStaleDeviceGenerationBeforeStepDispatch) +{ + config::DeviceManagerConfig config; + config.mutable_safety()->set_mode( + config::SafetyCoordinatorConfig::ENFORCE_ALL); + auto& manager = device::DeviceManager::getInstance(config); + action_trace_ = std::make_shared(); + action_arm_ = std::make_shared( + "action-arm", action_trace_); + manager.registerDevice(action_arm_); + manager.safetyCoordinator().markStartupComplete(); + std::optional current_device_generation; + const auto snapshot_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (std::chrono::steady_clock::now() < snapshot_deadline) { + const auto safety = manager.safetyCoordinator().snapshot(); + ASSERT_EQ( + safety.system_state, safety::SystemAdmissionState::Open); + const auto safety_device = std::find_if( + safety.devices.begin(), safety.devices.end(), + [this](const auto& device) { + return device.descriptor.device_id == action_arm_->id(); + }); + if (safety_device != safety.devices.end() && + safety_device->safety.fresh) { + current_device_generation = + safety_device->safety.snapshot.device_generation; + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + ASSERT_TRUE(current_device_generation.has_value()); + service_ = std::make_unique(); + + api::GetSystemInfoCommand_Request info_request; + api::GetSystemInfoCommand_Feedback info_response; + grpc::ServerContext info_context; + ASSERT_TRUE(service_->GetSystemInfo( + &info_context, &info_request, &info_response).ok()); + + api::ActionQueueCommand_Request request; + request.set_action_id("stale-step-generation"); + request.set_expected_service_instance_id( + info_response.action_service_instance_id()); + auto* step = addMoveLStep( + request, "move", action_arm_->id(), 1.0); + step->mutable_arm_move_l() + ->mutable_header() + ->set_expected_device_generation( + *current_device_generation + 1U); + + api::ActionQueueCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->ExecuteActionQueue( + &context, &request, &response); + + EXPECT_TRUE(status.ok()) << status.error_message(); + EXPECT_FALSE(response.header().success()); + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ(response.completed_steps(), 0U); + ASSERT_TRUE(response.has_failed_step_index()); + EXPECT_EQ(response.failed_step_index(), 0U); + EXPECT_NE( + response.header().error_message().find("generation"), + std::string::npos) + << response.header().error_message(); + EXPECT_EQ(action_arm_->motionCalls(), 0); + EXPECT_TRUE(action_trace_->names().empty()); +} + TEST_F(GrpcSystemServiceTest, ActionQueuePreservesArmDelayAgvOrder) { initializeActionDevices(); @@ -2432,14 +2675,14 @@ TEST_F(GrpcSystemServiceTest, auto trace = std::make_shared(); auto arm = std::make_shared( "health-blocked-arm", trace); - manager.registerDevice(arm); - service_ = std::make_unique(); - arm->blockHealthSnapshot(); - auto health_snapshot = std::async( - std::launch::async, [&manager] { return manager.snapshot(); }); + auto registration = std::async( + std::launch::async, [&manager, arm] { + manager.registerDevice(arm); + }); const bool health_call_blocked = arm->waitForHealthSnapshot( std::chrono::milliseconds(500)); + service_ = std::make_unique(); api::StopAllCommand_Request request; auto stop_all = std::async(std::launch::async, [this, &request] { @@ -2456,9 +2699,9 @@ TEST_F(GrpcSystemServiceTest, arm->releaseHealthSnapshot(); ASSERT_EQ( - health_snapshot.wait_for(std::chrono::seconds(1)), + registration.wait_for(std::chrono::seconds(1)), std::future_status::ready); - (void)health_snapshot.get(); + registration.get(); ASSERT_EQ( stop_all.wait_for(std::chrono::seconds(1)), std::future_status::ready); @@ -2634,7 +2877,11 @@ TEST_F(GrpcSystemServiceTest, EXPECT_TRUE(second_status.ok()) << second_status.error_message(); EXPECT_TRUE(second_response.header().success()) << second_response.header().error_message(); - EXPECT_GE(action_arm_->stopMotionCalls(), 4); + EXPECT_EQ(first_response.operation_id(), second_response.operation_id()); + EXPECT_EQ( + first_response.current_safety_epoch(), + second_response.current_safety_epoch()); + EXPECT_EQ(action_arm_->stopMotionCalls(), 2); } TEST_F(GrpcSystemServiceTest, @@ -2694,9 +2941,13 @@ TEST_F(GrpcSystemServiceTest, ASSERT_TRUE(status.ok()) << status.error_message(); EXPECT_FALSE(response.header().success()); - EXPECT_NE( - response.header().error_message().find("remain quarantined"), - std::string::npos); + const auto* agv_target = findSafetyTarget(response, action_agv_->id()); + ASSERT_NE(agv_target, nullptr); + EXPECT_EQ( + agv_target->reason_code(), + api::COMMAND_REASON_CODE_DEVICE_STILL_MOVING); + EXPECT_EQ( + agv_target->result(), api::SAFETY_OPERATION_RESULT_FAILED); const auto lease = control::ControlAuthorityManager::instance().tryAcquire( action_agv_->id(), @@ -2734,11 +2985,11 @@ TEST_F(GrpcSystemServiceTest, << response.header().error_message(); EXPECT_EQ(camera->stopRecordingCalls(), 1); EXPECT_FALSE(camera->isRecording()); - EXPECT_EQ(microphone->stopRecordingCalls(), 1); + EXPECT_EQ(microphone->stopRecordingCalls(), 2); EXPECT_FALSE(microphone->isRecording()); EXPECT_EQ(speaker->stopPlaybackCalls(), 2); EXPECT_EQ(camera->lifecycleStopCalls(), 0); - EXPECT_EQ(camera->operationalStopCalls(), 1); + EXPECT_GE(camera->operationalStopCalls(), 2); EXPECT_FALSE(camera->operationalActive()); EXPECT_EQ(microphone->lifecycleStopCalls(), 0); EXPECT_EQ(speaker->lifecycleStopCalls(), 0); @@ -2765,7 +3016,7 @@ TEST_F(GrpcSystemServiceTest, ASSERT_TRUE(status.ok()) << status.error_message(); ASSERT_TRUE(response.header().success()) << response.header().error_message(); - EXPECT_EQ(camera->operationalStopCalls(), 1); + EXPECT_EQ(camera->operationalStopCalls(), 2); EXPECT_FALSE(camera->operationalActive()); EXPECT_EQ(camera->lifecycleStopCalls(), 0); } @@ -2935,10 +3186,12 @@ TEST_F(GrpcSystemServiceTest, ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); EXPECT_FALSE(stop_response.header().success()); - EXPECT_NE( - stop_response.header().error_message().find( - "could not confirm that every device stopped"), - std::string::npos); + const auto* camera_target = findSafetyTarget( + stop_response, camera->id()); + ASSERT_NE(camera_target, nullptr); + EXPECT_EQ( + camera_target->reason_code(), + api::COMMAND_REASON_CODE_STOP_UNCONFIRMED); EXPECT_EQ(camera->stopRecordingCalls(), 2); EXPECT_TRUE(camera->isRecording()); EXPECT_EQ(microphone->stopRecordingCalls(), 2); @@ -3025,7 +3278,7 @@ TEST_F(GrpcSystemServiceTest, } TEST_F(GrpcSystemServiceTest, - StopAllSharesTimedOutArmStopAndDetailAcrossServiceInstances) + RetriedStopAllReusesTimedOutArmBarrierAndCanRecover) { config::DeviceManagerConfig config; auto& manager = device::DeviceManager::getInstance(config); @@ -3059,153 +3312,47 @@ TEST_F(GrpcSystemServiceTest, const bool final_stop_started = initial_stop_completed && arm->waitForBlockedStopMotion(std::chrono::milliseconds(500)); - std::optional> - result; - std::optional> - second_result; - std::unique_ptr second_service; - std::promise last_destroy_started; - auto last_destroy_started_signal = last_destroy_started.get_future(); - std::promise replacement_construct_started; - auto replacement_construct_started_signal = - replacement_construct_started.get_future(); - std::future first_service_destroy; - std::future last_service_destroy; - std::future> - replacement_service_construct; - std::optional first_destroy_before_release; - std::optional last_destroy_before_release; - std::optional last_destroy_after_release; - std::optional replacement_before_release; - std::optional replacement_after_release; - std::unique_ptr replacement_service; - bool dispatcher_destruction_observed{false}; - int stop_calls_before_second{-1}; - int stop_calls_after_second{-1}; - control::ControlAcquireResult control_while_failed_closed; - if (completion == std::future_status::ready) { - result = stop_all.get(); - stop_calls_before_second = arm->stopMotionCalls(); - second_service = std::make_unique( - std::chrono::milliseconds(300)); - api::StopAllCommand_Feedback second_response; - grpc::ServerContext second_context; - const auto second_status = second_service->StopAll( - &second_context, &request, &second_response); - second_result.emplace(second_status, std::move(second_response)); - stop_calls_after_second = arm->stopMotionCalls(); - control_while_failed_closed = authority.tryAcquire( - arm->id(), "control-after-timed-out-stop-all", - std::chrono::seconds(30)); - first_service_destroy = std::async( - std::launch::async, - [this] { service_.reset(); }); - first_destroy_before_release = - first_service_destroy.wait_for(std::chrono::seconds(1)); - if (*first_destroy_before_release == std::future_status::ready) { - first_service_destroy.get(); - last_service_destroy = std::async( - std::launch::async, - [&second_service, &last_destroy_started] { - last_destroy_started.set_value(); - second_service.reset(); - }); - last_destroy_started_signal.wait(); - dispatcher_destruction_observed = - gRPCSystemServiceImpl:: - waitForStopDispatcherDestructionForTesting( - std::chrono::seconds(1)); - if (dispatcher_destruction_observed) { - last_destroy_before_release = - last_service_destroy.wait_for( - std::chrono::milliseconds::zero()); - replacement_service_construct = std::async( - std::launch::async, - [&replacement_construct_started] { - replacement_construct_started.set_value(); - return std::make_unique( - std::chrono::milliseconds(300)); - }); - replacement_construct_started_signal.wait(); - replacement_before_release = - replacement_service_construct.wait_for( - std::chrono::milliseconds(100)); - } - } - } + ASSERT_EQ(completion, std::future_status::ready); + const auto [status, response] = stop_all.get(); + const auto stop_calls_before_retry = arm->stopMotionCalls(); + const auto control_while_failed_closed = authority.tryAcquire( + arm->id(), "control-after-timed-out-stop-all", + std::chrono::seconds(30)); - // Always release both test blocks before an assertion can abort the test; - // the dispatcher owns the final stop worker past the RPC deadline. + // Let the deadline-overrunning final typed stop finish before retrying the + // retained barrier. The old control owner is then allowed to drain. arm->releaseBlockedStopMotion(); authority.release(old_handler.token); - if (first_service_destroy.valid()) { - if (first_service_destroy.wait_for(std::chrono::seconds(1)) == - std::future_status::ready) { - first_service_destroy.get(); - } + + auto second_service = std::make_unique( + std::chrono::seconds(1)); + api::StopAllCommand_Feedback second_response; + grpc::ServerContext second_context; + const auto second_status = second_service->StopAll( + &second_context, &request, &second_response); + const auto recovered_control = authority.tryAcquire( + arm->id(), "control-after-retried-stop-all", + std::chrono::seconds(30)); + if (recovered_control.acquired) { + authority.release(recovered_control.token); } - if (last_service_destroy.valid()) { - last_destroy_after_release = - last_service_destroy.wait_for(std::chrono::seconds(1)); - if (*last_destroy_after_release == std::future_status::ready) { - last_service_destroy.get(); - } - } - if (replacement_service_construct.valid()) { - replacement_after_release = - replacement_service_construct.wait_for(std::chrono::seconds(1)); - if (*replacement_after_release == std::future_status::ready) { - replacement_service = replacement_service_construct.get(); - } - } - replacement_service.reset(); EXPECT_TRUE(initial_stop_completed); EXPECT_TRUE(final_stop_started); - ASSERT_EQ(completion, std::future_status::ready); - ASSERT_TRUE(result.has_value()); - ASSERT_TRUE(second_result.has_value()); - const auto& [status, response] = *result; - const auto& [second_status, second_response] = *second_result; ASSERT_TRUE(status.ok()) << status.error_message(); EXPECT_FALSE(response.header().success()); - EXPECT_NE( - response.header().error_message().find( - "timed out waiting for the preempted RobotArm control handler " - "to exit"), - std::string::npos); + const auto* failed_arm = findSafetyTarget(response, arm->id()); + ASSERT_NE(failed_arm, nullptr); EXPECT_EQ( - response.header().error_message().find( - "stop operation did not complete before the deadline"), - std::string::npos); + failed_arm->reason_code(), + api::COMMAND_REASON_CODE_PARTICIPANT_TIMEOUT); ASSERT_TRUE(second_status.ok()) << second_status.error_message(); - EXPECT_FALSE(second_response.header().success()); - EXPECT_NE( - second_response.header().error_message().find( - "timed out waiting for the preempted RobotArm control handler " - "to exit"), - std::string::npos); - EXPECT_EQ( - second_response.header().error_message().find( - "stop operation did not complete before the deadline"), - std::string::npos); - EXPECT_EQ(stop_calls_before_second, 2); - EXPECT_EQ(stop_calls_after_second, stop_calls_before_second); + EXPECT_TRUE(second_response.header().success()) + << second_response.header().error_message(); + EXPECT_EQ(stop_calls_before_retry, 2); + EXPECT_GE(arm->stopMotionCalls(), 4); EXPECT_FALSE(control_while_failed_closed.acquired); - ASSERT_TRUE(first_destroy_before_release.has_value()); - EXPECT_EQ( - *first_destroy_before_release, std::future_status::ready); - EXPECT_TRUE(dispatcher_destruction_observed); - ASSERT_TRUE(last_destroy_before_release.has_value()); - ASSERT_TRUE(last_destroy_after_release.has_value()); - ASSERT_TRUE(replacement_before_release.has_value()); - ASSERT_TRUE(replacement_after_release.has_value()); - EXPECT_EQ( - *last_destroy_before_release, std::future_status::timeout); - EXPECT_EQ(*last_destroy_after_release, std::future_status::ready); - EXPECT_EQ( - *replacement_before_release, std::future_status::timeout); - EXPECT_EQ(*replacement_after_release, std::future_status::ready); + EXPECT_TRUE(recovered_control.acquired) << recovered_control.detail; } TEST_F(GrpcSystemServiceTest, diff --git a/cmvr-es/service/grpc/tests/media_activity_coordinator_test.cpp b/cmvr-es/service/grpc/tests/media_activity_coordinator_test.cpp index eef4d910..f93155f6 100644 --- a/cmvr-es/service/grpc/tests/media_activity_coordinator_test.cpp +++ b/cmvr-es/service/grpc/tests/media_activity_coordinator_test.cpp @@ -257,6 +257,23 @@ int main() CHECK_TRUE(coordinator.finishStopAll(concurrent_ticket_b, true)); CHECK_TRUE(coordinator.beginSession()); + { + cmvr::service::MediaActivityCoordinator detailed_coordinator; + const auto first = detailed_coordinator.beginStopAll(true); + const auto second = detailed_coordinator.beginStopAll(true); + const auto first_result = + detailed_coordinator.finishStopAllDetailed(first, true); + CHECK_TRUE(first_result.ticket_consumed); + CHECK_TRUE(first_result.participant_stopped); + CHECK_TRUE(!first_result.admission_resumed); + const auto second_result = + detailed_coordinator.finishStopAllDetailed(second, true); + CHECK_TRUE(second_result.ticket_consumed); + CHECK_TRUE(second_result.participant_stopped); + CHECK_TRUE(second_result.admission_resumed); + CHECK_TRUE(detailed_coordinator.beginSession()); + } + std::cout << "media_activity_coordinator_test: PASS\n"; return 0; } diff --git a/cmvr-es/service/grpc/tests/motor_activity_coordinator_test.cpp b/cmvr-es/service/grpc/tests/motor_activity_coordinator_test.cpp index 8a6d9c8d..703f51b4 100644 --- a/cmvr-es/service/grpc/tests/motor_activity_coordinator_test.cpp +++ b/cmvr-es/service/grpc/tests/motor_activity_coordinator_test.cpp @@ -261,6 +261,23 @@ int main() CHECK_TRUE(coordinator.finishStopAll(recovery_ticket, true)); CHECK_TRUE(coordinator.lockAdmission().accepting()); + { + MotorActivityCoordinator detailed_coordinator; + const auto first = detailed_coordinator.beginStopAll(true); + const auto second = detailed_coordinator.beginStopAll(true); + const auto first_result = + detailed_coordinator.finishStopAllDetailed(first, true); + CHECK_TRUE(first_result.ticket_consumed); + CHECK_TRUE(first_result.participant_stopped); + CHECK_TRUE(!first_result.admission_resumed); + const auto second_result = + detailed_coordinator.finishStopAllDetailed(second, true); + CHECK_TRUE(second_result.ticket_consumed); + CHECK_TRUE(second_result.participant_stopped); + CHECK_TRUE(second_result.admission_resumed); + CHECK_TRUE(detailed_coordinator.lockAdmission().accepting()); + } + std::cout << "motor_activity_coordinator_test: PASS\n"; return 0; } diff --git a/cmvr-es/service/quic_edge/src/quic_edge_service.cpp b/cmvr-es/service/quic_edge/src/quic_edge_service.cpp index 7081a9df..0cdc02e2 100644 --- a/cmvr-es/service/quic_edge/src/quic_edge_service.cpp +++ b/cmvr-es/service/quic_edge/src/quic_edge_service.cpp @@ -24,6 +24,8 @@ #include "cmvr/quic_edge/v1/quic_edge.pb.h" #include "common/base/logging/logger.h" +#include "manager/device_manager/include/device_manager.h" +#include "manager/media_source_hub/include/device_media_source_adapter.h" namespace cmvr::quic_edge { namespace { @@ -1448,6 +1450,18 @@ void QuicEdgeService::refreshMediaTracks( recordMediaError(source_error); continue; } + safety::DispatchGuard source_dispatch; + if (using_global_media_hub_) { + source_dispatch = media::beginMediaSourceStartDispatch( + device::DeviceManager::getInstance().safetyCoordinator(), + track_config.device_id()); + if (!source_dispatch.acquired()) { + recordMediaError( + "MediaSourceHub safety admission rejected: " + + track.source_track_id); + continue; + } + } track.subscription = media_hub_->subscribe( track.source_track_id, media::MediaSourceHub::StartPosition::LATEST_AVAILABLE, diff --git a/cmvr-es/task/grpc_server_task/include/grpc_server_task.h b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h index bf8a5513..0a275eed 100644 --- a/cmvr-es/task/grpc_server_task/include/grpc_server_task.h +++ b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h @@ -13,6 +13,8 @@ namespace cmvr::service { class ArmTeleopBackend; +class GrpcSecurityGateway; +class RecoveryAuditSink; } namespace cmvr::task { @@ -66,6 +68,8 @@ private: std::unique_ptr arm_teleop_service_; std::shared_ptr arm_teleop_backend_; + std::shared_ptr security_gateway_; + std::shared_ptr recovery_audit_sink_; std::unique_ptr motor_service_; std::unique_ptr agv_service_; std::unique_ptr hlc_service_; diff --git a/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp index c7e46a9f..4356d607 100644 --- a/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp +++ b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp @@ -1,5 +1,6 @@ #include "task/grpc_server_task/include/grpc_server_task.h" +#include #include #include @@ -14,6 +15,8 @@ #include "service/grpc/include/grpc_arm_service.h" #include "service/grpc/include/grpc_arm_teleop_service.h" #include "service/grpc/include/grpc_robot_arm_teleop_backend.h" +#include "service/grpc/include/grpc_recovery_audit.h" +#include "service/grpc/include/grpc_security.h" #include "service/grpc/include/grpc_camera_service.h" #include "service/grpc/include/grpc_dexhand_service.h" #include "service/grpc/include/grpc_error_logging_interceptor.h" @@ -29,6 +32,13 @@ namespace cmvr::task { namespace { +std::uint64_t unixTimeMs() noexcept +{ + const auto value = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + return value > 0 ? static_cast(value) : 1U; +} + std::shared_ptr createGrpcServerTask(const config::TaskConfigEntry& entry) { if (entry.id().empty()) { @@ -104,20 +114,35 @@ bool GrpcServerTask::start() camera_service_ = std::make_unique( service::makeCameraStreamLowLatencyConfig( cfg_.camera_stream_max_pending_frames(), - cfg_.camera_stream_max_frame_age_ms())); - system_service_ = std::make_unique(); - speaker_service_ = std::make_unique(); - microphone_service_ = std::make_unique(); - dexhand_service_ = std::make_unique(); - biohand_service_ = std::make_unique(); - arm_service_ = std::make_unique(); + cfg_.camera_stream_max_frame_age_ms()), + security_gateway_); + system_service_ = std::make_unique( + std::chrono::seconds(15), + security_gateway_, + recovery_audit_sink_); + speaker_service_ = + std::make_unique(security_gateway_); + microphone_service_ = + std::make_unique(security_gateway_); + dexhand_service_ = + std::make_unique(security_gateway_); + biohand_service_ = + std::make_unique(security_gateway_); + arm_service_ = + std::make_unique(security_gateway_); arm_teleop_service_ = std::make_unique( arm_teleop_backend_ ? arm_teleop_backend_ - : service::makeDisabledArmTeleopBackend()); - motor_service_ = std::make_unique(); - agv_service_ = std::make_unique(); - hlc_service_ = std::make_unique(); + : service::makeDisabledArmTeleopBackend(), + nullptr, + security_gateway_, + &device::DeviceManager::getInstance().safetyCoordinator()); + motor_service_ = + std::make_unique(security_gateway_); + agv_service_ = + std::make_unique(security_gateway_); + hlc_service_ = + std::make_unique(security_gateway_); grpc::ServerBuilder builder; builder.AddListeningPort(local_address, grpc::InsecureServerCredentials()); @@ -151,7 +176,15 @@ bool GrpcServerTask::start() address_ = local_address; state_ = TaskState::RUNNING; - CMVR_LOG(INFO) << "[GrpcServerTask] gRPC server started, address=" << address_; + const auto& security = security_gateway_->config(); + CMVR_LOG(INFO) << "[GrpcServerTask] gRPC server started, address=" << address_ + << ", transport=" << service::toString(security.transport) + << ", authentication=" + << service::toString(security.authentication) + << ", recovery=" + << service::toString(security.recovery_exposure) + << ", insecure_non_loopback=" + << security.insecure_non_loopback; wait_thread_ = std::thread(&GrpcServerTask::waitLoop, this); return true; } @@ -170,6 +203,54 @@ bool GrpcServerTask::init() return false; } + const std::string effective_host = + cfg_.host().empty() ? "0.0.0.0" : cfg_.host(); + const auto security_result = + service::resolveGrpcSecurityConfig(cfg_, effective_host); + if (!security_result.valid) { + last_error_ = "invalid gRPC security config: " + security_result.error; + CMVR_LOG(ERROR) << "[GrpcServerTask] " << last_error_; + state_ = TaskState::FAILED; + return false; + } + for (const auto& warning : security_result.warnings) { + CMVR_LOG(WARNING) << "[GrpcServerTask] " << warning; + } + security_gateway_ = service::makeGrpcSecurityGateway( + security_result.config, + [](const service::GrpcSecurityAuditRecord& record) { + if (!record.allowed) { + CMVR_LOG(WARNING) + << "[gRPC security] request denied, correlation_id=" + << record.correlation_id + << ", method=" << record.full_method_name + << ", principal=" << record.principal_id + << ", peer=" << record.peer + << ", code=" << static_cast(record.status_code); + } + }); + recovery_audit_sink_.reset(); + if (security_result.config.recovery_exposure != + service::GrpcRecoveryExposure::Disabled) { + recovery_audit_sink_ = service::makeFileRecoveryAuditSink( + security_result.config.recovery_audit_file); + service::RecoveryAuditRecord audit_probe; + audit_probe.occurred_at_unix_ms = unixTimeMs(); + audit_probe.stage = "sink_initialized"; + audit_probe.principal_id = "system"; + audit_probe.result = "ready"; + std::string audit_error; + if (!recovery_audit_sink_->append(audit_probe, &audit_error)) { + last_error_ = + "recovery audit initialization failed: " + audit_error; + CMVR_LOG(ERROR) << "[GrpcServerTask] " << last_error_; + recovery_audit_sink_.reset(); + security_gateway_.reset(); + state_ = TaskState::FAILED; + return false; + } + } + arm_teleop_backend_ = service::makeDisabledArmTeleopBackend(); if (cfg_.has_arm_teleop_backend() && cfg_.arm_teleop_backend().enable()) { diff --git a/cmvr-es/task/touch_screen_task/include/touch_screen_task.h b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h index 76ae242b..4198c9e6 100644 --- a/cmvr-es/task/touch_screen_task/include/touch_screen_task.h +++ b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h @@ -61,11 +61,22 @@ public: RETRACTING, // 正在回退离开屏幕。 DONE, // 流程成功完成。 STOPPED, // 被外部 stop() 主动停止。 + SAFETY_ADMISSION_REVOKED, // 统一安全会话在硬件下发前失效。 ROBOT_STATE_FAILED, // 读取机器人状态失败。 ROBOT_COMMAND_FAILED, // 向机器人下发控制命令失败。 TASK_BUSY // 已有触屏流程正在运行，新的 touch 请求被拒绝。 }; + struct SafetyHooks { + using HardwareOperation = std::function; + using Dispatch = + std::function; + + std::function revalidate; + Dispatch dispatch_actuation; + Dispatch dispatch_stop; + }; + explicit TouchScreenTask(const cmvr::config::TouchScreenTaskConfig& cfg); ~TouchScreenTask() = default; @@ -79,7 +90,8 @@ public: bool touchIfCurrent( int u, int v, - const std::function& still_admitted); + const std::function& still_admitted, + SafetyHooks safety_hooks = {}); bool touch(int u, int v); bool startFromPixel(int u, int v); bool step(double dt) override; @@ -110,6 +122,7 @@ public: int lastTouchNonzeroCount() const; int lastActiveTagId() const; Eigen::Vector3d lastAlignErrorCamera() const; + std::string controlDeviceId() const; const std::shared_ptr& perception() const { return perception_; } const perception::TagRelativeTarget3D& tracker() const { return tracker_; } @@ -129,6 +142,12 @@ private: bool activityControlCurrent() const; std::function activityCancellationRequested() const; control::ControlDispatchGuard tryBeginActivityDispatch() const; + bool activitySafetyCurrent() const; + bool runArmActuationIfCurrent( + const SafetyHooks::HardwareOperation& operation) const; + bool runArmStopIfCurrent( + const SafetyHooks::HardwareOperation& operation) const; + void clearActivitySafetyHooksUnlocked() noexcept; void releaseActivityControlUnlocked() noexcept; void finishActivityUnlocked(Phase phase, Status status) noexcept; bool startFromPixelUnlocked(int u, int v); @@ -183,6 +202,7 @@ private: std::atomic activity_active_{false}; std::atomic activity_generation_{1U}; control::ControlLeaseToken activity_control_token_; + SafetyHooks activity_safety_hooks_; bool target_locked_{false}; bool ibvs_target_initialized_{false}; bool touch_command_started_{false}; diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_admission_test.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_admission_test.cpp index 2a78ed1c..f8ef3ecc 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_admission_test.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_admission_test.cpp @@ -52,6 +52,12 @@ public: task.phase_ = TouchScreenTask::Phase::IDLE; task.last_status_ = TouchScreenTask::Status::NOT_INITIALIZED; } + + static bool sendJointVelocity(TouchScreenTask& task) + { + std::lock_guard lock(task.mutex_); + return task.sendJointVelocity({}); + } }; } // namespace cmvr::task @@ -106,6 +112,7 @@ public: cmvr::device::Result speedJ( const cmvr::device::JointVelocityCommand&, double, double) override { + ++speed_j_calls_; return success(); } cmvr::device::Result stopJ(double) override { return success(); } @@ -128,7 +135,11 @@ public: { return success(); } - cmvr::device::Result stopMotion() override { return success(); } + cmvr::device::Result stopMotion() override + { + ++stop_motion_calls_; + return success(); + } cmvr::device::Result startServoMode( const cmvr::device::ServoOptions&) override { @@ -199,11 +210,20 @@ public: } bool busy() const override { return false; } + int speedJCalls() const noexcept { return speed_j_calls_.load(); } + int stopMotionCalls() const noexcept + { + return stop_motion_calls_.load(); + } + private: static cmvr::device::Result success() { return cmvr::device::Result::success(); } + + std::atomic speed_j_calls_{0}; + std::atomic stop_motion_calls_{0}; }; class AdmissionCamera final : public cmvr::device::AbstractCamera { @@ -356,6 +376,82 @@ TEST_F(TouchScreenAdmissionTest, EXPECT_TRUE(task.stopActivity()); } +TEST_F(TouchScreenAdmissionTest, + SafetyHooksFenceEveryArmActuation) +{ + auto arm = std::make_shared(); + auto camera = std::make_shared( + "touch-safety-hooks-camera"); + cmvr::task::TouchScreenTask task(cmvr::config::TouchScreenTaskConfig{}); + cmvr::task::TouchScreenTaskAdmissionTestPeer::prepare( + task, arm, camera); + + bool admitted = true; + int revalidations = 0; + int dispatches = 0; + cmvr::task::TouchScreenTask::SafetyHooks hooks; + hooks.revalidate = [&] { + ++revalidations; + return admitted; + }; + hooks.dispatch_actuation = [&](const auto& operation) { + ++dispatches; + return operation(); + }; + hooks.dispatch_stop = [](const auto& operation) { + return operation(); + }; + + ASSERT_TRUE(task.touchIfCurrent( + 10, 20, [] { return true; }, std::move(hooks))); + EXPECT_TRUE( + cmvr::task::TouchScreenTaskAdmissionTestPeer::sendJointVelocity(task)); + EXPECT_EQ(arm->speedJCalls(), 1); + EXPECT_EQ(dispatches, 1); + EXPECT_GE(revalidations, 2); + + admitted = false; + EXPECT_FALSE( + cmvr::task::TouchScreenTaskAdmissionTestPeer::sendJointVelocity(task)); + EXPECT_EQ(arm->speedJCalls(), 1); + EXPECT_EQ(dispatches, 1); + EXPECT_TRUE(task.stopActivity()); +} + +TEST_F(TouchScreenAdmissionTest, + RevokedSafetySessionStopsActivityThroughStopLane) +{ + auto arm = std::make_shared(); + auto camera = std::make_shared( + "touch-revoked-safety-camera"); + cmvr::task::TouchScreenTask task(cmvr::config::TouchScreenTaskConfig{}); + cmvr::task::TouchScreenTaskAdmissionTestPeer::prepare( + task, arm, camera); + + bool admitted = true; + int stop_dispatches = 0; + cmvr::task::TouchScreenTask::SafetyHooks hooks; + hooks.revalidate = [&] { return admitted; }; + hooks.dispatch_actuation = [](const auto& operation) { + return operation(); + }; + hooks.dispatch_stop = [&](const auto& operation) { + ++stop_dispatches; + return operation(); + }; + + ASSERT_TRUE(task.touchIfCurrent( + 10, 20, [] { return true; }, std::move(hooks))); + admitted = false; + EXPECT_FALSE(task.step(0.01)); + EXPECT_EQ(task.phase(), cmvr::task::TouchScreenTask::Phase::FAILED); + EXPECT_EQ( + task.lastStatus(), + cmvr::task::TouchScreenTask::Status::SAFETY_ADMISSION_REVOKED); + EXPECT_EQ(stop_dispatches, 1); + EXPECT_EQ(arm->stopMotionCalls(), 1); +} + TEST_F(TouchScreenAdmissionTest, StopAllStopsCameraActivityAndNextTouchRestartsIt) { diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp index d3a739a4..7ec8dc77 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp @@ -467,7 +467,8 @@ bool TouchScreenTask::touch(const int u, const int v) { bool TouchScreenTask::touchIfCurrent( const int u, const int v, - const std::function& still_admitted) + const std::function& still_admitted, + SafetyHooks safety_hooks) { auto& admission_gate = service::globalStopAllAdmissionGate(); std::uint64_t admission_generation = 0U; @@ -534,9 +535,21 @@ bool TouchScreenTask::touchIfCurrent( return false; } + resetActivityUnlocked(); + activity_safety_hooks_ = std::move(safety_hooks); + if (!activitySafetyCurrent()) { + last_status_ = Status::SAFETY_ADMISSION_REVOKED; + activity_active_.store(false, std::memory_order_release); + clearActivitySafetyHooksUnlocked(); + releaseActivityControlUnlocked(); + rollback_camera(); + return false; + } + const bool started = startFromPixelUnlocked(u, v); if (!started) { activity_active_.store(false, std::memory_order_release); + clearActivitySafetyHooksUnlocked(); releaseActivityControlUnlocked(); rollback_camera(); return false; @@ -556,6 +569,7 @@ bool TouchScreenTask::touchIfCurrent( } activity_active_.store(false, std::memory_order_release); resetActivityUnlocked(); + clearActivitySafetyHooksUnlocked(); releaseActivityControlUnlocked(); rollback_camera(); return false; @@ -643,6 +657,61 @@ TouchScreenTask::tryBeginActivityDispatch() const token); } +bool TouchScreenTask::activitySafetyCurrent() const +{ + if (!activity_safety_hooks_.revalidate) { + return true; + } + try { + return activity_safety_hooks_.revalidate(); + } catch (...) { + return false; + } +} + +bool TouchScreenTask::runArmActuationIfCurrent( + const SafetyHooks::HardwareOperation& operation) const +{ + if (!operation || !activitySafetyCurrent()) { + return false; + } + auto authority_dispatch = tryBeginActivityDispatch(); + if (!authority_dispatch.acquired()) { + return false; + } + try { + return activity_safety_hooks_.dispatch_actuation + ? activity_safety_hooks_.dispatch_actuation(operation) + : operation(); + } catch (...) { + return false; + } +} + +bool TouchScreenTask::runArmStopIfCurrent( + const SafetyHooks::HardwareOperation& operation) const +{ + if (!operation) { + return false; + } + auto authority_dispatch = tryBeginActivityDispatch(); + if (!authority_dispatch.acquired()) { + return false; + } + try { + return activity_safety_hooks_.dispatch_stop + ? activity_safety_hooks_.dispatch_stop(operation) + : operation(); + } catch (...) { + return false; + } +} + +void TouchScreenTask::clearActivitySafetyHooksUnlocked() noexcept +{ + activity_safety_hooks_ = {}; +} + void TouchScreenTask::releaseActivityControlUnlocked() noexcept { control::ControlLeaseToken token; @@ -663,6 +732,7 @@ void TouchScreenTask::finishActivityUnlocked( touch_command_started_ = false; retract_command_started_ = false; activity_active_.store(false, std::memory_order_release); + clearActivitySafetyHooksUnlocked(); releaseActivityControlUnlocked(); } @@ -680,7 +750,6 @@ bool TouchScreenTask::startFromPixelUnlocked(int u, int v) { return false; } - resetActivityUnlocked(); if (!moveToInitPositionBeforeStartIfEnabled()) { return false; } @@ -724,9 +793,19 @@ bool TouchScreenTask::step(const double dt) { !activityControlCurrent()) { activity_active_.store(false, std::memory_order_release); resetActivityUnlocked(); + clearActivitySafetyHooksUnlocked(); releaseActivityControlUnlocked(); return true; } + if (activity_active_.load(std::memory_order_acquire) && + !activitySafetyCurrent()) { + (void)runArmStopIfCurrent([this] { + return arm_ && arm_->stopMotion().ok(); + }); + finishActivityUnlocked( + Phase::FAILED, Status::SAFETY_ADMISSION_REVOKED); + return false; + } if (!initialized_) { last_status_ = Status::NOT_INITIALIZED; return false; @@ -840,6 +919,7 @@ bool TouchScreenTask::stopActivity() { isBusyUnlocked()) { resetActivityUnlocked(); } + clearActivitySafetyHooksUnlocked(); releaseActivityControlUnlocked(); } @@ -977,6 +1057,14 @@ Eigen::Vector3d TouchScreenTask::lastAlignErrorCamera() const { return last_align_error_camera_; } +std::string TouchScreenTask::controlDeviceId() const { + std::lock_guard lock(mutex_); + if (arm_ && !arm_->id().empty()) { + return arm_->id(); + } + return config_.devices().arm_id(); +} + std::string TouchScreenTask::stateString() const { return taskStateToString(state()); } @@ -1020,6 +1108,7 @@ const char* TouchScreenTask::statusToString(const Status status) { case Status::RETRACTING: return "RETRACTING"; case Status::DONE: return "DONE"; case Status::STOPPED: return "STOPPED"; + case Status::SAFETY_ADMISSION_REVOKED: return "SAFETY_ADMISSION_REVOKED"; case Status::ROBOT_STATE_FAILED: return "ROBOT_STATE_FAILED"; case Status::ROBOT_COMMAND_FAILED: return "ROBOT_COMMAND_FAILED"; case Status::TASK_BUSY: return "TASK_BUSY"; @@ -1688,9 +1777,9 @@ bool TouchScreenTask::stepRetracting() { << ", final_tcp_delta_base=unavailable"; } - try { - arm_->stopL(); - } catch (...) { + if (!runArmStopIfCurrent([this] { + return arm_ && arm_->stopL().ok(); + })) { enterFailed(Status::ROBOT_COMMAND_FAILED); return false; } @@ -1738,17 +1827,11 @@ bool TouchScreenTask::sendJointVelocity(const std::vector& qdot) const { return false; } - auto dispatch = tryBeginActivityDispatch(); - if (!dispatch.acquired()) { - return false; - } device::JointVelocityCommand cmd; cmd.velocity = qdot; - const auto result = arm_->speedJ(cmd, 0.0, 0.0); - if (!result.ok()) { - return false; - } - return true; + return runArmActuationIfCurrent([this, &cmd] { + return arm_ && arm_->speedJ(cmd, 0.0, 0.0).ok(); + }); } bool TouchScreenTask::sendZeroJointVelocity() const { @@ -1835,15 +1918,9 @@ bool TouchScreenTask::holdCurrentControlledPosition() const { joints.position.push_back(it->second); } - auto dispatch = tryBeginActivityDispatch(); - if (!dispatch.acquired()) { - return false; - } - const auto result = arm_->servoJ(joints); - if (!result.ok()) { - return false; - } - return true; + return runArmActuationIfCurrent([this, &joints] { + return arm_ && arm_->servoJ(joints).ok(); + }); } bool TouchScreenTask::buildInitJointPositions(std::vector& positions_out) const { @@ -1870,8 +1947,9 @@ bool TouchScreenTask::moveToInitPositionBeforeStartIfEnabled() { options.velocity = config_.initialization().velocity(); options.acceleration = config_.initialization().acceleration(); options.cancellation_requested = activityCancellationRequested(); - const auto result = arm_->moveJ(init_cmd, options); - if (!result.ok()) { + if (!runArmActuationIfCurrent([this, &init_cmd, &options] { + return arm_ && arm_->moveJ(init_cmd, options).ok(); + })) { last_status_ = Status::ROBOT_COMMAND_FAILED; return false; } @@ -1892,17 +1970,16 @@ bool TouchScreenTask::moveToInitPositionIfEnabled() const { options.velocity = config_.initialization().velocity(); options.acceleration = config_.initialization().acceleration(); options.cancellation_requested = activityCancellationRequested(); - const auto result = arm_->moveJ(init_cmd, options); - if (!result.ok()) { - return false; - } - return true; + return runArmActuationIfCurrent([this, &init_cmd, &options] { + return arm_ && arm_->moveJ(init_cmd, options).ok(); + }); } bool TouchScreenTask::handleTouchTriggered(const bool stop_forward_motion) { if (stop_forward_motion) { - const auto result = arm_->stopL(); - if (!result.ok()) { + if (!runArmStopIfCurrent([this] { + return arm_ && arm_->stopL().ok(); + })) { return false; } } @@ -1936,17 +2013,15 @@ bool TouchScreenTask::startTouchPhase() { last_status_ = Status::ROBOT_STATE_FAILED; return false; } - auto dispatch = tryBeginActivityDispatch(); - if (!dispatch.acquired()) { - last_status_ = Status::ROBOT_COMMAND_FAILED; - return false; - } - const auto result = arm_->speedL(toCartesianVelocity( - cmvr::common::math::toEigenVec6(speed_l.twist_tool())), - speed_l.acceleration(), - 0.0, - device::FrameType::Tool); - if (!result.ok()) { + const auto velocity = toCartesianVelocity( + cmvr::common::math::toEigenVec6(speed_l.twist_tool())); + if (!runArmActuationIfCurrent([this, velocity, &speed_l] { + return arm_ && arm_->speedL( + velocity, + speed_l.acceleration(), + 0.0, + device::FrameType::Tool).ok(); + })) { last_status_ = Status::ROBOT_COMMAND_FAILED; return false; } @@ -1973,8 +2048,10 @@ bool TouchScreenTask::startTouchPhase() { options.joint_velocity_limits.assign(move_l.joint_velocity_limits().begin(), move_l.joint_velocity_limits().end()); options.cancellation_requested = activityCancellationRequested(); - const auto result = arm_->moveL(pose_cmd, options, device::FrameType::Tool); - if (!result.ok()) { + if (!runArmActuationIfCurrent([this, &pose_cmd, &options] { + return arm_ && arm_->moveL( + pose_cmd, options, device::FrameType::Tool).ok(); + })) { last_status_ = Status::ROBOT_COMMAND_FAILED; return false; } @@ -2018,17 +2095,14 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract, << ", start_tcp_base=unavailable"; } - auto dispatch = tryBeginActivityDispatch(); - if (!dispatch.acquired()) { - return false; - } - const auto result = arm_->speedL(retract_cmd, - retract.acceleration(), - 0.0, - device::FrameType::Tool); - if (!result.ok()) { - CMVR_LOG(ERROR) << "[TouchScreenTask][RETRACT_START] speedL failed: " - << result.message; + if (!runArmActuationIfCurrent([this, &retract_cmd, &retract] { + return arm_ && arm_->speedL( + retract_cmd, + retract.acceleration(), + 0.0, + device::FrameType::Tool).ok(); + })) { + CMVR_LOG(ERROR) << "[TouchScreenTask][RETRACT_START] speedL failed"; return false; } @@ -2043,12 +2117,9 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract, } void TouchScreenTask::enterFailed(const Status status) { - try { - if (arm_) { - arm_->stopL(); - } - } catch (...) { - } + (void)runArmStopIfCurrent([this] { + return arm_ && arm_->stopL().ok(); + }); hardStopIbvsMotion(); holdCurrentControlledPosition(); const auto final_status = moveToInitPositionIfEnabled() diff --git a/docs/device_safety_control_plane_architecture.md b/docs/device_safety_control_plane_architecture.md new file mode 100644 index 00000000..693e53a5 --- /dev/null +++ b/docs/device_safety_control_plane_architecture.md @@ -0,0 +1,1821 @@ +# CMVR-ES 统一设备安全控制面架构设计 + +状态：Implemented in software；默认 Shadow rollout；真实硬件与发布验收待完成 + +修订：2026-08-14，具体认证实现延后，保留统一安全扩展边界并收紧匿名 Recover 暴露 + +范围：设备命令准入、控制权、StopAll、软件锁止恢复、gRPC 请求上下文与安全扩展边界、命令幂等、设备安全状态上报 + +实施方式：阶段 0 到阶段 5，共六个阶段 + +## 当前实施状态（2026-08-14） + +当前分支已经完成六阶段所需的软件骨架和主要入口迁移，但没有把“代码已具备能力”误写成 +“生产安全验收已完成”。源码默认配置仍使用 `SHADOW`、`INSECURE + DISABLED` 和 +`RECOVERY_DISABLED`；切换 `ENFORCE_SELECTED/ENFORCE_ALL` 或开放本机恢复前，必须先完成 +对应设备和部署环境的验收。 + +| 阶段 | 当前状态 | 已落地内容 | 尚未完成 | +| --- | --- | --- | --- | +| 0 | 软件完成 | 统一 RequestContext、完整 method policy/call guard、anonymous principal、显式 Insecure/Disabled 配置、RecoveryExposure 和持久审计边界 | TLS、Token、JWT、mTLS provider 按本轮决策不实现 | +| 1 | 软件完成 | safety types、SnapshotStore、设备 adapter、公共 reason/execution state、service instance、CommandLedger | 厂商结果语义仍需逐台真机校准 | +| 2 | 软件完成 | DeviceManager 持有 SafetyCoordinator、legacy participant、GetSafetyState、启动 coverage 校验、Shadow/Enforce 配置 | 真实运行 Shadow 日志评审 | +| 3 | 软件完成 | Arm、ArmTeleop、AGV、Motor、DexHand、PTZ、ActionQueue 统一准入；dispatch 前 permit/final check；PTZ START/STOP 服务端派生 lane | 每类设备真实停车确认和 ACK/断网故障注入 | +| 4 | 软件完成 | 通用 participant StopAll、RecoveryLedger、RecoverSafetyState、LocalOnly/Authorized policy、审计失败 fail-closed、shutdown quiesce | LocalOnly 现场入口和审计文件运维验收 | +| 5 | 部分完成 | Camera/Microphone/Speaker/BioHead/HLC、TouchScreenTask、媒体 Hub/QUIC Sensor start 已迁移；Enforce 启动覆盖校验已实现 | 默认切换 EnforceAll、删除兼容 gate、capability manifest、安装产物 smoke、TSAN 和真机台架 | + +认证和证书不是本次整改的运行前提。当前只实现无证书的兼容 Profile；配置选择尚未实现的 +认证或 TLS 模式会启动失败，不会静默回退。匿名部署的恢复默认关闭，只有显式配置 +`RECOVERY_LOCAL_ONLY`、服务端确认实际 peer 为 loopback/Unix socket 且持久审计可写时才可开放。 + +AUBO 另有一条设备内硬件语义：真实硬件急停曾有效、随后输入消失且控制器重新报告 +`Normal/ReducedMode` 时，驱动会在重新清理队列并确认 quiescent 后自动解除该硬件锁存； +软件 `emergencyStop()` 使用独立 `SoftwareEmergencyStop` 锁存，即使它与硬件急停重叠也绝不被 +硬件输入释放自动清除。自动流程不上电、不 resume、不重放旧目标。 + +## 1. 决策摘要 + +本设计采用以下核心决策： + +1. `DeviceManager` 继续负责设备注册和生命周期，并持有一个独立、可测试的 + `SafetyCoordinator`。状态机代码不直接堆入 `DeviceManager`。 +2. Service 不再自行组合 StopAll gate、设备状态和控制权判断。所有会改变设备或 + 活动状态的命令必须声明 `CommandIntent`，通过统一准入获得短生命周期的 + `AdmissionPermit`。 +3. 设备层不把厂商硬件规则上移。设备或设备适配器持续发布硬件事实，并在真正下发 + 命令前执行最后一次硬件安全检查。 +4. 生命周期、健康状态和安全状态保持正交。`Running`、`Healthy` 都不等于当前可以 + 接受执行器命令。 +5. 策略分为 `SensorPolicy` 和 `ControlPolicy` 两个默认族，但最终按命令意图判定。 + DexHand、Camera PTZ 等混合设备不能只按整个设备归类。 +6. Stop、状态查询和安全恢复使用独立安全通道，不受普通命令 gate 阻塞。 +7. 对外接口命名为 `RecoverSafetyState`。它只在重新验证硬件事实后清除软件准入锁止， + 不能忽略急停、保护停、未知状态或仍未确认的运动。 +8. 普通 unary 控制命令使用统一幂等账本；流式控制使用 session epoch 和严格递增的 + sequence。任何 `OUTCOME_UNKNOWN` 都不得自动重发。 +9. 当前小范围部署不把 TLS、Token、mTLS 等具体认证实现作为设备安全架构的前置条件， + 但阶段 0 必须先建立统一 `RequestContext`、认证提供者、授权策略和审计扩展边界。 +10. 认证关闭时由服务端注入固定的 `anonymous` principal，不能接受客户端自报身份或角色； + `RecoverSafetyState` 默认禁用，只有本机受限模式可以显式开放。 +11. 传输加密、身份认证和方法授权是三个正交层。后续增加静态 Token、JWT 或 mTLS 时， + 只能替换安全网关组件，不修改设备、`SafetyCoordinator` 或业务 Proto。 +12. 该软件控制面不替代独立物理急停，也不声明 SIL、PL 或其他功能安全等级。 + +## 2. 改造前基础与需要保留的行为 + +项目已有一些经过并发测试的能力，应当迁移和复用，而不是重新实现： + +| 当前能力 | 目标用法 | +| --- | --- | +| `ControlAuthorityManager` 的 lease、generation、dispatch fence、quarantine | 作为 `SafetyCoordinator` 内部控制权组件 | +| `StopAllAdmissionGate` 的并发 round 和失败后 fail-closed | 作为阶段 2、3 的兼容参与者，最终由统一状态机接管 | +| `ActionQueueExecutor` 的 `action_id`、service instance 和精确 retired ID 账本 | 作为普通命令幂等账本的语义模板 | +| Motor、Media、Camera activity coordinator | 先包装为 `SafetyParticipant`，最后逐步合并 | +| `StopOperationDispatcher` 的有界异步停止和生命周期管理 | 作为统一 StopAll 执行器的基础 | +| `DeviceManager::inventorySnapshot()` | StopAll 枚举设备时继续使用，禁止持锁进入驱动 | +| `Runtime` 的启动失败回滚和退出时 Task/Device 停止 | 保留，并在退出前增加全局安全准入关闭 | + +整改开始时的主要缺口是： + +- gRPC 默认全网卡明文监听，没有认证和授权；当前阶段可以接受该部署风险，但必须通过显式配置、 + 网络隔离和恢复接口限制控制暴露范围； +- 普通控制 RPC 缺少 `command_id` 和统一重试语义； +- StopAll 在 `grpc_system_service.cpp` 内按设备类型硬编码； +- Arm、AGV、Motor、Media、ActionQueue 分别维护 gate 和 generation； +- 只有少数设备实现 `healthSnapshot()`，且健康状态不能表示硬件安全事实； +- `DeviceManager::snapshot()` 仍会在调用线程逐个调用设备方法； +- 普通 service 直接取得设备并调用驱动，无法保证所有入口都经过同一准入； +- Aubo 等驱动存在“硬件可能已接受，但软件未观察到执行 ID”的不确定结果窗口； +- 错误类型没有统一映射，客户端容易对不应重试的命令进行重试。 + +当前实现锚点： + +- gRPC 监听和 credentials：`cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp`； +- SystemService StopAll：`cmvr-es/service/grpc/src/grpc_system_service.cpp`； +- 全局 gate：`cmvr-es/service/stop_all/include/stop_all_admission_gate.h`； +- 控制权：`cmvr-es/manager/control_authority/include/control_authority_manager.h`； +- 设备生命周期和快照：`cmvr-es/manager/device_manager/`； +- 通用请求头：`protos/cmvr/api/common.proto`； +- System API：`protos/cmvr/api/system_service.proto` 和 `system_command.proto`。 + +## 3. 目标、非目标与安全不变量 + +### 3.1 目标 + +- 所有 gRPC、ActionQueue、Teleoperation 和未来 QUIC 控制入口共享同一安全准入。 +- 新增设备时，安全层只要求注册能力和策略，不修改 StopAll 或 SystemService 主流程。 +- 单个驱动、网络或状态刷新阻塞不能拖住其他设备的 Stop、Status 或 Recover。 +- StopAll、设备级不确定结果和进程重启都能通过 generation 使旧命令永久失效。 +- 锁止原因、状态新鲜度、阻塞设备和恢复结果能够被机器读取和审计。 +- 迁移期间保持现有 API wire compatibility，并允许按设备逐步启用 enforce。 +- 具体认证实现延后时，所有请求仍经过稳定的安全上下文边界；未来启用认证不需要改动 + Service 业务分支、状态机、驱动接口或业务 Proto。 + +### 3.2 非目标 + +- 不把厂商协议、关节限制、工作空间或设备专用故障码搬入 `DeviceManager`。 +- 不允许远程接口绕过真实硬件急停或保护停。 +- 不保证所有设备在一个阶段内同时完成迁移。 +- 不用软件 StopAll 替代机器人、PLC、驱动器或安全控制器的独立安全回路。 +- 不在本设计中实现运行期卸载设备插件；只消除安全协调器中的设备类型硬编码。 +- 首轮六阶段实施不要求交付证书、静态 Token、JWT 或 mTLS；这些能力作为独立的后续安全加固项。 + +### 3.3 必须始终成立的不变量 + +1. `Stop`、`Status`、`GetSafetyState` 和 `RecoverSafetyState` 不依赖普通准入为 OPEN。 +2. 一次 StopAll 开始后，旧 epoch 的命令不能再进入设备下发点。 +3. StopAll 与正常命令竞态时，命令只能是“未下发”或“已被登记并纳入停止”，不能存在 + 未登记的第三种状态。 +4. 状态过期、设备失联、安全能力缺失对控制命令一律 fail-closed。 +5. 同一 effective principal 的同一 `command_id` 最多触发一次硬件提交；认证关闭时所有调用者 + 共享服务端生成的 `anonymous` principal，因此 command ID 必须在整个匿名部署内唯一。 +6. 相同 ID、不同语义 payload 必须返回冲突，不能覆盖旧记录。 +7. 硬件结果不确定时保存 `OUTCOME_UNKNOWN`，重试只能查询该结果，不能再次下发。 +8. 恢复成功只表示软件准入可重新评估，不表示设备被上电、使能、解除急停或自动运动。 +9. `SafetyCoordinator` 持有内部锁时不得调用设备、网络或可能阻塞的 participant 方法。 +10. 驱动最终安全检查失败时，即使已经获得 permit，也不能下发设备命令。 +11. 进程重启后，控制设备在新鲜状态确认完成前不能自动恢复到可控制状态。 +12. 所有安全状态转换都增加 epoch、产生事件，并记录明确 reason code。 +13. 匿名网络调用不能执行 `RecoverSafetyState`；认证关闭时只能显式禁用恢复，或仅允许 + 服务端判定为 loopback/Unix Domain Socket 的本机调用。该限制不能由请求字段覆盖。 + +## 4. 统一概念模型 + +### 4.1 三个互不替代的状态维度 + +| 维度 | 说明 | 示例 | +| --- | --- | --- | +| Lifecycle | 进程对设备对象的生命周期管理 | Initializing、Running、Stopped、Error | +| Health | 设备是否正常工作和是否存在故障 | Healthy、Degraded、Fault、Unknown | +| Safety | 当前事实是否足以接受某一类命令 | Nominal、Restricted、Unsafe、Unknown | + +`Lifecycle=Running` 只说明设备 worker 已启动；`Health=Healthy` 只说明没有已知故障； +两者都不能替代 Safety 中的状态新鲜度、急停、运动、使能和代际信息。 + +### 4.2 命令意图 + +```cpp +enum class CommandIntent { + Observe, // 只读状态、图像、传感数据 + StartActivity, // 启动采集、录制、播放等非运动活动 + Configure, // 修改参数、地图、非安全 IO 等 + Actuate, // 运动、速度、力、位置、使能、扭矩输出 + Stop, // 停车、取消、torque-off、停止活动 + ResetFault, // 设备级、不会自动产生运动的故障复位 + RecoverAdmission, // 系统级软件锁止恢复 +}; +``` + +每个 handler 必须在代码中声明固定意图。客户端不能通过请求字段自行选择意图。 + +### 4.3 两个默认策略族 + +```cpp +enum class SafetyPolicyFamily { + Sensor, + Control, +}; +``` + +- `Sensor` 表示非运动型采集或外围活动，不要求它在字面上一定是传感器。Camera、 + Microphone、Battery、普通 Speaker activity 可使用该策略。 +- `Control` 表示能够导致位置、速度、力、扭矩或机械结构变化的能力。Arm、AGV、Motor、 + DexHand 动作、Camera PTZ、BioHead 表情和仿真执行器使用该策略。 +- 混合设备按命令选择策略。例如 Camera 图像流是 Sensor，PTZ START 是 Control， + PTZ STOP 是 Stop。 + +### 4.4 硬件事实使用三值逻辑 + +安全事实不能把“未实现”解释为 false： + +```cpp +enum class TriState { Unknown, False, True }; + +enum class SafetyCondition { + Nominal, // 已知正常，可继续按具体命令规则判断 + Restricted, // 已知静止或受硬件抑制，但不满足普通执行条件 + Unsafe, // 已观察到不允许继续控制的状态 + Unknown, // 缺失、过期、失联或无法解释 +}; +``` + +推荐的进程内快照： + +```cpp +struct DeviceSafetySnapshot { + std::string device_id; + SafetyCondition condition{SafetyCondition::Unknown}; + std::uint64_t device_generation{0}; + std::uint64_t sample_sequence{0}; + std::chrono::steady_clock::time_point observed_at; + std::uint64_t observed_at_unix_ms{0}; + + TriState connected{TriState::Unknown}; + TriState operational_ready{TriState::Unknown}; + TriState quiescent{TriState::Unknown}; + TriState motion_active{TriState::Unknown}; + TriState actuator_enabled{TriState::Unknown}; + TriState emergency_stop_active{TriState::Unknown}; + TriState protective_stop_active{TriState::Unknown}; + TriState fault_active{TriState::Unknown}; + + std::vector blockers; +}; +``` + +`SafetyBlocker` 必须包含稳定 reason、来源和作用域： + +```cpp +enum class BlockerScope { Device, System }; +enum class RecoveryRequirement { + RefreshOnly, + ClearSoftwareLatch, + HardwareReleaseRequired, + ManualInspectionRequired, +}; + +struct SafetyBlocker { + SafetyReason reason; + BlockerScope scope; + RecoveryRequirement recovery_requirement; + std::string source_id; + std::string operation_id; +}; +``` + +设备本地急停可以只阻塞该设备；连接到整机安全链的急停必须发布 `System` scope，并阻止 +全局准入恢复。作用域由经过评审的设备/安全链 adapter 固定，不能由客户端或普通配置修改。 + +安全判断使用本机 monotonic age。UTC 只用于日志和跨系统诊断，不能参与准入时序。 + +`device_generation` 在以下情况增加： + +- 设备后端重新初始化； +- SDK、总线或网络 session 重连； +- 驱动状态机执行需要使旧命令失效的 reset； +- 设备被重新注册。 + +## 5. 目标架构与所有权 + +```mermaid +flowchart LR + Client["gRPC / QUIC / Local Task"] --> Gateway["Request Context Gateway
限制 可选认证 授权策略 审计"] + Gateway --> Adapter["Typed Service Adapter
声明 CommandIntent"] + Adapter --> Ledger["CommandLedger
幂等和结果"] + Adapter --> Coordinator["DeviceManager::SafetyCoordinator"] + Coordinator --> Policy["SensorPolicy / ControlPolicy"] + Coordinator --> Authority["ControlAuthorityManager"] + Coordinator --> Participants["SafetyParticipant Registry"] + Coordinator --> Cache["SafetySnapshotStore"] + Coordinator --> Permit["AdmissionPermit"] + Permit --> Driver["DeviceSafetyEndpoint + Driver"] + Driver --> Cache + StopRecover["StopAll / Recover 安全通道"] --> Coordinator +``` + +### 5.1 建议目录 + +```text +cmvr-es/manager/safety/ + include/safety_types.h + include/safety_reason.h + include/device_safety_endpoint.h + include/safety_participant.h + include/safety_snapshot_store.h + include/command_admission_controller.h + include/command_ledger.h + include/safety_operation_orchestrator.h + include/safety_coordinator.h + src/... + tests/... + +cmvr-es/service/grpc/security/ + include/grpc_request_context.h + include/grpc_authentication_provider.h + include/grpc_authorization_policy.h + include/grpc_method_policy_registry.h + include/grpc_security_interceptor.h + src/... + tests/... +``` + +新增 CMake target：`cmvr_es::safety_coordinator`。它可以依赖通用类型和 +`ControlAuthorityManager`，但不能依赖 gRPC、具体设备后端或厂商 SDK。 + +### 5.2 `DeviceManager` 的职责变化 + +`DeviceManager` 增加： + +```cpp +SafetyCoordinator& safetyCoordinator() noexcept; +const SafetyCoordinator& safetyCoordinator() const noexcept; +``` + +它负责： + +- 在设备创建后注册设备描述、Safety endpoint 和快照槽位； +- 在设备销毁前注销 participant； +- 在 lifecycle start/stop/restart 时推进 device generation； +- 在 Runtime shutdown 开始时先关闭全局准入； +- 对外提供纯内存的 Manager + Safety 联合快照。 + +它不负责： + +- 解析 Aubo、Huayan、SEER、Modbus 等厂商状态； +- 根据 `DeviceKind` 写具体停止逻辑； +- 在 Manager mutex 下执行硬件 I/O； +- 判断具体运动目标是否超限。 + +### 5.3 依赖注入 + +最终状态下，gRPC service 构造函数显式接收： + +```cpp +DeviceManager& +SafetyCoordinator& +CommandLedger& +GrpcSecurityGateway& +``` + +每个 handler 通过一个统一 call guard 取得不可变 `RequestContext`。认证 metadata 的解析、 +peer 归一化、角色生成和恢复接口暴露检查只存在于 `GrpcSecurityGateway`；业务 Service 不直接 +解析 metadata，也不从 request message 读取 principal 或 role。具体实现可以由 interceptor +完成前置检查，再由 call guard 取得同一次调用的上下文，但不能依赖 thread-local 传递上下文。 + +阶段 2 可以保留无参构造函数作为兼容入口，但它只能转发到上述依赖。阶段 5 删除业务 +代码对进程级 safety singleton 的直接访问。测试使用独立 coordinator，不再依赖 +`clearForTesting()` 清理全局状态。 + +### 5.4 管理面与设备运行面分离 + +“Stop/Status/Recover 始终可达”不仅要求它们绕过普通 gate，也要求单个设备初始化失败时 +gRPC 管理面仍能启动。目标 Runtime 启动顺序为： + +```text +load config and logging + -> construct DeviceManager core and SafetyCoordinator + -> validate selected gRPC security profile and start management-plane services + -> initialize/start devices + -> reconcile required control snapshots + -> start operational tasks + -> Open with device-level Blocked, or global Latched +``` + +- 配置损坏、所选安全 Profile 的身份材料无效或 SafetyCoordinator 核心构造失败仍使进程启动失败； +- `AuthenticationMode::Disabled` 是显式兼容模式，不伪装成“已认证”；如果使用非 loopback + 明文监听，启动日志、GetSystemInfo 和指标必须持续暴露该风险； +- 单个设备 create/init/start 失败记录在 inventory，并使相关资源 Blocked； +- Device scope 的动态 Unknown/Unsafe 使相关资源 Blocked，System scope blocker 使全局 Latched， + 两者都不停止 SystemService； +- 普通设备 service 可以继续注册，访问失败设备时返回结构化 DEVICE_UNAVAILABLE； +- TaskManager 需要区分 ManagementPlane 和 Operational task，后者失败不能回滚前者； +- 物理急停和本机停机手段始终独立于该网络管理面。 + +在完成该拆分前，“始终可达”只保证 gRPC server 已成功运行期间的行为，不能覆盖当前 +Runtime 在任一 required device 启动失败时直接退出的情况。 + +## 6. 快照发布与线程模型 + +### 6.1 发布模型 + +正常准入和状态查询只能读取 `SafetySnapshotStore`，不能同步查询驱动。 + +```text +driver callback / polling worker + | + v +DeviceSafetyEndpoint normalizes vendor facts + | + v +SafetySnapshotPublisher::publish(value) + | + v +SafetySnapshotStore atomic value slot +``` + +现有设备可以先通过独立 adapter worker 拉取状态；新设备应优先在自己的 SDK callback 或 +状态轮询线程中发布。Manager heartbeat、gRPC Status 和准入只复制值。 + +现有 `healthSnapshot()` 在迁移期只能由 DeviceMonitor 后台 worker 调用，并使用每设备隔离和 +超时；不能继续由 `DeviceManager::snapshot()` 的调用线程执行。目标状态是设备同时发布缓存的 +Health 和 Safety，两者仍保持独立字段。 + +### 6.2 Freshness + +每个 `DeviceSafetyDescriptor` 声明默认最大状态年龄。配置只允许将阈值收紧，不能在生产模式下 +放宽超过代码定义的硬上限。 + +- Sensor Observe 可在业务允许时返回带 `stale=true` 的历史值； +- Sensor StartActivity 在 Missing/Stale 时拒绝； +- Control Actuate、Configure 和 ResetFault 在 Missing/Stale 时拒绝； +- Stop 不因状态过期而拒绝，仍直接尝试安全停止； +- Recover 必须获得一个晚于本次 refresh request 的新样本。 + +### 6.3 主动刷新 + +`RecoverSafetyState` 需要主动复核，但仍不在 gRPC 线程直接做设备 I/O： + +1. 记录当前 `sample_sequence`； +2. 调用 endpoint 的非阻塞 `requestSafetyRefresh()`； +3. 在专用 recovery executor 等待 sequence 增加； +4. 超时则产生 `SAFETY_STATE_STALE` blocker，保持锁止。 + +### 6.4 锁和执行器约束 + +- Coordinator 锁只保护状态转换、epoch、slot 和 participant registry； +- Snapshot slot 使用原子 shared snapshot 或短持有独立 mutex； +- `beginDispatch()` 只建立 fence 和 in-flight 计数，不等待物理运动结束； +- 设备调用、停止和刷新均在锁外执行； +- Stop/Recover 使用有界 worker pool，不为每次调用创建 detached thread； +- 每个 legacy health probe 最多允许一个 in-flight worker，卡住后只标记 stale，不重复创建线程； +- participant 注销前必须等待自己的 callback 和 stop handle 退出。 + +## 7. 状态机 + +### 7.1 系统准入状态 + +```mermaid +stateDiagram-v2 + [*] --> Starting + Starting --> Open: required snapshots reconciled + Starting --> Latched: unknown active control or startup failure + Open --> Stopping: StopAll + Stopping --> Open: all participants quiescent + Stopping --> Latched: timeout or unconfirmed stop + Latched --> Recovering: authorized recovery + Recovering --> Open: every required latch cleared + Recovering --> Latched: blocker remains + Starting --> ShuttingDown: shutdown + Open --> ShuttingDown: shutdown + Stopping --> ShuttingDown: shutdown + Latched --> ShuttingDown: shutdown + Recovering --> ShuttingDown: shutdown +``` + +每次进入 `Stopping`、`Latched`、`Recovering` 或 `ShuttingDown` 都增加 `safety_epoch`。 + +### 7.2 设备准入状态 + +```cpp +enum class DeviceAdmissionState { + Observing, // 已注册，等待第一份有效快照 + Open, + Blocked, // 确定的硬件或生命周期条件不满足 + Quarantined, // 命令或停止结果不确定，需要人工恢复流程 + Recovering, + Removed, +}; +``` + +- `Blocked` 可以在新鲜硬件事实改善后自动回到 Open，例如设备故障被合法清除； +- `Quarantined` 不能仅靠一份健康快照自动清除，必须经过 recovery transaction； +- 一个普通设备命令的结果不确定时，默认只 quarantine 对应 control resource； +- StopAll 无法确认全部参与者时，全局状态进入 Latched。 + +### 7.3 命令生命周期 + +```text +RECEIVED + -> RESERVED + -> REJECTED_BEFORE_DISPATCH + -> ADMITTED + -> DISPATCHING + -> ACCEPTED_BY_HARDWARE + -> COMPLETED | FAILED | CANCELED | OUTCOME_UNKNOWN +``` + +`ACCEPTED_BY_HARDWARE` 的含义必须由每个 adapter 明确定义。SDK 返回码、queue ID、任务 ID、 +ACK 或写入总线分别可能具有不同强度，不能统一解释成“运动完成”。 + +## 8. 命令准入算法 + +### 8.1 普通 unary 控制命令 + +1. Request Context Gateway 完成大小限制、correlation ID、上下文构造和授权策略检查；只有 + 当前 Profile 启用 AuthN 时才执行身份认证。 +2. 解析 `command_id`、service instance、device generation 和本地有效期。 +3. CommandLedger 按 effective principal 预留 ID，并计算语义 payload hash；Disabled 模式使用 + 服务端固定的 `anonymous` namespace。 +4. 若存在同 ID 记录：相同 hash 返回/等待原结果；不同 hash 返回冲突。 +5. Service 使用固定 `CommandDescriptor` 调用 `SafetyCoordinator::admit()`。 +6. Coordinator 在短锁内读取 global state、safety epoch、device slot 和 cached snapshot。 +7. Policy 判断意图、freshness、硬件事实、设备状态和调用角色。 +8. Control intent 获取或校验 `ControlAuthorityManager` lease。 +9. 返回 move-only `AdmissionPermit`，其中固定所有 generation 和 deadline。 +10. Service 调用 `beginDispatch(permit)`。这里是 StopAll 与命令提交的线性化点。 +11. 驱动在自己的串行化上下文中执行最终硬件检查。 +12. 通过检查后调用 SDK，并把结果写入 ledger；失败或异常也必须终结 ledger 记录。 + +`AdmissionPermit` 至少包含： + +```cpp +struct AdmissionPermit { + std::string command_id; + std::string device_id; + CommandIntent intent; + std::uint64_t safety_epoch; + std::uint64_t device_generation; + std::uint64_t authority_generation; + std::chrono::steady_clock::time_point deadline; +}; +``` + +### 8.2 Stop 与 ResetFault + +- Stop 走安全通道，不需要普通 permit，也不受全局 Latched 阻塞； +- Stop 仍需经过 RequestContext、当前 Profile 的访问策略、参数校验、超时和审计；认证关闭时 + 不额外拒绝 Stop，因为停止能力必须保持可达； +- Stop 不因 expected service instance、device generation 或 safety epoch 过期而拒绝；这些字段 + 对 Stop 仅用于诊断，因为安全停止必须优先于防重放限制； +- 重复 Stop 应加入当前 stop operation 或执行幂等停止，不能因为缺少 command ID 而拒绝； +- ResetFault 可在 Latched 下执行，但必须声明为不会上电、使能或恢复运动； +- 如果某厂商的 clear-fault 同时会 resume、enable 或移动，必须拆成两个 typed 操作， + 不能把它登记成 ResetFault； +- ResetFault 成功只触发安全快照刷新，不直接清除 central quarantine。 + +### 8.3 流式控制 + +Teleoperation、Motor cyclic stream 和未来连续控制不为每个 setpoint 写 unary ledger： + +1. OPEN frame 生成 `control_session_id`，绑定 service instance、device generation、 + safety epoch 和 control lease； +2. setpoint sequence 必须严格递增，且带本地可解释的 `valid_for`； +3. 每个 setpoint 下发前轻量校验 session epoch、permit epoch、watchdog 和快照 freshness； +4. StopAll 或 Recover 增加 epoch 后，旧 stream 立即进入 HOLD/STOP，不可自动续接； +5. reconnect 必须创建新 session，不能恢复旧 ACTIVE 状态； +6. stream 结束、write/read 失败和 watchdog 均执行可验证的安全停止。 + +### 8.4 ActionQueue + +- 保留现有 ActionQueue `action_id` 和 service instance； +- 预校验阶段确认所有 step 已分类，但不提前获取可长期持有的 permit； +- 每个 step 真正开始前重新准入并取得当前 epoch 的 permit； +- step 的内部 command ID 由 `action_id + step_id` 确定性派生，不能要求嵌套请求再提供一个 + 可与 action 身份冲突的独立 ID； +- StopAll 仍能抢占队列和活动 step； +- 迁移完成后 ActionQueue 不再拥有独立的普通准入 generation，只保留队列执行 generation； +- 队列结果和单步 CommandLedger 记录使用关联 ID，但不得造成一次 step 两套独立重试语义。 + +## 9. 策略矩阵 + +### 9.1 SensorPolicy + +| Intent | Global Latched | Snapshot 要求 | 默认结果 | +| --- | --- | --- | --- | +| Observe | 允许 | 可返回 stale 标记；不能伪造为 fresh | Allow | +| StartActivity | 拒绝或按设备 Blocked | connected、fresh、无已知 fault | Conditional | +| Configure | 拒绝 | connected、fresh、配置操作已分类 | Conditional | +| Stop | 允许 | 不要求 fresh | Allow safety lane | +| ResetFault | 仅明确支持的设备 | fresh，且操作不会启动活动 | Conditional | +| RecoverAdmission | SafetyAdmin only | fresh sample required | Safety lane | + +### 9.2 ControlPolicy + +| Intent | 必要条件 | +| --- | --- | +| Observe | 始终允许读取缓存；返回 freshness 和 blocker | +| Configure | 全局 Open、设备非 Quarantined、fresh、命令不会隐式 Actuate | +| Actuate | 全局 Open、设备 Open、condition=Nominal、fresh、generation 匹配、lease 有效、deadline 有效、最终硬件检查通过 | +| Stop | 无条件进入安全通道；状态未知仍尝试 stop | +| ResetFault | 允许在 Blocked/Latched 下执行，但只允许非使能、非运动的 typed reset | +| RecoverAdmission | SafetyAdmin、expected epoch 匹配、主动刷新完成、quiescent=true、没有未知 stop worker | + +`emergency_stop_active=true` 或 `protective_stop_active=true` 可以表示设备被硬件抑制且物理上 +静止，但不能被 Recover 清除。Device scope blocker 继续阻止该设备 Actuate；System scope +blocker 继续保持全局 Latched。只有硬件被合法处理并发布新的事实后，恢复事务才可清理对应 +软件 latch。 + +## 10. 设备安全能力接口 + +### 10.1 描述与注册 + +```cpp +struct DeviceSafetyDescriptor { + std::string device_id; + DeviceKind kind; + SafetyPolicyFamily default_policy; + std::chrono::milliseconds maximum_snapshot_age; + bool requires_safe_stop; + bool supports_active_refresh; + bool supports_non_enabling_fault_reset; +}; + +struct DeviceSafetyRegistration { + DeviceSafetyDescriptor descriptor; + std::shared_ptr endpoint; + std::shared_ptr participant; +}; +``` + +控制设备在 enforce 模式下缺少 endpoint、fresh snapshot 或 safe-stop participant 时： + +- 启动阶段标记设备 safety capability invalid； +- 设备可以继续出现在 inventory 和诊断接口中； +- 所有 Actuate 命令拒绝； +- 配置要求严格启动时，可以直接使 Runtime 初始化失败。 + +新增全新 DeviceKind 仍可能需要修改现有 DeviceFactory、配置 Proto 和对外业务 API；本设计 +保证的是不再修改 SafetyCoordinator、StopAll 和 Recover 的设备类型分支。 + +### 10.2 Endpoint + +```cpp +class DeviceSafetyEndpoint { +public: + virtual ~DeviceSafetyEndpoint() = default; + + virtual DeviceSafetyDescriptor descriptor() const = 0; + virtual void bindPublisher(SafetySnapshotPublisher publisher) = 0; + virtual void requestSafetyRefresh() noexcept = 0; + + // 在驱动自己的串行化上下文内执行。不得更改设备状态。 + virtual HardwareCheckResult validateBeforeDispatch( + const AdmissionPermit&) = 0; + + // 只协调驱动内部软件状态，不得上电、使能或启动运动。 + virtual RecoveryCheckResult reconcileAdmissionState( + const RecoveryContext&) = 0; +}; +``` + +如果最终检查需要厂商 I/O，它必须有设备级 deadline，并在驱动 executor 中执行；Manager +不持锁等待。超时返回 Unknown，不允许继续下发。 + +### 10.3 Safe-stop participant + +```cpp +class SafetyParticipant { +public: + virtual ParticipantDescriptor descriptor() const = 0; + + // 纯内存、快速关闭本 participant 的新工作准入。 + virtual BarrierToken beginBarrier(const SafetyOperationContext&) = 0; + + // 非阻塞提交停止，返回可等待 handle。 + virtual StopHandle requestQuiesce( + const BarrierToken&, const SafetyOperationContext&) = 0; + + // 验证“旧 generation 不会继续、当前输出已静止”。 + virtual QuiescenceResult verifyQuiescent( + const BarrierToken&, const SafetyOperationContext&) = 0; + + // 只清理软件 gate、retired holder 或内部 session。 + virtual RecoveryCheckResult recoverAdmission( + const BarrierToken&, const RecoveryContext&) = 0; + + virtual void releaseBarrier(const BarrierToken&) noexcept = 0; +}; +``` + +participant 可以代表设备，也可以代表 ActionQueue、MediaSourceHub、Motor session registry 等 +跨设备活动域。StopAll 不再知道具体 C++ 设备类型。 + +## 11. CommandLedger 与结果语义 + +### 11.1 Key 和 payload hash + +推荐 key：`(effective_principal_id, command_id)`。 + +`effective_principal_id` 只能由服务端安全网关产生：认证启用时取认证结果中的稳定 principal ID； +认证关闭时固定为 `anonymous`。不能使用 request、普通 metadata 中自报的 client ID，也不使用 +易变化的 TCP source port。匿名模式因此要求 command ID 在整个部署内全局唯一。 + +payload hash 包含： + +- 完整 gRPC method 名； +- device/resource ID； +- deterministic protobuf semantic payload； +- expected service instance 和 device generation； +- 不包含诊断 timestamp、认证 metadata 和 command ID 本身。 + +同一 effective principal 内 command ID 必须全局唯一。不同 method 复用同一 ID 会因 hash 不同而冲突。 + +### 11.2 记录内容 + +```cpp +struct CommandRecord { + CommandKey key; + PayloadHash payload_hash; + CommandLifecycle lifecycle; + CommandOutcome outcome; + std::uint64_t safety_epoch; + std::uint64_t device_generation; + bool hardware_submission_possible; + steady_clock::time_point accepted_at; + steady_clock::time_point terminal_at; +}; +``` + +- In-flight 和近期 terminal 结果保留完整响应； +- 淘汰完整结果后保留精确 retired-ID tombstone； +- 达到硬容量后拒绝新 ID，不淘汰仍可能被重放的 tombstone； +- 账本耗尽使用 `RESOURCE_EXHAUSTED/LEDGER_EXHAUSTED`，不能降级成无幂等执行； +- 进程内账本不要求落盘，但客户端必须携带 expected service instance； +- 进程重启后旧 instance 请求拒绝，控制设备先完成 startup reconciliation 才可 Open。 + +### 11.3 RPC 取消 + +- 在 RESERVED/ADMITTED 且未 dispatch 时取消：终结为 `CANCELED_BEFORE_DISPATCH`； +- 硬件已接受后客户端断开：不能假设命令取消，继续记录实际结果； +- 需要“断线即停”的命令必须显式使用 session/watchdog 协议； +- retry 相同 ID 只能加入原执行或读取原结果。 + +## 12. StopAll 统一事务 + +### 12.1 Participant 分组 + +建议固定阶段而非依赖注册顺序： + +1. `Ingress`：关闭 gRPC/QUIC/local task 普通准入； +2. `Scheduler`：取消 ActionQueue 和待执行作业； +3. `ControlSession`：撤销 teleop、motor cyclic、velocity 等连续控制 session； +4. `Actuator`：Arm、AGV、Motor、DexHand、PTZ、BioHead 请求安全停止； +5. `PeripheralActivity`：Camera、Microphone、Speaker、recording 和媒体 producer； +6. `Verification`：等待每个 required participant 的 quiescence 证明。 + +组内可以并行，组间顺序固定。每个 participant 有独立 timeout，外层还有总 deadline。 + +### 12.2 算法 + +1. 使用 process-wide operation mutex 创建或加入当前 StopAll round； +2. 状态设为 Stopping，增加 safety epoch，关闭全局准入； +3. 对当前 participant registry 建立不可变快照； +4. 对所有 participant 调用 `beginBarrier()`； +5. 撤销普通 control lease，并等待已进入 dispatch fence 的短提交退出； +6. 分阶段提交 `requestQuiesce()`； +7. 等待 handle，并调用 `verifyQuiescent()`； +8. 任一 required participant 超时、异常或 Unknown：保留其 barrier，记录 blocker，进入 Latched； +9. 全部成功：清理旧 retired holders，释放本轮 barrier，状态回到 Open； +10. 返回结构化 per-participant 结果和新 epoch。 + +多个 StopAll 调用加入同一 round，并各自得到同一最终结果。RPC waiter 取消不能取消已经开始的 +系统停止事务。 + +成功 StopAll 是一个“已经在本轮确认全部静止”的同步屏障，不是持续维护锁。状态回到 Open 后， +其他通过当前访问策略的客户端可以提交新命令，甚至可能在 StopAll 调用方收到响应前完成准入。需要长期禁止 +控制时应设计独立的 maintenance lock，不复用 StopAll 或 Recover 语义。 + +### 12.3 迟到结果 + +- 旧 round 的 worker 完成后只允许更新该 round 的诊断记录； +- worker 不能凭旧 token 重新打开准入； +- participant 的 release 必须验证 operation ID 和 epoch； +- worker 仍运行时 participant 保持 blocker，Recover 不得跳过它。 + +## 13. RecoverSafetyState 事务 + +### 13.1 语义边界 + +`RecoverSafetyState` 的准确含义是： + +> 在独立安全通道中重新确认设备和活动域已经静止，并清除由软件 StopAll、超时、取消、 +> 旧 session 或不确定结果留下的准入锁止。 + +它不执行： + +- 解除物理急停； +- 自动解除保护停； +- torque-on、power-on、brake-release； +- 自动继续旧轨迹、导航、ActionQueue 或 teleop session； +- 把 Unknown 解释为 Safe； +- 调用测试接口 `clearForTesting()`。 + +### 13.2 恢复算法 + +1. Request Context Gateway 先执行恢复暴露策略：认证模式要求 `SafetyAdmin`；认证关闭时默认 + 禁用，只有 `LOCAL_ONLY` 且服务端确认 peer 为 loopback/Unix Domain Socket 才允许继续； + 随后校验 `recovery_id`、scope、reason 和 deadline； +2. RecoveryLedger 对 `recovery_id` 做幂等处理； +3. 与 StopAll 串行化；expected safety epoch 不匹配立即拒绝； +4. 状态转为 Recovering 并增加 epoch，使所有旧 permit/session 失效； +5. 选取 scope 内处于 Blocked/Quarantined 的 device 和 subsystem participant； +6. 确认没有旧 stop worker、dispatch fence 或 active command 仍未退出； +7. 对设备发起 active refresh，等待晚于本次请求的新 snapshot； +8. 控制设备要求 `quiescent=true`，且 condition 不能是 Unsafe/Unknown；任何 System scope + hardware blocker 都保持全局锁止； +9. 调用 endpoint/participant 的非使能 `reconcileAdmissionState()` 和 `recoverAdmission()`； +10. 清理已经验证的 quarantine、retired safety holder 和软件 gate； +11. scope 外 blocker 或失败 participant 继续保留；只有全部 required latch 清除才回到 Open； +12. 返回 previous/new epoch、每个对象的 before/after 状态和 blocker。 + +### 13.3 部分恢复 + +- 请求可以只恢复指定 device，但不能偷偷排除 subsystem blocker； +- 设备级 quarantine 可以单独清除； +- 如果 global gate 仍被其他 participant 持有，系统状态仍为 Latched； +- response 必须区分 `RECOVERED`、`VERIFIED_BUT_STILL_BLOCKED`、`BLOCKER_REMAINS`、 + `EPOCH_MISMATCH` 和 `NOTHING_TO_RECOVER`。 + +## 14. gRPC API 设计 + +### 14.1 扩展公共命令头 + +保持字段 1、2 不变，使用新 tag 增量扩展： + +```protobuf +message CommandHeader { + message Request { + string device_id = 1; + google.protobuf.Timestamp timestamp = 2; // 仅诊断 + string command_id = 3; + string expected_service_instance_id = 4; + optional uint64 expected_device_generation = 5; + uint32 valid_for_ms = 6; + } + + message Feedback { + bool success = 1; + string error_message = 2; + google.protobuf.Timestamp timestamp = 3; + CommandReasonCode reason_code = 4; + string command_id = 5; + string service_instance_id = 6; + uint64 safety_epoch = 7; + uint64 device_generation = 8; + CommandExecutionState execution_state = 9; + } +} +``` + +兼容阶段中旧客户端字段为空： + +- Legacy/Shadow 模式允许，但记录 `missing_command_identity`； +- EnforceSelected 只对已迁移设备要求； +- EnforceAll 下所有普通 mutating unary 命令必须提供； +- Observe 不要求 command ID；安全 Stop 为保持可达性不把 ID/epoch 作为前置条件，但有 ID 时 + 用于合并结果和审计；Recover 必须提供 recovery ID 和 expected safety epoch。 + +`valid_for_ms` 从服务端收到请求的 monotonic time 开始计算，不能信任跨机器 timestamp。 +Safety Stop 忽略已经过期的普通命令有效期，但仍受服务端 stop operation 总 deadline 约束。 + +- `command_id` 建议使用 UUID，服务端至少限制字符集和最大长度； +- `valid_for_ms=0` 在 Legacy/Shadow 下使用有界服务端默认值，不能表示无限； +- EnforceSelected/EnforceAll 可以要求 Actuate 显式提供非零有效期； +- 服务端对所有客户端有效期施加硬上限，retry 不能延长原 ledger 记录的 deadline； +- expected service instance 为空只在兼容模式接受。 + +### 14.2 新增安全 API + +建议新增 `protos/cmvr/api/safety_command.proto`，并在现有 `SystemService` 增加： + +```protobuf +rpc GetSafetyState(GetSafetyStateCommand.Request) + returns (GetSafetyStateCommand.Feedback); + +rpc RecoverSafetyState(RecoverSafetyStateCommand.Request) + returns (RecoverSafetyStateCommand.Feedback); +``` + +建议核心消息： + +```protobuf +message DeviceIdList { + repeated string device_ids = 1; +} + +message SafetyScope { + oneof target { + bool all_devices = 1; // 必须显式为 true + DeviceIdList devices = 2; // 必须非空且无重复 + } +} + +message RecoverSafetyStateCommand { + enum Mode { + MODE_UNSPECIFIED = 0; + VERIFY_ONLY = 1; + CLEAR_SOFTWARE_LATCH = 2; + } + + message Request { + string recovery_id = 1; + SafetyScope scope = 2; + uint64 expected_safety_epoch = 3; + Mode mode = 4; + string reason = 5; + uint32 timeout_ms = 6; + } + + message Feedback { + CommandHeader.Feedback header = 1; + RecoveryResult result = 2; + uint64 previous_safety_epoch = 3; + uint64 current_safety_epoch = 4; + SystemAdmissionState system_state = 5; + repeated RecoveryTargetResult targets = 6; + } +} +``` + +不要增加 `force=true` 或 `ignore_hardware_state=true`。 + +### 14.3 GetSafetyState + +至少返回： + +- global admission state、safety epoch、control service instance ID； +- device lifecycle、health、device admission state； +- SafetyCondition、snapshot fresh、sample age、device generation； +- active control owner 的脱敏标识； +- blocker reason code、首次发生时间、最后更新时间、来源 operation/command ID； +- 当前 StopAll/Recover operation 的阶段和 deadline； +- subsystem participant 状态。 + +该接口只读内存，即使设备失联或 driver worker 卡住也必须及时返回。 + +### 14.4 gRPC status 与业务结果 + +- 没有产生有效业务结果时使用非 OK status：认证失败、权限不足、格式错误、服务关闭； +- command ID 已预留后，执行结果使用 `grpc::Status::OK + CommandOutcome`，便于相同 ID + 重放完整结果； +- Recover 的部分失败也返回 OK 和 per-target result； +- `INTERNAL` 只表示代码异常或不变量破坏； +- 兼容旧服务时继续填充 `success/error_message`，客户端应迁移到 reason code。 + +## 15. 稳定错误模型 + +建议公共 reason code 至少包括： + +| Reason | gRPC 映射 | 是否可用同 ID 重试 | +| --- | --- | --- | +| INVALID_ARGUMENT | INVALID_ARGUMENT | 否，修正后使用新 ID | +| UNAUTHENTICATED | UNAUTHENTICATED | 认证后重新请求 | +| PERMISSION_DENIED | PERMISSION_DENIED | 否 | +| RECOVERY_RPC_DISABLED | FAILED_PRECONDITION | 是；未进入 RecoveryLedger，本机改配置并重启后可重试 | +| DEVICE_NOT_FOUND | NOT_FOUND | 否 | +| UNSUPPORTED_COMMAND | UNIMPLEMENTED | 否 | +| SYSTEM_STOPPING | ABORTED | 原 ID 查询，不重新下发 | +| SAFETY_LATCHED | FAILED_PRECONDITION | 恢复后新 ID | +| SAFETY_STATE_STALE | UNAVAILABLE | 状态刷新后新 ID | +| HARDWARE_UNSAFE | FAILED_PRECONDITION | 处理硬件后新 ID | +| EMERGENCY_STOP_ACTIVE | FAILED_PRECONDITION | 物理处理后新 ID | +| PROTECTIVE_STOP_ACTIVE | FAILED_PRECONDITION | 合法复位后新 ID | +| DEVICE_DISCONNECTED | UNAVAILABLE | 重连并校验 generation 后新 ID | +| CONTROL_BUSY | RESOURCE_EXHAUSTED | 释放控制权后新 ID | +| GENERATION_MISMATCH | ABORTED | 刷新状态后新 ID | +| COMMAND_ID_CONFLICT | ALREADY_EXISTS | 否 | +| LEDGER_EXHAUSTED | RESOURCE_EXHAUSTED | 稍后提交新 ID | +| BACKPRESSURE | RESOURCE_EXHAUSTED | 仅确认未接收后使用新 ID | +| DEADLINE_EXCEEDED_BEFORE_DISPATCH | DEADLINE_EXCEEDED | 新 ID | +| OUTCOME_UNKNOWN | ABORTED | 不得自动重下发 | +| INTERNAL_ERROR | INTERNAL | 不得自动重下发运动 | + +response 可以附带 retry directive，但 `retryable=true` 不能用于 `OUTCOME_UNKNOWN`。 + +## 16. gRPC 请求上下文与安全扩展设计 + +### 16.1 延后认证的边界 + +本轮设备安全改造允许不实现 TLS、静态 Token、JWT 和 mTLS，但不能把“暂不认证”等同于 +“不设计认证边界”。现在必须固定以下三层接口，后续安全加固只能替换实现： + +| 层 | 当前小范围部署 | 后续可选实现 | 业务层是否感知 | +| --- | --- | --- | --- | +| Transport security | Insecure，可受限到 loopback/隔离网 | Server TLS、mTLS、Unix Domain Socket | 否 | +| Authentication | Disabled，服务端生成 `anonymous` | Static Token、JWT/OIDC、TLS client certificate、Unix peer credential | 否 | +| Authorization | Compatibility policy + Recover exposure policy | 基于 role/capability 的 policy | 只接收允许/拒绝结果 | + +这种调整只表示认证交付可以延后，不表示明文匿名网络具备安全性。使用非 loopback 明文监听时， +安全依赖部署网络、主机防火墙和物理访问控制；该风险必须在配置、启动日志、SystemInfo 和指标中 +保持可见。 + +认证信息继续使用 gRPC metadata 或 transport auth context，不进入业务 request Proto。这样不会 +污染设备 API，也避免以后为了增加 Token 给所有命令消息增加字段。 + +### 16.2 `RequestContext` 与扩展接口 + +建议定义与设备层无关的不可变上下文： + +```cpp +enum class AuthenticationMethod { + Disabled, + StaticToken, + Jwt, + TlsClientCertificate, + UnixPeerCredential, +}; + +struct Principal { + std::string id; // 由服务端生成 + AuthenticationMethod method; + bool authenticated; + std::vector roles; // Disabled 模式只能是 Anonymous +}; + +struct RequestContext { + std::string correlation_id; + std::string full_method_name; + std::string peer; + Principal principal; + bool transport_encrypted; + bool local_peer; + std::chrono::steady_clock::time_point received_at; + std::chrono::steady_clock::time_point deadline; +}; +``` + +核心扩展接口： + +```cpp +class GrpcAuthenticationProvider { +public: + virtual ~GrpcAuthenticationProvider() = default; + virtual AuthenticationResult authenticate(const GrpcCallFacts&) = 0; +}; + +class GrpcAuthorizationPolicy { +public: + virtual ~GrpcAuthorizationPolicy() = default; + virtual AuthorizationDecision authorize( + const RequestContext&, const GrpcMethodPolicy&) const = 0; +}; + +class GrpcSecurityGateway { +public: + virtual GrpcCallGuard beginCall( + grpc::ServerContext&, const GrpcMethodPolicy&) = 0; +}; +``` + +当前提供 `DisabledAuthenticationProvider`：它不读取客户端自报身份，始终产生 +`{id="anonymous", authenticated=false, roles=[Anonymous]}`。同时提供可注入的 fake provider， +用于证明未来切换认证实现时不需要修改 handler。 + +每个 gRPC handler 在入口取得 `GrpcCallGuard`，之后只使用其中的 `RequestContext`。call guard +负责上下文生命周期、统一拒绝状态和审计结束事件。可以用 server interceptor 完成全局前置检查， +但不能依赖 thread-local 在 interceptor 和 handler 之间传递身份；同步、异步和 callback RPC 都必须 +具有明确的 per-call 所有权。 + +`SafetyCoordinator` 不依赖 gRPC 类型。Service 只把从 `RequestContext` 派生的稳定 +`CommandActor`/capability 传给准入和 ledger；驱动层完全不可见认证方式。 + +### 16.3 部署 Profile + +建议提供以下验证 Profile。Profile 是一组配置约束，不是散落在 Service 中的条件分支： + +| Profile | 监听与传输 | AuthN | Recover | 适用范围 | +| --- | --- | --- | --- | --- | +| `LOCAL_COMPATIBILITY` | loopback 或 Unix Domain Socket，可明文 | Disabled | 默认 Disabled，可显式 LocalOnly | 单机开发和维护 | +| `TRUSTED_NETWORK_COMPATIBILITY` | 显式受控网卡，可明文 | Disabled | Disabled | 当前隔离的小范围运行 | +| `LIGHTWEIGHT_AUTHENTICATED` | Server TLS | Static Token | SafetyAdmin | 客户端无需证书的轻量方案 | +| `PRODUCTION_AUTHENTICATED` | Server TLS 或 mTLS | JWT/Token/client certificate | SafetyAdmin | 后续正式部署 | + +本轮只要求实现前两个 Compatibility Profile 和后两个 Profile 的配置校验占位；选择尚未编译的 +认证 provider 必须启动失败，不能静默退回 Disabled。 + +约束如下： + +- Disabled 必须是显式模式，不能因为证书或 Token 文件加载失败而自动进入； +- 非 loopback 的 Insecure + Disabled 必须额外配置 `allow_insecure_non_loopback=true`，并产生 + 高可见度持续告警； +- Profile 只能通过本机配置和进程重启改变，不提供远程降级接口； +- QUIC heartbeat 和 GetSystemInfo 发布实际 transport/authentication/recovery exposure，不能 + 根据配置意图伪报； +- 后续 Static Token 客户端通过统一 client interceptor 添加 `authorization: Bearer ...`， + 业务调用点不变化； +- 启用了 TLS 时才校验证书、私钥、CA、有效期和文件权限。 + +### 16.4 授权与 Recover 暴露策略 + +方法权限仍预先分类，作为未来认证启用后的稳定契约： + +| Role | 权限 | +| --- | --- | +| Anonymous | Compatibility Profile 中除 Recover 外的现有兼容行为 | +| Observer | GetSystemInfo、GetDeviceList、GetSafetyState、只读状态和传感流 | +| Operator | Observer + 普通控制 + 设备 Stop + StopAll | +| SafetyAdmin | Operator + RecoverSafetyState + 安全配置诊断 | + +`GrpcMethodPolicyRegistry` 至少保存完整 method 名、read/mutate/stop/recover 分类、 +`CommandIntent` 和最低 role。未知方法在 Authenticated Profile 中 fail-closed；Compatibility +Profile 可以只为已有 RPC 保留当前行为，但仍必须产生 `unclassified_method` 告警并在阶段 2 前清零。 + +`RecoverSafetyState` 额外使用独立的 `RecoveryExposure`： + +| RecoveryExposure | 行为 | +| --- | --- | +| `DISABLED` | 返回稳定的 `RECOVERY_RPC_DISABLED`，不进入 RecoveryLedger | +| `LOCAL_ONLY` | 仅接受服务端从实际 peer 判定的 loopback/Unix Domain Socket 调用 | +| `AUTHORIZED` | 要求 authenticated principal 且具有 SafetyAdmin | + +校验规则： + +- Authentication Disabled + Recovery Authorized 是非法配置； +- `TRUSTED_NETWORK_COMPATIBILITY` 不能配置 LocalOnly 后再信任代理转发的 IP/header；只有 gRPC + 连接的实际 peer 可以用于本机判定； +- LocalOnly 是部署范围限制，不宣称调用者身份已经认证；优先使用 Unix Domain Socket， + 平台支持时再校验 UID/GID； +- TCP loopback 的 LocalOnly 等价于信任主机上的所有进程。多用户主机、共享容器宿主机或存在 + 不可信本地进程时必须保持 Disabled，或改用具有文件权限/peer credential 的 Unix Domain Socket； +- request 中的 role、principal、`force=true` 或类似字段一律不能改变该策略； +- Stop/StopAll 始终走独立安全通道，不受普通 safety gate 或审计 sink 故障阻塞；Authenticated + Profile 仍执行其访问策略，独立物理急停不能依赖网络认证服务。 + +### 16.5 调用链与状态码 + +```text +request limits -> correlation ID -> selected authentication provider + -> immutable RequestContext -> method/recovery authorization policy + -> audit begin -> service handler -> audit outcome +``` + +- Disabled provider 成功结果仍标记 `authenticated=false`，不能伪造为 Observer/Operator； +- 认证信息无效返回 `UNAUTHENTICATED`，身份有效但权限不足返回 `PERMISSION_DENIED`； +- Recover 被部署配置关闭返回业务 reason `RECOVERY_RPC_DISABLED`； +- LocalOnly 收到非本机 peer 返回 `PERMISSION_DENIED`； +- reflection 独立配置。Authenticated Profile 默认关闭或仅向 SafetyAdmin 开放；Compatibility + Profile 保持显式开关，不能依靠 reflection 状态表示访问安全。 + +### 16.6 审计字段与失败策略 + +- effective principal ID、authentication method、authenticated、roles、peer； +- transport encrypted、security profile、recovery exposure； +- gRPC method、device/resource、intent； +- correlation ID、command/recovery/stop operation ID； +- payload hash，不记录 Token、Authorization metadata、原始音视频和敏感大 payload； +- 准入结果、reason code、safety/device/authority generation； +- 硬件提交状态、终态、耗时； +- Recover reason、before/after blocker； +- 审计写入失败的处理策略。 + +阶段 0 可以先把统一审计事件接入现有日志 sink，但事件 schema 必须稳定。阶段 4 开放任何形式的 +Recover 前必须具备持久本地审计；Recover 审计无法写入时 fail-closed。Stop/StopAll 不能因审计 +sink 不可用而被拒绝，实现应保留本地应急日志并继续停止。普通控制是否因审计失败而拒绝由 +Profile 决定。 + +### 16.7 后续启用认证的变更面 + +阶段 0 边界完成后，从 Disabled 升级到 Static Token/JWT/mTLS 只允许修改或新增： + +- `GrpcServerTask` 的 credential/provider builder； +- `GrpcAuthenticationProvider` 实现、secret/identity 配置加载和角色映射； +- 客户端统一 metadata interceptor 或 channel credential； +- 对应 Profile 的集成测试、密钥轮换和部署 Runbook。 + +以下内容不应因认证升级而修改： + +- 设备命令 request/feedback Proto； +- `DeviceManager`、`SafetyCoordinator`、Sensor/Control policy； +- `DeviceSafetyEndpoint`、`SafetyParticipant` 和厂商驱动； +- handler 内的命令准入、StopAll 或 Recover 业务分支。 + +安全 Profile 只能在重启时切换。重启会生成新的 service instance ID，进程内 anonymous ledger +自然失效；不做运行期 `anonymous -> authenticated principal` 账本迁移，也不允许认证加载失败时 +保留旧监听并降级运行。 + +## 17. 配置设计 + +### 17.1 SafetyCoordinatorConfig + +建议在 `DeviceManagerConfig` 中增加： + +```protobuf +message SafetyCoordinatorConfig { + enum EnforcementMode { + ENFORCEMENT_MODE_UNSPECIFIED = 0; + LEGACY = 1; + SHADOW = 2; + ENFORCE_SELECTED = 3; + ENFORCE_ALL = 4; + } + + EnforcementMode mode = 1; + repeated string enforced_device_ids = 2; + uint32 stop_all_timeout_ms = 3; + uint32 recovery_timeout_ms = 4; + uint32 command_ledger_result_capacity = 5; + uint32 command_ledger_total_id_capacity = 6; + uint32 event_history_capacity = 7; + bool fail_startup_on_missing_control_capability = 8; +} +``` + +阶段 1 到阶段 4 允许旧配置缺失并进入 Legacy/Shadow，同时产生高可见度告警。阶段 5 的 +Production 配置若仍为 UNSPECIFIED 或 LEGACY，启动失败。 + +### 17.2 设备级覆盖 + +设备 entry 可增加： + +- 是否纳入当前 enforce rollout； +- 更短的 snapshot freshness； +- 更短的 stop/recovery timeout； +- 是否为启动所必需。 + +配置不能： + +- 把 Control endpoint 改成 Sensor； +- 把 required safe-stop 改成 optional； +- 允许 Unknown 通过； +- 关闭驱动最终硬件检查； +- 远程修改 enforce 为 legacy。 + +策略族和 capability 由编译后的 adapter 注册，配置只能收紧。 + +该启动失败开关只针对结构性缺陷，例如控制设备没有 endpoint 或 safe-stop capability。运行时 +设备断线、急停或动态 Unknown 应进入 Blocked/Latched，并保持管理面在线。 + +### 17.3 GRPCSecurityConfig + +建议在现有 `GRPCServerConfig` 中增加独立安全配置。Transport 和 Authentication 不合并成一个 +布尔值，避免以后只能通过客户端证书获得加密连接： + +```protobuf +message GRPCSecurityConfig { + enum TransportMode { + TRANSPORT_MODE_UNSPECIFIED = 0; + INSECURE = 1; + SERVER_TLS = 2; + MUTUAL_TLS = 3; + } + + enum AuthenticationMode { + AUTHENTICATION_MODE_UNSPECIFIED = 0; + DISABLED = 1; + STATIC_TOKEN = 2; + JWT = 3; + TLS_CLIENT_CERTIFICATE = 4; + } + + enum RecoveryExposure { + RECOVERY_EXPOSURE_UNSPECIFIED = 0; + RECOVERY_DISABLED = 1; + RECOVERY_LOCAL_ONLY = 2; + RECOVERY_AUTHORIZED = 3; + } + + TransportMode transport_mode = 1; + AuthenticationMode authentication_mode = 2; + RecoveryExposure recovery_exposure = 3; + bool allow_insecure_non_loopback = 4; + bool enable_reflection = 5; + + string server_certificate_file = 6; + string server_private_key_file = 7; + string client_ca_file = 8; + string static_token_file = 9; + string jwt_issuer = 10; + string jwt_audience = 11; + string audit_file = 12; +} +``` + +阶段 0 只实现 `INSECURE + DISABLED`，以及 `RECOVERY_DISABLED/RECOVERY_LOCAL_ONLY` 的策略； +其他枚举值先形成稳定配置契约，选择未编译能力时返回明确启动错误。旧配置缺失该 message 时可有 +一个发布周期映射到当前行为，但必须告警；迁移窗口结束后 UNSPECIFIED 一律启动失败。 + +后续 provider 实现后的组合约束： + +| Transport | Authentication | 是否允许 | 说明 | +| --- | --- | --- | --- | +| Insecure/Unix socket | Disabled | 是 | Compatibility；按监听范围和 RecoveryExposure 限制 | +| Insecure TCP | StaticToken/JWT | 否 | 凭据可被监听和重放 | +| Server TLS | Disabled | 是 | 只加密，不识别客户端，仍属于 Compatibility | +| Server TLS | StaticToken/JWT | 是 | 推荐的轻量远程认证 | +| Mutual TLS | TLSClientCertificate | 是 | 后续强身份 Profile | +| Mutual TLS | Disabled | 否 | 要求客户端证书却丢弃身份没有明确语义 | + +首轮不实现多因素组合；如果未来需要 mTLS + Token，必须定义唯一 principal、角色合并和审计规则， +不能简单拼接两个 provider 的结果。 + +配置只保存凭据文件路径，不保存 Token 明文。后续实现静态 Token 时使用权限受限的独立文件， +日志和审计只能记录 token ID，不能记录 secret。所有组合在启动时集中校验，并把实际生效值写入 +capability manifest/SystemInfo。 + +## 18. 当前设备迁移映射 + +| 设备/入口 | 策略 | 关键安全事实 | Stop/恢复要点 | +| --- | --- | --- | --- | +| Aubo Arm | Control | connected、robot mode、exec/queue、power、硬件/软件 EStop、protective stop、fault | 硬件 EStop 释放后仅在 Normal/Reduced、队列清空和 quiescent 确认后自动恢复；软件 EStop 独立锁存；无法确认 exec 时 OutcomeUnknown | +| Huayan Arm | Control | lifecycle generation、motion state、fault、stop confirmation | 保留已强化的 fail-closed 生命周期，映射为统一 endpoint | +| MotorRobotArm | Control | group atomicity、joint freshness、bus generation | 不具备原子 group servo 时继续拒绝 teleop capability | +| UME RobotArm | Control | CAN session、watchdog、torque enable、feedback freshness | reconnect 不恢复 torque；本地 haptic loop 不做网络调用 | +| ArmTeleopService | Control（stream） | session ID、sequence、watchdog、lease、safety epoch | OPEN 时准入；每帧校验 epoch；StopAll 后必须新 session | +| SEER AGV | Control | controller session、navigation terminal、velocity、EStop、fault | cancel ACK 不等于停稳；连续零速度采样后才 quiescent | +| MotorManager/Motor | Control | bus session epoch、CiA402 state、enabled、quick-stop、actual velocity | 每个 motor resource 注册；Quick Stop 未确认则 quarantine | +| DexHand control | Control | hand lifecycle、command generation、actuator idle | tactile stream 与控制命令分开分类；stopOperationalActivity 必须可证明 | +| DexHand tactile | Sensor | polling worker、sample freshness | StopAll 可停 stream，但不能把 stream 状态当作手部运动状态 | +| Camera capture | Sensor | opened、streaming、worker generation | 复用 MediaSourceHub；gRPC/QUIC 和直接 startStreaming 在启动设备 producer 前取得 Sensor/StartActivity dispatch guard | +| Camera PTZ | Control | PTZ activity generation、stop ACK | 服务端按已校验 action 派生 START=Actuate、STOP=安全通道，锁止时 STOP 仍可下发 | +| Microphone | Sensor | capture lifecycle、sample freshness | activity participant | +| Speaker | Sensor | playback lifecycle、worker generation | Stop 始终允许；不视为机械执行器 | +| BioHead | Control | expression/speech activity、hardware fault | 表情可能产生机械运动，不能归入纯媒体 | +| Battery | Sensor | sample freshness、communication state | 只读；不参与 actuator StopAll | +| MuJoCo control | Control | simulation generation、active command | 使用与真机相同策略，便于故障注入，但不能代替真机验收 | +| HLC/touch | Control | 服务端解析真实 arm ID、任务 safety session、每次 arm submission 复核 | `touch` 为 Actuate；每次 move/speed/servo 前依次复核 permit、control authority、Coordinator dispatch 和设备最终检查；撤销后走 Stop lane | +| QUIC media | Sensor | Camera/Microphone source ID、snapshot freshness、media generation | 不提供执行器控制；全局 Hub 的设备 producer 启动使用与 gRPC 相同的 Sensor/StartActivity dispatch guard | + +Aubo JSON `get_di/get_do` 可以归类 Observe；`set_do` 必须归类 Configure/Control，并最终迁移 +为 typed RPC。未知 JSON command 在 Control 设备上默认拒绝。 + +## 19. 六阶段实施方案 + +| 阶段 | 核心产物 | 是否改变生产准入 | +| --- | --- | --- | +| 0 | RequestContext、AuthN/AuthZ/Audit 扩展边界 | 否，显式保持现有兼容行为 | +| 1 | 类型、快照、错误、幂等契约 | 否，旧 gate 仍权威 | +| 2 | SafetyCoordinator shadow | 否，只比较决策 | +| 3 | 高风险设备逐个 enforce | 仅改变选中设备 | +| 4 | 泛化 StopAll、正式 Recover | 改变系统安全事务 | +| 5 | EnforceAll、真机签字、移除旧路径 | 全量切换 | + +每个阶段只有满足退出条件后才能进入下一阶段；不能为了尽快提供 Recover 而跳过阶段 0、1、 +2 或高风险设备迁移。 + +### 阶段 0：建立控制面安全扩展边界 + +#### 目标 + +在不增加现有客户端证书或 Token 配置负担的前提下，建立统一请求上下文和可替换的安全网关， +使以后增加认证只替换 provider/policy，不横向修改所有 Service。当前明文匿名暴露被显式记录， +但本阶段不改变已有普通 RPC 的允许/拒绝行为。 + +#### 实施项 + +1. 扩展 `grpc_server_config.proto`，增加 Transport、Authentication、RecoveryExposure 和 + `allow_insecure_non_loopback`；本阶段只实现 Insecure + Disabled。 +2. 定义不可变 `RequestContext`、`Principal`、与 gRPC 无关的 `CommandActor`，以及明确的 + effective principal 规则。 +3. 实现 `DisabledAuthenticationProvider`、`GrpcAuthorizationPolicy`、 + `GrpcSecurityGateway/GrpcCallGuard` 和可注入 fake provider。 +4. 建立完整 method 名驱动的 `GrpcMethodPolicyRegistry`，先固定 read/mutate/stop/recover、 + 最低未来 role；CommandIntent 最迟在阶段 2 补齐。 +5. 所有现有 handler 统一通过 call guard 取得上下文；Service 不直接读取认证 metadata， + 不从 request 读取 principal/role。 +6. Compatibility policy 保持现有 RPC 行为。`RecoverSafetyState` 标记为独立 Recovery policy， + 此阶段只预留，不注册实现。 +7. 增加 correlation ID 和统一审计事件 schema，先接入现有日志 sink；保留 error logging + interceptor，并明确多个 interceptor/call guard 的生命周期顺序。 +8. GrpcServerTask 集中校验配置组合；选择 StaticToken/JWT/mTLS 等未实现 provider 时启动失败， + 不允许静默退回 Disabled。 +9. QUIC heartbeat、GetSystemInfo、启动日志和指标发布实际 transport、authentication 和 + recovery exposure；非 loopback 明文匿名模式持续告警。 +10. 为旧配置提供一个发布周期的兼容映射，并更新部署文档和显式示例配置。 + +#### 测试 + +- Disabled provider 忽略伪造的 principal/role metadata，结果始终是未认证 `anonymous`； +- 现有客户端不增加 metadata 仍可调用已有 RPC，允许/拒绝结果不变； +- IPv4/IPv6 loopback、Unix Domain Socket 和非本机 peer 分类； +- Disabled + Authorized Recovery、未实现 provider、非 loopback insecure 未显式确认等非法组合； +- fake authenticated provider 下 Observer/Operator/SafetyAdmin 的 method policy； +- sync、stream、callback、deadline/cancellation 下 RequestContext 生命周期和清理； +- correlation ID、审计字段脱敏、audit sink 异常和 interceptor 顺序； +- 旧配置迁移及实际 security capability 上报。 + +#### 退出条件 + +- 每个现有 gRPC method 都经过统一 gateway，并有 access class； +- `anonymous` 只能由服务端生成，伪造 metadata 不会获得 role； +- 当前兼容模式行为未改变，但明文/匿名/监听范围在配置和运行状态中可见； +- 非本机匿名 Recover 没有可执行路径； +- fake provider 测试证明启用认证不需要修改业务 handler、Coordinator 或驱动； +- 部署文档、风险说明和显式示例配置已更新。 + +#### 回滚边界 + +可以让 Compatibility policy 继续保持旧行为，但 RequestContext、method registry 和配置字段 +不能删除；否则会重新引入后续横向改造。任何安全 Profile 变化只能通过本机配置和进程重启， +不能提供远程降级 RPC。 + +### 阶段 1：统一类型、快照、错误与幂等契约 + +#### 目标 + +建立后续状态机所需的数据契约，但不改变现有命令准入结果。 + +#### 实施项 + +1. 新建 `manager/safety` target 和 `safety_types.h`。 +2. 定义 CommandIntent、SafetyCondition、TriState、SafetyBlocker、SafetySnapshot。 +3. 定义 DeviceSafetyDescriptor、Endpoint、Participant 接口。 +4. 在 DeviceManager 中建立 `SafetySnapshotStore`，Manager snapshot 只读缓存。 +5. 为现有设备建立 adapter；未支持设备发布 Unknown，不伪造安全。 +6. 扩展 `CommandHeader` 和 Feedback；新增 reason code 和 execution state。 +7. 生成统一 control service instance ID，并通过 GetSystemInfo 暴露。 +8. 实现进程内 CommandLedger，复用 ActionQueue 的容量和 retired-ID 设计原则。 +9. 建立 vendor result 到公共 reason code 的映射层。 +10. 给 AbstractDevice 的宽松默认生命周期能力增加弃用标记；Control 注册要求显式能力。 +11. 更新 Manager/Service/Proto 文档中已经过时的生命周期说明。 + +#### 测试 + +- snapshot 并发发布/读取、stale 计算、generation 单调性； +- endpoint 未实现、设备重连、设备重新注册； +- ledger 同 ID 同 payload、不同 payload、in-flight join、淘汰和容量耗尽； +- deterministic payload hash； +- reason code 映射完整性； +- protobuf 旧 client payload 解析和旧配置解析。 + +#### 退出条件 + +- gRPC 状态查询和 QUIC heartbeat 不再调用设备方法； +- 所有 Control 类型至少有显式 Unknown adapter，不再依赖默认 Healthy； +- 新字段保持 wire compatible； +- ledger 的 OutcomeUnknown 测试证明不会二次 dispatch； +- 此阶段生产行为仍由旧 gate 决定。 + +#### 回滚边界 + +新 Proto 字段不可删除或复用；可以停止使用新字段，但必须保留 wire schema。SnapshotStore +可以退回仅诊断模式，不影响旧 gate。 + +### 阶段 2：SafetyCoordinator 影子运行 + +#### 目标 + +在不改变线上允许/拒绝结果的情况下，对全部命令计算新策略结果并验证分类完整性。 + +#### 实施项 + +1. DeviceManager 构造并持有 SafetyCoordinator。 +2. 在阶段 0 的 GrpcMethodPolicyRegistry 中补齐 CommandIntent，并引入 typed CommandDescriptor。 +3. gRPC service 增加可注入构造函数；GrpcServerTask 统一传入 coordinator/ledger，沿用既有 + GrpcSecurityGateway。 +4. 每个 handler 在旧 gate 前后调用 shadow evaluation，记录旧/新决策差异。 +5. 把 StopAll gate、Motor、Media、Camera registry、ActionQueue 包装成 legacy participant。 +6. 新增只读 `GetSafetyState`，先发布 shadow decision、freshness 和 blocker。 +7. 增加 admission latency、decision mismatch、unknown snapshot、unclassified method 指标。 +8. 启动状态使用 Starting；只做诊断，不因 shadow 结果阻止旧业务。 +9. 将 Runtime/TaskManager 划分为 management-plane 和 operational 启动组；设备动态故障不再 + 使已经通过所选安全 Profile 配置校验的 SystemService 一并退出。 + +#### 影子比较分类 + +| 旧结果 | 新结果 | 处理 | +| --- | --- | --- | +| Allow | Allow | 正常 | +| Deny | Deny | 正常，比较 reason | +| Allow | Deny | 记录 `would_deny`，优先修正快照或旧行为 | +| Deny | Allow | 高风险 `would_allow`，在进入阶段 3 前必须归零或有书面解释 | + +#### 测试 + +- 所有 protobuf service method 都在 method access policy 和 command-intent registry 中； +- shadow evaluation 无硬件 I/O； +- coordinator 销毁时 participant 全部注销且无 callback； +- StopAll 与 shadow admit 并发不改变旧 gate 行为； +- GetSafetyState 在设备 endpoint 阻塞时仍快速返回。 +- 单个设备 create/init/start 失败时，GetSafetyState 仍可按当前安全 Profile 访问。 + +#### 退出条件 + +- 所有 mutating method 均有固定 intent； +- 所有 Control 设备都有 DeviceSafetyDescriptor； +- 没有未解释的 `legacy deny / new allow`； +- Shadow 准入 p99 只包含内存操作，不受硬件 RTT 影响； +- 至少完成一轮真实运行日志评审。 +- 管理面 degraded-start 和正常 shutdown 路径都有生命周期测试。 + +#### 回滚边界 + +可关闭 shadow evaluation，但保留 snapshot 和 method registry。旧 gate 仍是唯一 authority。 + +### 阶段 3：迁移高风险控制设备 + +#### 目标 + +让 Arm、Teleoperation、AGV、Motor、DexHand control 和 Camera PTZ 的普通命令由 +SafetyCoordinator 权威准入，并统一幂等与执行结果语义。 + +#### 迁移顺序 + +1. Aubo/Huayan unary Arm； +2. ArmTeleopService 和 UME session； +3. AGV navigation/velocity； +4. Motor unary 和 cyclic stream； +5. DexHand control； +6. Camera PTZ； +7. ActionQueue step dispatch。 + +#### 单设备迁移步骤 + +1. 完成 endpoint 和新鲜 snapshot； +2. 明确每个 SDK 返回值的 Accepted/Completed/Rejected/Unknown 语义； +3. 实现 safe-stop request 和 quiescence verification； +4. handler 先通过 Coordinator，再保留 legacy gate 作为第二道 deny-only adapter； +5. dispatch 前执行 permit revalidation 和 driver final check； +6. unary mutating 命令接入 CommandLedger； +7. 流式命令绑定 safety epoch、device generation 和 lease generation； +8. 开启该设备 `ENFORCE_SELECTED`； +9. 通过 fake、并发和真机测试后，删除该设备 handler 中重复的旧状态判断。 + +#### 驱动执行契约 + +- Aubo queue full 只有在 SDK 能证明命令未接收时才能返回 Backpressure； +- 未及时观察到 exec ID 但无法证明未接收时返回 OutcomeUnknown，并 quarantine arm； +- Huayan 保留已有的停止确认和生命周期 generation； +- AGV cancel/zero command ACK 后继续等待导航终态和连续零速度样本； +- Motor Quick Stop 未确认时保留 motor resource quarantine； +- Teleop/stream 的 reconnect 总是新 session，旧轨迹不续跑； +- DexHand void 返回接口逐步改为结构化 result，不能只靠日志判断成功。 + +#### 测试 + +- admit 与 StopAll 的线性化竞态； +- snapshot 在 admit 后、dispatch 前过期； +- lease 在 dispatch 前被抢占； +- ACK 丢失、队列满、控制器断线、进程重连； +- 同 command ID 并发请求； +- stream sequence 重复、倒退、watchdog、断线； +- Stop 后旧 session/permit 无法恢复； +- 每个设备的真实停车确认。 + +#### 退出条件 + +- 上述高风险入口不存在绕过 Coordinator 的设备调用； +- 每类命令都有结构化结果，Internal 不再承载所有业务错误； +- 旧 gate 只作为兼容 deny，不再能单独 reopen 新 Coordinator； +- 设备级 OutcomeUnknown 可通过 GetSafetyState 定位并进入恢复流程； +- 新控制设备接入安全层不需要修改 SafetyCoordinator switch。 + +#### 回滚边界 + +按 device ID 从 EnforceSelected 退回 Shadow。已产生的 quarantine 不能因回滚配置自动清除， +必须 StopAll 成功或通过当前 RecoveryExposure 允许的恢复流程处理。 + +### 阶段 4：泛化 StopAll 并开放正式恢复 RPC + +#### 目标 + +删除 SystemService 中的设备类型停止编排，使 StopAll 和 Recover 成为统一、可观察、可重试的 +安全事务。 + +#### 实施项 + +1. 实现 `SafetyOperationOrchestrator` 和 participant 分阶段执行。 +2. 所有 legacy coordinator 注册为 participant，保留其已有 barrier 语义。 +3. 设备通过注册的 safe-stop participant 加入，不再由 SystemService dynamic cast。 +4. `gRPCSystemServiceImpl::StopAll` 缩减为 RequestContext、参数适配和结果映射。 +5. StopAll 请求增加 operation ID、诊断用 service instance 和结构化 per-target response；旧或 + 缺失 instance 不能阻止安全停止。 +6. 实现 RecoveryLedger、active refresh、quiescence check 和软件 latch reconcile。 +7. 在 SystemService 注册 `GetSafetyState` 和 `RecoverSafetyState`。 +8. Recover 统一执行 RecoveryExposure：Disabled 拒绝、LocalOnly 校验实际本机 peer、 + Authorized 要求 authenticated SafetyAdmin；所有允许路径都要求非空 reason 并写持久审计。 +9. `clearForTesting()` 保持测试可见或移入 test support，运行代码无法调用。 +10. 调整 Runtime shutdown：先进入 ShuttingDown/关闭准入，再在 participant 存活时执行 + 有界 quiesce，之后停止 TaskManager 和 DeviceManager。 + +#### 并发规则 + +| 并发场景 | 规则 | +| --- | --- | +| StopAll vs normal command | epoch 线性化；命令要么被拒绝，要么被登记并停止 | +| StopAll vs StopAll | 加入同一 round，返回同一结果 | +| Recover vs normal command | Recovering 全程关闭普通准入 | +| Recover vs StopAll | process operation mutex 串行化，Stop 优先 | +| Recover vs Recover | 相同 recovery ID join；不同 ID 串行 | +| RPC cancellation vs Stop/Recover | 只取消 waiter，不取消已开始的系统安全事务 | +| late worker vs new epoch | 只能写旧 operation 诊断，不能 release 新 barrier | +| shutdown vs Recover | shutdown 终止新 recovery，保持 gate 关闭并执行 quiesce | + +#### 测试 + +- participant 注册顺序随机但执行阶段稳定； +- participant throw、timeout、永不返回、返回 Unknown； +- 部分恢复和 global blocker； +- Recover 时硬件 EStop、protective stop、fault、stale、disconnect、still moving； +- 恢复过程中状态再次变坏； +- expected epoch mismatch 和 recovery ID payload conflict； +- Disabled/LocalOnly/Authorized 三种 RecoveryExposure，尤其是非本机伪造 metadata 无法绕过； +- audit 写失败； +- service 析构、dispatcher join 和 Runtime shutdown。 + +#### 退出条件 + +- SystemService StopAll 不包含具体 DeviceKind 停止分支； +- StopAll/Recover 都返回 per-participant 结构化结果； +- 任何失败路径都保持明确 Latched/Quarantined 状态； +- Recover 无法绕过 Unknown、未结束 worker 或未确认运动； +- 一次成功 Recover 使系统能接受新命令，但不会恢复旧命令或自动使能硬件； +- Recover RPC 默认 Disabled；匿名部署只有 LocalOnly 验收通过才能开放，远程开放必须启用 + authenticated SafetyAdmin；任何开放模式都要求持久审计验收通过。 + +#### 回滚边界 + +保留旧 StopAll adapter 一个发布周期，可通过本地启动配置切换实现。正式 Recover 已产生的 +epoch 和审计不能回滚；禁止退回运行期无检查 clear。 + +### 阶段 5：全量 enforce、真机验证和旧路径移除 + +#### 目标 + +覆盖剩余外围设备和所有命令入口，验证发布产物与真实硬件，并删除重复全局状态。 + +#### 实施项 + +1. 迁移 Camera/Microphone/Speaker/BioHead/HLC 和剩余 local task； +2. 对所有 protobuf method 做 descriptor 驱动的 access-policy/intent 完整性测试； +3. Production 强制 `ENFORCE_ALL`，Control Unknown 拒绝； +4. 删除 service 对 `globalStopAllAdmissionGate()`、global media/motor registry 的业务依赖； +5. 保留必要 registry 的领域功能，但准入 authority 只属于 Coordinator； +6. 建立 build capability manifest，包含已编译的 transport/auth provider、MsQuic、硬件 SDK、 + Safety schema version； +7. GetSystemInfo 和启动日志发布实际 capability，不以源码存在推断二进制能力； +8. 建立安装产物 smoke test，而不只测试 build tree； +9. 建立真实设备台架和故障注入矩阵； +10. 完成运维 Runbook：StopAll、blocker 诊断、硬件处理、dry-run、Recover 和审计查询。 + +#### 真机矩阵 + +至少覆盖： + +- Aubo/Huayan：运动中断网、ACK 丢失、queue full、保护停、急停释放、SDK reconnect； +- AGV：导航取消 ACK 后仍移动、速度反馈丢失、地图切换中 StopAll； +- Motor：Modbus/EtherCAT 断线、bus epoch 改变、Quick Stop 未确认、stream watchdog； +- DexHand/PTZ/BioHead：连续命令中 StopAll、stop ACK 丢失、sensor stream 并发； +- 进程：控制中 SIGTERM、任务启动失败、服务重启后硬件仍活动； +- 网络：RPC 超时后相同 ID 重试、不同 payload 冲突、匿名 namespace、实际 peer 分类；如果构建 + 包含认证 provider，再覆盖 Token/证书轮换和权限变化； +- 多平台 QUIC：重连和多 heartbeat 不能改变本地控制 safety epoch。 + +#### 退出条件 + +- 所有 mutating 入口都经过 Coordinator 或显式安全通道； +- 所有 Control capability 都有 fresh snapshot、final check、safe-stop 和真机签字； +- 进程重启不会自动继续运动或接受旧请求； +- install artifact capability 与配置一致； +- 全量自动测试、TSAN/并发测试和硬件台架通过； +- 运维人员能只依赖结构化状态完成一次故障定位和恢复； +- 删除遗留 runtime clear 和无主 global gate。 + +#### 回滚边界 + +单设备可以通过本地配置从 EnforceSelected 退回 Shadow，但 RequestContext 安全边界、幂等和 +Stop/Recover epoch 不能关闭。RecoveryExposure 或网络暴露范围的任何放宽都要求重启、审计和 +现场审批，不能通过远程 RPC 修改。 + +## 20. 测试体系 + +### 20.1 单元测试 + +- Sensor/Control policy 全状态表； +- safety state transition 和非法 transition； +- snapshot freshness 和 generation； +- permit move/expiry/revalidation； +- ledger 和 recovery ledger； +- reason code 与 gRPC 映射； +- participant barrier/token epoch； +- RequestContext 构造、anonymous effective principal、provider/policy 组合； +- RecoveryExposure 和 actual peer classifier。 + +### 20.2 并发与性质测试 + +建议用可控 scheduler/fake clock 重复验证： + +1. StopAll 和 dispatch 的所有交错最终满足“不丢失活动”； +2. Recover 和 late stop callback 的所有交错都不能提前 reopen； +3. 相同 command ID 任意并发度下 dispatch count 最大为 1； +4. generation 单调增加，旧 permit 永远无法重新合法； +5. participant 异常不会跳过其他 required stop； +6. Coordinator 销毁后不存在访问已释放对象的 callback。 + +### 20.3 Service 集成测试 + +- Disabled/Fake provider、method policy 和伪造 metadata； +- legacy/new protobuf client； +- handler 分类完整性； +- Stop/Status 在 Latched 时可调用，Recover 在 exposure 允许时不受普通 gate 阻塞； +- Recover per-target response； +- gRPC deadline/cancellation 与 operation 生命周期分离。 + +### 20.4 故障注入 + +| 故障点 | 期望结果 | +| --- | --- | +| Admit 后 snapshot 过期 | final check 拒绝，无 SDK dispatch | +| SDK 调用超时且接收状态未知 | OutcomeUnknown + device quarantine | +| Stop worker 超时 | global Latched，worker token 保留 | +| Recovery refresh 超时 | blocker remains，不清 gate | +| 相同 ID 不同 payload | conflict，无第二次 dispatch | +| 进程重启期间硬件仍运动 | Starting/Latched，不自动 Open | +| 审计落盘失败 | Recover fail-closed | +| 一个设备 health worker 卡住 | 其他 Status/Stop/Recover 仍可执行 | + +### 20.5 发布产物验证 + +- 对安装目录启动二进制； +- 校验 capability manifest； +- Insecure + Disabled 的 loopback 和显式 trusted-network grpcurl smoke； +- Recovery Disabled/LocalOnly 的实际 peer smoke； +- reflection 与安全 Profile 的显式配置 smoke； +- 配置要求未编译 AuthN/Transport provider、MsQuic 或 driver capability 时启动失败； +- 构建包含可选 Token/TLS/mTLS provider 时，才执行对应认证矩阵； +- 生成 C++ 和 Java API 兼容测试。 + +## 21. 可观测性与运维 + +### 21.1 指标 + +建议至少提供： + +```text +cmvr_safety_admission_decisions_total{device,intent,decision,reason} +cmvr_safety_snapshot_age_ms{device} +cmvr_safety_device_state{device,state} +cmvr_safety_global_state{state} +cmvr_safety_stop_duration_ms{participant,result} +cmvr_safety_recovery_attempts_total{result} +cmvr_command_dedup_total{status} +cmvr_command_outcome_unknown_total{device,method} +cmvr_control_lease_conflicts_total{resource} +cmvr_grpc_authz_denied_total{method,role} +cmvr_grpc_requests_total{auth_method,authenticated} +cmvr_grpc_insecure_listener{non_loopback} +cmvr_grpc_recovery_access_denied_total{exposure,peer_kind,reason} +``` + +### 21.2 事件历史 + +Coordinator 保存有界内存事件环，并将关键事件写入持久审计： + +- global/device transition； +- snapshot stale/fresh、generation changed； +- quarantine 创建和清除； +- StopAll/Recover phase； +- permit reject、final check reject； +- OutcomeUnknown； +- participant timeout/late completion。 + +错误字符串只用于人读，自动化必须使用 reason code 和结构化 blocker。 + +### 21.3 Runbook 顺序 + +1. 调用 GetSafetyState 获取 epoch 和 blocker； +2. 确认当前 RecoveryExposure；Disabled 模式不能尝试网络绕过，需按本机变更流程处理； +3. 在现场确认物理环境和设备状态； +4. 必要时调用 typed ResetFault，不能直接 Recover 代替硬件处理； +5. 使用符合 LocalOnly/Authorized 策略的调用方执行 RecoverSafetyState `VERIFY_ONLY`； +6. blocker 全部满足后调用 `CLEAR_SOFTWARE_LATCH`； +7. 重新读取 SafetyState，确认新 epoch 和 Open/设备级状态； +8. 使用新的 command ID、service instance 和 device generation 下发后续命令。 + +## 22. 建议 PR 拆分 + +为避免一次性改动所有设备，建议按以下独立可回滚单元提交： + +1. 设计文档和安全不变量； +2. GRPCSecurityConfig、RequestContext、Disabled/Fake provider、method policy 和 call guard； +3. correlation/audit schema、实际 security capability 上报和 Compatibility Profile 测试； +4. Safety types、snapshot store 和 tests； +5. CommandHeader/reason code additive Proto； +6. CommandLedger； +7. SafetyCoordinator shadow 和 GetSafetyState； +8. legacy participant adapters； +9. Arm + ArmTeleop migration； +10. AGV migration； +11. Motor migration； +12. DexHand/PTZ/ActionQueue migration； +13. generic StopAll； +14. RecoverSafetyState； +15. peripheral migration、EnforceAll 和 legacy removal； +16. capability manifest、install smoke 和硬件验证报告； +17. 可选后续：Server TLS + Static Token provider 和客户端 interceptor； +18. 可选后续：JWT/OIDC 或 mTLS provider、正式身份生命周期和密钥轮换。 + +每个迁移 PR 必须包含：命令分类表、snapshot 定义、stop verification、错误映射、竞态测试和 +回滚开关。不得只把 handler 前的一个 `if` 移到 Coordinator 就宣称完成迁移。 + +## 23. 最终验收清单 + +- [x] 所有 gRPC handler 都通过统一 RequestContext/call guard，业务代码不解析认证 metadata +- [x] Disabled 模式始终产生未认证 anonymous principal，实际安全 Profile 和监听风险可观测 +- [x] 所有方法都有 access policy 和 CommandIntent，未知方法不会绕过安全网关 +- [x] Recover 默认 Disabled；匿名模式只允许 LocalOnly，远程模式只允许 authenticated SafetyAdmin +- [x] Recover 所有开放方式都要求持久审计，审计失败时 fail-closed +- [ ] 启用 Authenticated Profile 时，传输/AuthN/AuthZ 按该 Profile 的独立验收矩阵通过 +- [x] 生命周期、健康和安全状态是独立字段 +- [x] Manager snapshot/status 路径只读 Manager 缓存，不执行设备 I/O +- [x] 普通 mutating unary 命令使用 command ID、service instance 和统一 ledger 语义 +- [x] 流式控制绑定 session、sequence、watchdog 和 safety epoch +- [x] 已迁移的 Control 命令经过统一准入和驱动最终检查 +- [x] Stop/Status 在软件锁止状态下可达；Recover 在 exposure 允许时不受普通 gate 阻塞 +- [x] SystemService StopAll 不按 DeviceKind 硬编码设备停止逻辑 +- [x] StopAll 失败留下结构化 blocker 和不可绕过 latch +- [x] Recover 不能忽略 Unknown、EStop、未确认运动或旧 worker +- [x] Recover 不上电、不使能、不恢复旧轨迹或旧 session +- [x] 同一 command ID 最大一次 dispatch，OutcomeUnknown 不自动重发 +- [x] 新设备只需注册 DeviceSafetyDescriptor、Endpoint、Participant 和 method intent +- [x] startup/shutdown 与 safety epoch 有明确顺序 +- [x] 单个设备动态故障不会关闭按当前安全 Profile 启动的 SystemService 管理面 +- [ ] 所有发布 capability 来自实际安装二进制 +- [ ] fake、并发、故障注入、安装产物和真机台架全部完成 + +## 24. 实施前需要确定的工程参数 + +以下参数需要通过设备手册和台架测量确定，但不改变架构： + +- 各设备 snapshot polling 周期和 maximum age； +- StopAll 总 deadline 与每类 participant timeout； +- 连续零速度/静止确认的阈值和样本数； +- Aubo、Huayan、SEER 等 SDK 对“已接受命令”的精确定义； +- ledger 容量和结果保留时间； +- 审计文件保留、轮转和上传策略； +- 当前部署允许监听的网卡/CIDR、主机防火墙规则和 `allow_insecure_non_loopback` 责任人； +- RecoveryExposure 初始选择 Disabled 还是 LocalOnly，以及本机维护入口； +- 旧安全配置兼容窗口长度，以及启用 Static Token/Server TLS 的触发条件； +- 后续认证启用时的 principal 命名、Token/证书轮换和角色绑定； +- 哪些设备是启动时 required control device。 + +这些值必须以显式配置和硬上限进入代码，不能以缺省零值表达“无限”或“允许全部”。 diff --git a/protos/cmvr/api/common.proto b/protos/cmvr/api/common.proto index 5686a18a..64988d45 100644 --- a/protos/cmvr/api/common.proto +++ b/protos/cmvr/api/common.proto @@ -4,6 +4,59 @@ package cmvr.api; import "google/protobuf/timestamp.proto"; +enum CommandReasonCode { + COMMAND_REASON_CODE_UNSPECIFIED = 0; + COMMAND_REASON_CODE_NONE = 1; + COMMAND_REASON_CODE_INVALID_ARGUMENT = 2; + COMMAND_REASON_CODE_UNAUTHENTICATED = 3; + COMMAND_REASON_CODE_PERMISSION_DENIED = 4; + COMMAND_REASON_CODE_RECOVERY_RPC_DISABLED = 5; + COMMAND_REASON_CODE_DEVICE_NOT_FOUND = 6; + COMMAND_REASON_CODE_DEVICE_UNAVAILABLE = 7; + COMMAND_REASON_CODE_UNSUPPORTED_COMMAND = 8; + COMMAND_REASON_CODE_SYSTEM_STARTING = 9; + COMMAND_REASON_CODE_SYSTEM_STOPPING = 10; + COMMAND_REASON_CODE_SAFETY_LATCHED = 11; + COMMAND_REASON_CODE_SAFETY_STATE_MISSING = 12; + COMMAND_REASON_CODE_SAFETY_STATE_STALE = 13; + COMMAND_REASON_CODE_HARDWARE_UNSAFE = 14; + COMMAND_REASON_CODE_EMERGENCY_STOP_ACTIVE = 15; + COMMAND_REASON_CODE_PROTECTIVE_STOP_ACTIVE = 16; + COMMAND_REASON_CODE_DEVICE_DISCONNECTED = 17; + COMMAND_REASON_CODE_DEVICE_FAULT = 18; + COMMAND_REASON_CODE_DEVICE_NOT_READY = 19; + COMMAND_REASON_CODE_DEVICE_STILL_MOVING = 20; + COMMAND_REASON_CODE_CONTROL_BUSY = 21; + COMMAND_REASON_CODE_GENERATION_MISMATCH = 22; + COMMAND_REASON_CODE_COMMAND_ID_REQUIRED = 23; + COMMAND_REASON_CODE_COMMAND_ID_CONFLICT = 24; + COMMAND_REASON_CODE_RESULT_EVICTED = 25; + COMMAND_REASON_CODE_LEDGER_EXHAUSTED = 26; + COMMAND_REASON_CODE_BACKPRESSURE = 27; + COMMAND_REASON_CODE_DEADLINE_EXCEEDED_BEFORE_DISPATCH = 28; + COMMAND_REASON_CODE_OUTCOME_UNKNOWN = 29; + COMMAND_REASON_CODE_PARTICIPANT_TIMEOUT = 30; + COMMAND_REASON_CODE_STOP_UNCONFIRMED = 31; + COMMAND_REASON_CODE_RECOVERY_EPOCH_MISMATCH = 32; + COMMAND_REASON_CODE_RECOVERY_REASON_REQUIRED = 33; + COMMAND_REASON_CODE_RECOVERY_AUDIT_FAILED = 34; + COMMAND_REASON_CODE_INTERNAL_ERROR = 35; +} + +enum CommandExecutionState { + COMMAND_EXECUTION_STATE_UNSPECIFIED = 0; + COMMAND_EXECUTION_STATE_RECEIVED = 1; + COMMAND_EXECUTION_STATE_RESERVED = 2; + COMMAND_EXECUTION_STATE_REJECTED_BEFORE_DISPATCH = 3; + COMMAND_EXECUTION_STATE_ADMITTED = 4; + COMMAND_EXECUTION_STATE_DISPATCHING = 5; + COMMAND_EXECUTION_STATE_ACCEPTED_BY_HARDWARE = 6; + COMMAND_EXECUTION_STATE_COMPLETED = 7; + COMMAND_EXECUTION_STATE_FAILED = 8; + COMMAND_EXECUTION_STATE_CANCELED_BEFORE_DISPATCH = 9; + COMMAND_EXECUTION_STATE_OUTCOME_UNKNOWN = 10; +} + message DeviceLifecycle { enum Lifecycle { STATE_INIT = 0; @@ -20,12 +73,22 @@ message CommandHeader { message Request { string device_id = 1; // 目标设备名称 google.protobuf.Timestamp timestamp = 2; // 请求时间 + string command_id = 3; + string expected_service_instance_id = 4; + optional uint64 expected_device_generation = 5; + uint32 valid_for_ms = 6; } message Feedback { bool success = 1; // 是否成功 string error_message = 2; // 错误信息（成功时为空） google.protobuf.Timestamp timestamp = 3; // 回复时间 + CommandReasonCode reason_code = 4; + string command_id = 5; + string service_instance_id = 6; + uint64 safety_epoch = 7; + uint64 device_generation = 8; + CommandExecutionState execution_state = 9; } } diff --git a/protos/cmvr/api/safety_command.proto b/protos/cmvr/api/safety_command.proto new file mode 100644 index 00000000..b227f842 --- /dev/null +++ b/protos/cmvr/api/safety_command.proto @@ -0,0 +1,186 @@ +syntax = "proto3"; + +package cmvr.api; + +import "cmvr/api/common.proto"; + +enum SafetyTriState { + SAFETY_TRI_STATE_UNKNOWN = 0; + SAFETY_TRI_STATE_FALSE = 1; + SAFETY_TRI_STATE_TRUE = 2; +} + +enum SafetyCondition { + SAFETY_CONDITION_UNKNOWN = 0; + SAFETY_CONDITION_NOMINAL = 1; + SAFETY_CONDITION_RESTRICTED = 2; + SAFETY_CONDITION_UNSAFE = 3; +} + +enum SystemAdmissionState { + SYSTEM_ADMISSION_STATE_UNSPECIFIED = 0; + SYSTEM_ADMISSION_STATE_STARTING = 1; + SYSTEM_ADMISSION_STATE_OPEN = 2; + SYSTEM_ADMISSION_STATE_STOPPING = 3; + SYSTEM_ADMISSION_STATE_LATCHED = 4; + SYSTEM_ADMISSION_STATE_RECOVERING = 5; + SYSTEM_ADMISSION_STATE_SHUTTING_DOWN = 6; +} + +enum DeviceAdmissionState { + DEVICE_ADMISSION_STATE_UNSPECIFIED = 0; + DEVICE_ADMISSION_STATE_OBSERVING = 1; + DEVICE_ADMISSION_STATE_OPEN = 2; + DEVICE_ADMISSION_STATE_BLOCKED = 3; + DEVICE_ADMISSION_STATE_QUARANTINED = 4; + DEVICE_ADMISSION_STATE_RECOVERING = 5; + DEVICE_ADMISSION_STATE_REMOVED = 6; +} + +enum SafetyBlockerScope { + SAFETY_BLOCKER_SCOPE_UNSPECIFIED = 0; + SAFETY_BLOCKER_SCOPE_DEVICE = 1; + SAFETY_BLOCKER_SCOPE_SYSTEM = 2; +} + +enum SafetyRecoveryRequirement { + SAFETY_RECOVERY_REQUIREMENT_UNSPECIFIED = 0; + SAFETY_RECOVERY_REQUIREMENT_REFRESH_ONLY = 1; + SAFETY_RECOVERY_REQUIREMENT_CLEAR_SOFTWARE_LATCH = 2; + SAFETY_RECOVERY_REQUIREMENT_HARDWARE_RELEASE_REQUIRED = 3; + SAFETY_RECOVERY_REQUIREMENT_MANUAL_INSPECTION_REQUIRED = 4; +} + +enum SafetyOperationResult { + SAFETY_OPERATION_RESULT_UNSPECIFIED = 0; + SAFETY_OPERATION_RESULT_SUCCEEDED = 1; + SAFETY_OPERATION_RESULT_RECOVERED = 2; + SAFETY_OPERATION_RESULT_VERIFIED_BUT_STILL_BLOCKED = 3; + SAFETY_OPERATION_RESULT_BLOCKER_REMAINS = 4; + SAFETY_OPERATION_RESULT_EPOCH_MISMATCH = 5; + SAFETY_OPERATION_RESULT_NOTHING_TO_RECOVER = 6; + SAFETY_OPERATION_RESULT_TIMED_OUT = 7; + SAFETY_OPERATION_RESULT_FAILED = 8; +} + +message DeviceIdList { + repeated string device_ids = 1; +} + +message SafetyScope { + oneof target { + bool all_devices = 1; + DeviceIdList devices = 2; + } +} + +message SafetyBlockerInfo { + CommandReasonCode reason_code = 1; + SafetyBlockerScope scope = 2; + SafetyRecoveryRequirement recovery_requirement = 3; + string source_id = 4; + string operation_id = 5; + uint64 first_observed_at_unix_ms = 6; + uint64 last_observed_at_unix_ms = 7; +} + +message DeviceSafetyStateInfo { + string device_id = 1; + string device_kind = 2; + string policy_family = 3; + string lifecycle_state = 4; + string health_state = 5; + DeviceAdmissionState admission_state = 6; + SafetyCondition condition = 7; + bool has_sample = 8; + bool snapshot_fresh = 9; + uint64 sample_age_ms = 10; + uint64 sample_sequence = 11; + uint64 observed_at_unix_ms = 12; + uint64 device_generation = 13; + SafetyTriState connected = 14; + SafetyTriState operational_ready = 15; + SafetyTriState quiescent = 16; + SafetyTriState motion_active = 17; + SafetyTriState actuator_enabled = 18; + SafetyTriState emergency_stop_active = 19; + SafetyTriState protective_stop_active = 20; + SafetyTriState fault_active = 21; + repeated SafetyBlockerInfo blockers = 22; +} + +message SafetyOperationTargetResult { + string target_id = 1; + SafetyOperationResult result = 2; + CommandReasonCode reason_code = 3; + string detail = 4; + DeviceAdmissionState before_state = 5; + DeviceAdmissionState after_state = 6; +} + +message SafetyParticipantResultInfo { + bool recorded = 1; + bool success = 2; + CommandReasonCode reason_code = 3; + string detail = 4; +} + +message SafetyParticipantStateInfo { + string participant_id = 1; + string phase = 2; + bool required = 3; + bool registered = 4; + bool barrier_active = 5; + bool barrier_retained = 6; + string operation_id = 7; + uint64 safety_epoch = 8; + SafetyParticipantResultInfo last_request = 9; + SafetyParticipantResultInfo last_verify = 10; + SafetyParticipantResultInfo last_release = 11; +} + +message GetSafetyStateCommand { + message Request { + SafetyScope scope = 1; + } + + message Feedback { + CommandHeader.Feedback header = 1; + SystemAdmissionState system_state = 2; + uint64 safety_epoch = 3; + string control_service_instance_id = 4; + string enforcement_mode = 5; + repeated DeviceSafetyStateInfo devices = 6; + string active_operation_id = 7; + string active_operation_phase = 8; + uint64 sampled_at_unix_ms = 9; + repeated SafetyParticipantStateInfo participants = 10; + } +} + +message RecoverSafetyStateCommand { + enum Mode { + MODE_UNSPECIFIED = 0; + VERIFY_ONLY = 1; + CLEAR_SOFTWARE_LATCH = 2; + } + + message Request { + string recovery_id = 1; + SafetyScope scope = 2; + uint64 expected_safety_epoch = 3; + Mode mode = 4; + string reason = 5; + uint32 timeout_ms = 6; + } + + 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_command.proto b/protos/cmvr/api/system_command.proto index b572d230..d99f623e 100644 --- a/protos/cmvr/api/system_command.proto +++ b/protos/cmvr/api/system_command.proto @@ -3,6 +3,7 @@ syntax = "proto3"; import "cmvr/api/agv_command.proto"; import "cmvr/api/arm_command.proto"; import "cmvr/api/common.proto"; +import "cmvr/api/safety_command.proto"; package cmvr.api; @@ -109,6 +110,16 @@ message GetSystemInfoCommand { // Changes whenever the in-process ActionQueue idempotency ledger is // recreated. Clients bind submissions and retries to this value. string action_service_instance_id = 8; + + // Effective server-side control-plane settings. These fields describe + // what is running, not merely what the configuration requested. + string grpc_transport_security = 9; + string grpc_authentication = 10; + string grpc_recovery_exposure = 11; + bool grpc_insecure_non_loopback = 12; + string control_service_instance_id = 13; + string safety_enforcement_mode = 14; + uint32 safety_schema_version = 15; } } @@ -141,10 +152,18 @@ message UpdateParamsCommand { message StopAllCommand { message Request { CommandHeader.Request header = 1; + string operation_id = 2; + string expected_service_instance_id = 3; + uint32 timeout_ms = 4; } message Feedback { CommandHeader.Feedback header = 1; + string operation_id = 2; + uint64 previous_safety_epoch = 3; + uint64 current_safety_epoch = 4; + SystemAdmissionState system_state = 5; + repeated SafetyOperationTargetResult targets = 6; } } diff --git a/protos/cmvr/api/system_service.proto b/protos/cmvr/api/system_service.proto index bb6dc4b8..44267733 100644 --- a/protos/cmvr/api/system_service.proto +++ b/protos/cmvr/api/system_service.proto @@ -1,6 +1,7 @@ syntax = "proto3"; import "cmvr/api/system_command.proto"; +import "cmvr/api/safety_command.proto"; package cmvr.api; @@ -15,4 +16,7 @@ service SystemService { rpc StopAll(StopAllCommand.Request) returns (StopAllCommand.Feedback) {} rpc ExecuteActionQueue(ActionQueueCommand.Request) returns (ActionQueueCommand.Feedback) {} + + rpc GetSafetyState(GetSafetyStateCommand.Request) returns (GetSafetyStateCommand.Feedback) {} + rpc RecoverSafetyState(RecoverSafetyStateCommand.Request) returns (RecoverSafetyStateCommand.Feedback) {} } diff --git a/protos/cmvr/config/device_manager_config/device_manager_config.proto b/protos/cmvr/config/device_manager_config/device_manager_config.proto index d36d328b..6c2e1ad0 100644 --- a/protos/cmvr/config/device_manager_config/device_manager_config.proto +++ b/protos/cmvr/config/device_manager_config/device_manager_config.proto @@ -1,6 +1,25 @@ syntax = "proto3"; package cmvr.config; +message SafetyCoordinatorConfig { + enum EnforcementMode { + ENFORCEMENT_MODE_UNSPECIFIED = 0; + LEGACY = 1; + SHADOW = 2; + ENFORCE_SELECTED = 3; + ENFORCE_ALL = 4; + } + + EnforcementMode mode = 1; + repeated string enforced_device_ids = 2; + uint32 stop_all_timeout_ms = 3; + uint32 recovery_timeout_ms = 4; + uint32 command_ledger_result_capacity = 5; + uint32 command_ledger_total_id_capacity = 6; + uint32 event_history_capacity = 7; + bool fail_startup_on_missing_control_capability = 8; +} + message DeviceConfigEntry { enum DeviceType { reserved 1, 2, 3, 4, 5, 6, 7, 10, 11; @@ -27,6 +46,10 @@ message DeviceConfigEntry { DeviceType type = 2; string config_file = 3; bool enable = 4; + bool safety_enforce = 5; + uint32 maximum_safety_snapshot_age_ms = 6; + uint32 safety_stop_timeout_ms = 7; + bool required_control_device = 8; } message DeviceManagerConfig { @@ -35,6 +58,7 @@ message DeviceManagerConfig { string description = 3; repeated DeviceConfigEntry devices = 4; bool init_all_motors_when_no_active_joints = 20; + SafetyCoordinatorConfig safety = 21; } message DeviceManagerRootConfig { DeviceManagerConfig device_manager = 1; diff --git a/protos/cmvr/config/grpc_server_config/grpc_server_config.proto b/protos/cmvr/config/grpc_server_config/grpc_server_config.proto index 927cb7fc..f054f223 100644 --- a/protos/cmvr/config/grpc_server_config/grpc_server_config.proto +++ b/protos/cmvr/config/grpc_server_config/grpc_server_config.proto @@ -24,6 +24,45 @@ message ArmTeleopBackendConfig { double max_position_step_rad = 11; } +message GRPCSecurityConfig { + enum TransportMode { + TRANSPORT_MODE_UNSPECIFIED = 0; + INSECURE = 1; + SERVER_TLS = 2; + MUTUAL_TLS = 3; + } + + enum AuthenticationMode { + AUTHENTICATION_MODE_UNSPECIFIED = 0; + DISABLED = 1; + STATIC_TOKEN = 2; + JWT = 3; + TLS_CLIENT_CERTIFICATE = 4; + } + + enum RecoveryExposure { + RECOVERY_EXPOSURE_UNSPECIFIED = 0; + RECOVERY_DISABLED = 1; + RECOVERY_LOCAL_ONLY = 2; + RECOVERY_AUTHORIZED = 3; + } + + TransportMode transport_mode = 1; + AuthenticationMode authentication_mode = 2; + RecoveryExposure recovery_exposure = 3; + bool allow_insecure_non_loopback = 4; + + // Reserved for optional providers. Selecting an unsupported provider causes + // startup to fail; it never falls back to DISABLED. + string server_certificate_file = 5; + string server_private_key_file = 6; + string client_ca_file = 7; + string static_token_file = 8; + string jwt_issuer = 9; + string jwt_audience = 10; + string audit_file = 11; +} + message GRPCServerConfig { string host = 1; string port = 2; @@ -36,6 +75,7 @@ message GRPCServerConfig { // default so configurations written before these fields remain low-latency. uint32 camera_stream_max_frame_age_ms = 6; ArmTeleopBackendConfig arm_teleop_backend = 7; + GRPCSecurityConfig security = 8; } message GRPCServerRootConfig { GRPCServerConfig grpc_server = 1;