From 4c8b320b4bb505ac366239eaa9a3df6d30e8ddf6 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 14 Aug 2026 01:24:17 +0800 Subject: [PATCH 1/8] fix(system): stop operational activities without shutdown --- cmvr-es/CMakeLists.txt | 1 + .../include/cartesian_velocity_controller.h | 24 +- .../src/cartesian_velocity_controller.cpp | 216 ++- .../arm/motor_robot_arm/CMakeLists.txt | 1 + .../motor_robot_arm/include/motor_robot_arm.h | 13 +- .../motor_robot_arm/src/motor_robot_arm.cpp | 191 ++- .../src/motor_robot_arm_mujoco_test.cpp | 375 +++++ cmvr-es/devices/biohead/abstract_biohead.h | 139 +- .../biohead_esp32/include/biohead_esp32.h | 45 +- .../biohead_esp32/src/biohead_esp32.cpp | 232 ++- cmvr-es/devices/camera/abstract_camera.h | 6 + .../include/hikvision_camera.h | 3 +- .../hikvision_camera/src/hikvision_camera.cpp | 28 +- .../tests/hikvision_camera_callback_test.cpp | 21 +- .../mujoco_camera/include/mujoco_camera.h | 4 + .../mujoco_camera/src/mujoco_camera.cpp | 35 + .../mujoco_camera/src/mujoco_camera_test.cpp | 36 + .../include/realsense_camera.h | 22 +- .../realsense_camera/src/realsense_camera.cpp | 444 ++++-- .../camera/uvc_camera/include/uvc_camera.h | 22 +- .../camera/uvc_camera/src/uvc_camera.cpp | 452 ++++-- cmvr-es/devices/dexhand/abstract_dexhand.h | 11 + .../dexhand/px_6ax_gen3/include/px_6ax_gen3.h | 3 + .../dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp | 45 +- .../dexhand/rh56dftp_dexhand/CMakeLists.txt | 27 + .../include/rh56dftp_dexhand.h | 32 +- .../rh56dftp_dexhand/src/rh56dftp_dexhand.cpp | 164 +- .../tests/rh56dftp_dexhand_stop_all_test.cpp | 348 +++++ cmvr-es/devices/gripper/abstract_gripper.h | 5 + .../motor/manager/include/motor_manager.h | 16 + .../motor/manager/src/motor_manager.cpp | 114 ++ cmvr-es/devices/speaker/abstract_speaker.h | 5 + .../speaker/ffmpeg_speaker/CMakeLists.txt | 29 + .../ffmpeg_speaker/include/ffmpeg_speaker.h | 2 + .../ffmpeg_speaker/src/ffmpeg_speaker.cpp | 13 +- .../tests/ffmpeg_speaker_lifecycle_test.cpp | 54 + cmvr-es/manager/README.md | 3 +- .../include/control_authority_manager.h | 88 +- .../src/control_authority_manager.cpp | 563 +++++-- .../tests/control_authority_manager_test.cpp | 667 ++++++++ .../device_manager/include/device_manager.h | 9 + .../device_manager/src/device_manager.cpp | 18 + .../tests/device_manager_snapshot_test.cpp | 111 +- .../manager/media_source_hub/CMakeLists.txt | 8 + .../include/media_source_hub.h | 38 +- .../src/device_media_source_adapter.cpp | 37 +- .../media_source_hub/src/media_source_hub.cpp | 287 +++- .../tests/media_source_hub_test.cpp | 661 +++++++- cmvr-es/manager/task_manager/CMakeLists.txt | 2 + .../task_manager/include/task_manager.h | 10 + .../manager/task_manager/src/task_manager.cpp | 159 +- .../tests/task_manager_lifecycle_test.cpp | 238 ++- cmvr-es/service/CMakeLists.txt | 186 +++ .../action/include/action_queue_executor.h | 45 +- .../action/src/action_queue_executor.cpp | 285 +++- .../camera_operational_activity_registry.h | 108 ++ .../include/camera_ptz_activity_registry.h | 91 ++ .../service/grpc/include/grpc_motor_service.h | 12 +- .../grpc/include/grpc_system_service.h | 5 + .../grpc/include/media_activity_coordinator.h | 119 ++ .../grpc/include/motor_activity_coordinator.h | 149 ++ .../camera_operational_activity_registry.cpp | 328 ++++ .../grpc/src/camera_ptz_activity_registry.cpp | 224 +++ cmvr-es/service/grpc/src/grpc_agv_service.cpp | 287 +++- cmvr-es/service/grpc/src/grpc_arm_service.cpp | 272 +++- .../grpc/src/grpc_arm_teleop_service.cpp | 70 +- .../service/grpc/src/grpc_camera_service.cpp | 225 ++- .../service/grpc/src/grpc_dexhand_service.cpp | 278 +++- .../service/grpc/src/grpc_head_service.cpp | 127 +- cmvr-es/service/grpc/src/grpc_hlc_service.cpp | 22 +- .../grpc/src/grpc_microphone_service.cpp | 82 +- .../service/grpc/src/grpc_motor_service.cpp | 149 +- .../service/grpc/src/grpc_speaker_service.cpp | 119 +- .../service/grpc/src/grpc_system_service.cpp | 1276 ++++++++++++++-- .../grpc/src/media_activity_coordinator.cpp | 437 ++++++ .../grpc/src/motor_activity_coordinator.cpp | 488 ++++++ ...era_operational_activity_registry_test.cpp | 491 ++++++ .../camera_ptz_activity_registry_test.cpp | 264 ++++ .../grpc/tests/grpc_agv_service_test.cpp | 248 ++- .../grpc/tests/grpc_arm_service_test.cpp | 161 +- .../tests/grpc_arm_teleop_service_test.cpp | 108 ++ .../grpc/tests/grpc_dexhand_service_test.cpp | 351 +++++ .../grpc/tests/grpc_head_service_test.cpp | 391 +++++ .../grpc/tests/grpc_motor_service_test.cpp | 260 +++- .../grpc/tests/grpc_system_service_test.cpp | 1346 ++++++++++++++++- .../tests/media_activity_coordinator_test.cpp | 262 ++++ .../tests/motor_activity_coordinator_test.cpp | 266 ++++ cmvr-es/service/quic_edge/CMakeLists.txt | 22 + .../quic_edge/include/quic_edge_service.h | 12 +- .../quic_edge/src/quic_edge_service.cpp | 93 +- .../tests/quic_edge_protocol_test.cpp | 154 +- cmvr-es/service/stop_all/CMakeLists.txt | 28 + .../include/deferred_stop_operation.h | 29 + .../include/stop_all_admission_gate.h | 89 ++ .../include/stop_operation_dispatcher.h | 77 + .../stop_all/src/stop_all_admission_gate.cpp | 90 ++ .../src/stop_operation_dispatcher.cpp | 190 +++ .../tests/stop_all_admission_gate_test.cpp | 104 ++ .../tests/stop_operation_dispatcher_test.cpp | 317 ++++ cmvr-es/task/CMakeLists.txt | 31 + .../include/grpc_server_task.h | 4 + .../grpc_server_task/src/grpc_server_task.cpp | 5 +- cmvr-es/task/quic_edge_task/CMakeLists.txt | 6 +- .../quic_edge_task/include/quic_edge_task.h | 1 + .../quic_edge_task/src/quic_edge_task.cpp | 75 +- .../tests/quic_edge_task_test.cpp | 81 + cmvr-es/task/task.h | 13 + .../include/touch_screen_task.h | 37 +- .../src/touch_screen_admission_test.cpp | 463 ++++++ .../src/touch_screen_task.cpp | 436 +++++- .../src/touch_screen_task_test.cpp | 244 +++ cmvr-es/task/ume_teleop_task/CMakeLists.txt | 2 + .../ume_teleop_task/include/ume_teleop_task.h | 2 + .../ume_teleop_task/src/ume_teleop_task.cpp | 64 +- .../tests/ume_teleop_task_test.cpp | 141 +- 115 files changed, 17323 insertions(+), 1096 deletions(-) create mode 100644 cmvr-es/devices/dexhand/rh56dftp_dexhand/tests/rh56dftp_dexhand_stop_all_test.cpp create mode 100644 cmvr-es/devices/speaker/ffmpeg_speaker/tests/ffmpeg_speaker_lifecycle_test.cpp create mode 100644 cmvr-es/service/grpc/include/camera_operational_activity_registry.h create mode 100644 cmvr-es/service/grpc/include/camera_ptz_activity_registry.h create mode 100644 cmvr-es/service/grpc/include/media_activity_coordinator.h create mode 100644 cmvr-es/service/grpc/include/motor_activity_coordinator.h create mode 100644 cmvr-es/service/grpc/src/camera_operational_activity_registry.cpp create mode 100644 cmvr-es/service/grpc/src/camera_ptz_activity_registry.cpp create mode 100644 cmvr-es/service/grpc/src/media_activity_coordinator.cpp create mode 100644 cmvr-es/service/grpc/src/motor_activity_coordinator.cpp create mode 100644 cmvr-es/service/grpc/tests/camera_operational_activity_registry_test.cpp create mode 100644 cmvr-es/service/grpc/tests/camera_ptz_activity_registry_test.cpp create mode 100644 cmvr-es/service/grpc/tests/grpc_dexhand_service_test.cpp create mode 100644 cmvr-es/service/grpc/tests/grpc_head_service_test.cpp create mode 100644 cmvr-es/service/grpc/tests/media_activity_coordinator_test.cpp create mode 100644 cmvr-es/service/grpc/tests/motor_activity_coordinator_test.cpp create mode 100644 cmvr-es/service/stop_all/CMakeLists.txt create mode 100644 cmvr-es/service/stop_all/include/deferred_stop_operation.h create mode 100644 cmvr-es/service/stop_all/include/stop_all_admission_gate.h create mode 100644 cmvr-es/service/stop_all/include/stop_operation_dispatcher.h create mode 100644 cmvr-es/service/stop_all/src/stop_all_admission_gate.cpp create mode 100644 cmvr-es/service/stop_all/src/stop_operation_dispatcher.cpp create mode 100644 cmvr-es/service/stop_all/tests/stop_all_admission_gate_test.cpp create mode 100644 cmvr-es/service/stop_all/tests/stop_operation_dispatcher_test.cpp create mode 100644 cmvr-es/task/touch_screen_task/src/touch_screen_admission_test.cpp diff --git a/cmvr-es/CMakeLists.txt b/cmvr-es/CMakeLists.txt index 7ef09113..e59b4eb0 100644 --- a/cmvr-es/CMakeLists.txt +++ b/cmvr-es/CMakeLists.txt @@ -8,6 +8,7 @@ add_subdirectory(simulate) add_subdirectory(devices) add_subdirectory(manager/control_authority) add_subdirectory(manager/device_manager) +add_subdirectory(service/stop_all) add_subdirectory(manager/media_source_hub) add_subdirectory(service/quic_edge) add_subdirectory(service/arm_teleop_client) diff --git a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h b/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h index 9dd0fc29..78fd5bb8 100644 --- a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h +++ b/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h @@ -52,8 +52,16 @@ public: private: void ensureWorkerStarted_(); - void workerLoop_(); - void sendZero_(); + void workerLoop_(std::uint64_t worker_generation); + bool workerGenerationCurrent_(std::uint64_t worker_generation) const; + std::optional sendVelocityIfCurrent_( + const JointVelocityCommand& velocity, + double acceleration, + std::uint64_t worker_generation); + void finishCommandIfCurrent_(std::uint64_t command_version, + std::uint64_t worker_generation); + void sendZeroIfCurrent_(std::uint64_t worker_generation); + void sendZeroNow_(); static double velocityNorm_(const std::vector& velocity); static double twistNorm_(const CartesianVelocity& velocity); @@ -65,15 +73,25 @@ private: ReadStateCallback read_state_; SendVelocityCallback send_velocity_; + // lifecycle_mutex_ serializes worker creation, join, and reset. It is held + // across join so a new command cannot start until the retired worker exits. + mutable std::mutex lifecycle_mutex_; std::unique_ptr worker_; + std::atomic worker_running_{false}; + mutable std::mutex mutex_; std::condition_variable cv_; - std::atomic stop_requested_{false}; + bool stop_requested_{false}; bool command_active_{false}; CartesianVelocity target_twist_{}; FrameType target_frame_{FrameType::Base}; double target_acceleration_{0.25}; std::uint64_t command_version_{0}; + + // Every worker output is checked while holding output_mutex_. shutdown() + // advances the generation before sending zero, fencing stale worker writes. + mutable std::mutex output_mutex_; + std::atomic worker_generation_{0}; std::atomic busy_{false}; }; diff --git a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp index efad63b6..16237be1 100644 --- a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp +++ b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp @@ -61,20 +61,23 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity, if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || acceleration <= 0.0) { return Result::failure(ArmErrorCode::InvalidArgument, "speedL invalid input"); } - if ((!worker_ || !worker_->joinable()) && busy_.exchange(true)) { - return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy"); - } - - ensureWorkerStarted_(); - std::uint64_t command_version = 0; { - std::lock_guard lock(mutex_); - target_twist_ = velocity; - target_acceleration_ = acceleration; - target_frame_ = frame; - command_active_ = true; - command_version = ++command_version_; + std::lock_guard lifecycle_lock(lifecycle_mutex_); + if (worker_ && worker_->joinable() && !worker_running_.load()) { + worker_->join(); + worker_.reset(); + } + ensureWorkerStarted_(); + { + std::lock_guard lock(mutex_); + target_twist_ = velocity; + target_acceleration_ = acceleration; + target_frame_ = frame; + command_active_ = true; + command_version = ++command_version_; + } + busy_.store(true); } cv_.notify_all(); @@ -100,16 +103,21 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity, Result CartesianVelocityController::stop(const std::optional acceleration) { - if (!worker_ || !worker_->joinable()) { - return Result::success(); - } { - std::lock_guard lock(mutex_); - target_twist_ = {}; - target_frame_ = FrameType::Base; - target_acceleration_ = acceleration.has_value() ? *acceleration : config_.stop_acceleration; - command_active_ = true; - ++command_version_; + std::lock_guard lifecycle_lock(lifecycle_mutex_); + if (!worker_ || !worker_->joinable() || !worker_running_.load()) { + return Result::success(); + } + { + std::lock_guard lock(mutex_); + target_twist_ = {}; + target_frame_ = FrameType::Base; + target_acceleration_ = + acceleration.has_value() ? *acceleration + : config_.stop_acceleration; + command_active_ = true; + ++command_version_; + } } cv_.notify_all(); return Result::success(); @@ -117,22 +125,35 @@ Result CartesianVelocityController::stop(const std::optional acceleratio void CartesianVelocityController::shutdown() { + std::lock_guard lifecycle_lock(lifecycle_mutex_); if (!worker_ || !worker_->joinable()) { busy_.store(false); return; } + + // Revoke the worker before issuing zero. All worker outputs perform their + // final generation check under output_mutex_, so none can follow this zero. + worker_generation_.fetch_add(1, std::memory_order_acq_rel); { std::lock_guard lock(mutex_); - stop_requested_.store(true); + stop_requested_ = true; command_active_ = false; target_twist_ = {}; target_frame_ = FrameType::Base; + ++command_version_; } cv_.notify_all(); + + sendZeroNow_(); + busy_.store(false); + worker_->join(); worker_.reset(); - stop_requested_.store(false); - busy_.store(false); + { + std::lock_guard lock(mutex_); + stop_requested_ = false; + command_active_ = false; + } } CartesianVelocity CartesianVelocityController::getCommandTwistBase() const @@ -148,12 +169,26 @@ void CartesianVelocityController::ensureWorkerStarted_() if (worker_ && worker_->joinable()) { return; } - stop_requested_.store(false); - worker_ = std::make_unique(&CartesianVelocityController::workerLoop_, this); + const auto worker_generation = + worker_generation_.fetch_add(1, std::memory_order_acq_rel) + 1; + { + std::lock_guard lock(mutex_); + stop_requested_ = false; + command_active_ = false; + } + worker_running_.store(true); + worker_ = std::make_unique( + &CartesianVelocityController::workerLoop_, this, worker_generation); } -void CartesianVelocityController::workerLoop_() +void CartesianVelocityController::workerLoop_( + const std::uint64_t worker_generation) { + struct RunningGuard { + std::atomic& running; + ~RunningGuard() { running.store(false); } + } running_guard{worker_running_}; + const double dt = config_.control_period_s; auto next_tick = std::chrono::steady_clock::now(); @@ -164,9 +199,10 @@ void CartesianVelocityController::workerLoop_() { std::unique_lock lock(mutex_); cv_.wait(lock, [&]() { - return stop_requested_.load() || command_active_; + return stop_requested_ || command_active_; }); - if (stop_requested_.load()) { + if (stop_requested_ || + !workerGenerationCurrent_(worker_generation)) { break; } target_twist = target_twist_; @@ -176,43 +212,44 @@ void CartesianVelocityController::workerLoop_() next_tick = std::chrono::steady_clock::now(); while (true) { + bool stopping = false; + std::uint64_t active_command_version = 0; { std::lock_guard lock(mutex_); - if (stop_requested_.load()) { - sendZero_(); - busy_.store(false); - return; - } + stopping = stop_requested_; if (!command_active_) { break; } target_twist = target_twist_; acceleration = target_acceleration_; target_frame = target_frame_; + active_command_version = command_version_; + } + if (stopping || + !workerGenerationCurrent_(worker_generation)) { + return; } if (!planner_->updateSpeedLAcceleration(acceleration)) { if (twistNorm_(target_twist) < config_.stop_twist_norm && acceleration <= 0.0) { - std::lock_guard lock(mutex_); - command_active_ = false; - sendZero_(); - busy_.store(false); + finishCommandIfCurrent_( + active_command_version, worker_generation); break; } CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration=" << acceleration; - sendZero_(); - busy_.store(false); - return; + finishCommandIfCurrent_( + active_command_version, worker_generation); + break; } std::vector q_now; std::vector qd_now; if (!read_state_(q_now, qd_now)) { CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] read_state failed"; - sendZero_(); - busy_.store(false); - return; + finishCommandIfCurrent_( + active_command_version, worker_generation); + break; } std::vector qd_cmd; @@ -222,31 +259,31 @@ void CartesianVelocityController::workerLoop_() << target_twist.vz << ", " << target_twist.wx << ", " << target_twist.wy << ", " << target_twist.wz << "], frame=" << (target_frame == FrameType::Tool ? "Tool" : "Base"); - sendZero_(); - busy_.store(false); - return; + finishCommandIfCurrent_( + active_command_version, worker_generation); + break; } JointVelocityCommand velocity_command; velocity_command.velocity = qd_cmd; - const auto send_result = send_velocity_(velocity_command, acceleration); - if (!send_result.ok()) { - CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] send_velocity failed: " - << send_result.message; - sendZero_(); - busy_.store(false); + const auto send_result = sendVelocityIfCurrent_( + velocity_command, acceleration, worker_generation); + if (!send_result.has_value()) { return; } + if (!send_result->ok()) { + CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] send_velocity failed: " + << send_result->message; + finishCommandIfCurrent_( + active_command_version, worker_generation); + break; + } if (twistNorm_(target_twist) < config_.stop_twist_norm && velocityNorm_(qd_cmd) < config_.stop_command_velocity_norm && velocityNorm_(qd_now) < config_.stop_measured_velocity_norm) { - { - std::lock_guard lock(mutex_); - command_active_ = false; - } - sendZero_(); - busy_.store(false); + finishCommandIfCurrent_( + active_command_version, worker_generation); break; } @@ -256,12 +293,67 @@ void CartesianVelocityController::workerLoop_() } } - sendZero_(); - busy_.store(false); + sendZeroIfCurrent_(worker_generation); + if (workerGenerationCurrent_(worker_generation)) { + busy_.store(false); + } } -void CartesianVelocityController::sendZero_() +bool CartesianVelocityController::workerGenerationCurrent_( + const std::uint64_t worker_generation) const { + return worker_generation_.load(std::memory_order_acquire) == + worker_generation; +} + +std::optional CartesianVelocityController::sendVelocityIfCurrent_( + const JointVelocityCommand& velocity, + const double acceleration, + const std::uint64_t worker_generation) +{ + std::lock_guard lock(output_mutex_); + if (!workerGenerationCurrent_(worker_generation)) { + return std::nullopt; + } + return send_velocity_(velocity, acceleration); +} + +void CartesianVelocityController::finishCommandIfCurrent_( + const std::uint64_t command_version, + const std::uint64_t worker_generation) +{ + std::lock_guard output_lock(output_mutex_); + { + std::lock_guard lock(mutex_); + if (command_version_ != command_version) { + return; + } + command_active_ = false; + busy_.store(false); + } + if (!workerGenerationCurrent_(worker_generation)) { + return; + } + JointVelocityCommand zero; + zero.velocity.assign(dof_, 0.0); + (void)send_velocity_(zero, 0.0); +} + +void CartesianVelocityController::sendZeroIfCurrent_( + const std::uint64_t worker_generation) +{ + std::lock_guard lock(output_mutex_); + if (!workerGenerationCurrent_(worker_generation) || !send_velocity_) { + return; + } + JointVelocityCommand zero; + zero.velocity.assign(dof_, 0.0); + (void)send_velocity_(zero, 0.0); +} + +void CartesianVelocityController::sendZeroNow_() +{ + std::lock_guard lock(output_mutex_); if (!send_velocity_) { return; } diff --git a/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt b/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt index c0b7d99b..9b024801 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt @@ -26,6 +26,7 @@ add_executable(motor_robot_arm_mujoco_test target_link_libraries(motor_robot_arm_mujoco_test PRIVATE cmvr_es::device::motor_robot_arm + cmvr_es::algorithms::arm_control cmvr_es::device::motor_manager cmvr_es::device::mujoco_motor_driver cmvr_es::mujoco_viewer diff --git a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h index 5b1d0b9d..8f40ab7b 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h +++ b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h @@ -100,6 +100,12 @@ public: bool busy() const override; private: + enum class TrajectoryExecutionResult { + Completed, + Canceled, + Failed, + }; + bool containsJoint_(const std::string& joint_name) const; bool validatePositionCommand_(const JointPositionCommand& cmd, std::string& error) const; bool validateVelocityCommand_(const JointVelocityCommand& cmd, std::string& error) const; @@ -108,7 +114,10 @@ private: std::vector readJointPosition_() const; bool configureAlgorithms_(); - bool executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory); + TrajectoryExecutionResult executeMoveLTrajectory_( + const CartesianJointTrajectory& trajectory, + const std::function& cancellation_requested, + std::uint64_t motion_generation); static CartesianVelocityController::Config toCartesianVelocityControllerConfig_( const config::CartesianVelocityControllerConfig& config); @@ -124,6 +133,7 @@ private: std::unordered_set joint_set_; std::string motor_system_id_; std::shared_ptr motor_manager_{nullptr}; + std::uint64_t motor_control_claim_id_{0}; std::shared_ptr ik_solver_{nullptr}; std::shared_ptr joint_planner_{nullptr}; @@ -131,6 +141,7 @@ private: std::unique_ptr cartesian_velocity_controller_{nullptr}; mutable std::mutex mutex_; + std::atomic motion_generation_{0}; std::atomic busy_{false}; std::atomic powered_on_{false}; mutable std::atomic joint_state_sequence_{0}; diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp index cce28a7d..407b8f35 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp @@ -29,6 +29,19 @@ struct BusyGuard { ~BusyGuard() { busy.store(false); } }; +bool cancellationRequested( + const std::function& cancellation_requested) noexcept +{ + if (!cancellation_requested) { + return false; + } + try { + return cancellation_requested(); + } catch (...) { + return true; + } +} + const config::JointLimitsConfig* configuredJointLimits( const config::ArmKinematicsConfig& kinematics) { @@ -124,6 +137,9 @@ MotorRobotArm::~MotorRobotArm() if (cartesian_velocity_controller_) { cartesian_velocity_controller_->shutdown(); } + if (motor_manager_ && motor_control_claim_id_ != 0U) { + motor_manager_->releaseArmJoints(motor_control_claim_id_); + } } bool MotorRobotArm::init() @@ -172,6 +188,16 @@ bool MotorRobotArm::init() CMVR_LOG(ERROR) << "[MotorRobotArm] failed to configure algorithms: " << id_; return false; } + if (motor_control_claim_id_ == 0U) { + std::string claim_error; + if (!motor_manager_->claimArmJoints( + id_, joint_names_, motor_control_claim_id_, &claim_error)) { + CMVR_LOG(ERROR) + << "[MotorRobotArm] failed to claim direct motor control: " + << id_ << ", detail=" << claim_error; + return false; + } + } CMVR_LOG(INFO) << "[MotorRobotArm] (init): Arm '" << id_ << "' init success"; return true; } @@ -327,6 +353,7 @@ Result MotorRobotArm::calibrateZeroQ(const std::string& joint_name) Result MotorRobotArm::emergencyStop() { + motion_generation_.fetch_add(1, std::memory_order_acq_rel); if (cartesian_velocity_controller_) { cartesian_velocity_controller_->shutdown(); } @@ -355,6 +382,8 @@ Result MotorRobotArm::setSpeedScaling(const double scaling) Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOptions& options) { + const auto motion_generation = + motion_generation_.load(std::memory_order_acquire); std::string error; if (!validatePositionCommand_(target, error)) { return Result::failure(ArmErrorCode::InvalidArgument, error); @@ -366,7 +395,12 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti return Result::failure(ArmErrorCode::RobotNotReady, "[MotorRobotArm] arm is busy: " + id_); } BusyGuard busy_guard{busy_}; - std::lock_guard lock(mutex_); + + if (cancellationRequested(options.cancellation_requested)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[MotorRobotArm] moveJ canceled before planning: " + id_); + } std::vector samples; if (!joint_planner_->planMoveJ(readJointPosition_(), target, options, speed_scaling_, samples)) { @@ -378,15 +412,31 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti std::vector> motors; motors.reserve(joint_names_.size()); - for (const auto& joint_name : joint_names_) { - auto motor = getMotor_(joint_name); - if (!motor) { - return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); + if (cancellationRequested(options.cancellation_requested)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[MotorRobotArm] moveJ canceled before dispatch: " + id_); + } + { + std::lock_guard lock(mutex_); + if (motion_generation_.load(std::memory_order_acquire) != + motion_generation) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[MotorRobotArm] moveJ canceled before dispatch: " + id_); } - if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + for (const auto& joint_name : joint_names_) { + auto motor = getMotor_(joint_name); + if (!motor) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "motor not found for joint: " + joint_name); + } + if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + } + motors.push_back(std::move(motor)); } - motors.push_back(std::move(motor)); } const auto t0 = std::chrono::steady_clock::now(); @@ -396,12 +446,32 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti if (sample.position.size() != motors.size()) { return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch"); } - for (std::size_t i = 0; i < motors.size(); ++i) { - const double qd = i < sample.velocity.size() ? sample.velocity[i] : 0.0; - if (!motors[i]->commandCyclicPosition(sample.position[i], qd)) { - return Result::failure(ArmErrorCode::CommandFailed, - "failed to command cyclic position for joint: " + - motors[i]->jointName()); + if (cancellationRequested(options.cancellation_requested)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[MotorRobotArm] moveJ canceled during execution: " + id_); + } + { + // The local generation and one complete joint frame are ordered + // against stopMotion(). External cancellation is intentionally + // evaluated before taking the device mutex because it is caller code. + std::lock_guard lock(mutex_); + if (motion_generation_.load(std::memory_order_acquire) != + motion_generation) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[MotorRobotArm] moveJ canceled during execution: " + id_); + } + for (std::size_t i = 0; i < motors.size(); ++i) { + const double qd = + i < sample.velocity.size() ? sample.velocity[i] : 0.0; + if (!motors[i]->commandCyclicPosition( + sample.position[i], qd)) { + return Result::failure( + ArmErrorCode::CommandFailed, + "failed to command cyclic position for joint: " + + motors[i]->jointName()); + } } } if (k + 1 < samples.size()) { @@ -452,6 +522,7 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity, Result MotorRobotArm::stopJ(const double acceleration) { + motion_generation_.fetch_add(1, std::memory_order_acq_rel); JointVelocityCommand zero; zero.velocity.assign(joint_names_.size(), 0.0); return speedJ(zero, acceleration, 0.0); @@ -461,6 +532,8 @@ Result MotorRobotArm::moveL(const CartesianPose& target, const MotionOptions& options, const FrameType frame) { + const auto motion_generation = + motion_generation_.load(std::memory_order_acquire); if (cartesian_velocity_controller_) { cartesian_velocity_controller_->shutdown(); } @@ -478,6 +551,12 @@ Result MotorRobotArm::moveL(const CartesianPose& target, } BusyGuard busy_guard{busy_}; + if (cancellationRequested(options.cancellation_requested)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[MotorRobotArm] moveL canceled before planning: " + id_); + } + std::vector q_start; std::vector qd_now; if (!readArmState_(q_start, qd_now)) { @@ -502,8 +581,22 @@ Result MotorRobotArm::moveL(const CartesianPose& target, << ", executable_path_m=" << trajectory.executable_path_length; } - return executeMoveLTrajectory_(trajectory) ? Result::success() - : Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed"); + switch (executeMoveLTrajectory_( + trajectory, + options.cancellation_requested, + motion_generation)) { + case TrajectoryExecutionResult::Completed: + return Result::success(); + case TrajectoryExecutionResult::Canceled: + return Result::failure( + ArmErrorCode::CommandRejected, + "[MotorRobotArm] moveL canceled during execution: " + id_); + case TrajectoryExecutionResult::Failed: + return Result::failure( + ArmErrorCode::CommandFailed, "moveL execution failed"); + } + return Result::failure( + ArmErrorCode::CommandFailed, "moveL execution failed"); } Result MotorRobotArm::speedL(const CartesianVelocity& velocity, @@ -530,16 +623,26 @@ Result MotorRobotArm::stopL(const std::optional acceleration) Result MotorRobotArm::stopMotion() { - stopL(0.0); - return stopJ(0.0); + // Revoke position trajectories before stopping the velocity worker. The + // controller fences its old worker and sends zero before joining it. + motion_generation_.fetch_add(1, std::memory_order_acq_rel); + if (cartesian_velocity_controller_) { + cartesian_velocity_controller_->shutdown(); + } + JointVelocityCommand zero; + zero.velocity.assign(joint_names_.size(), 0.0); + return speedJ(zero, 0.0, 0.0); } Result MotorRobotArm::shutdown() { + motion_generation_.fetch_add(1, std::memory_order_acq_rel); if (cartesian_velocity_controller_) { cartesian_velocity_controller_->shutdown(); } - return stopJ(0.0); + JointVelocityCommand zero; + zero.velocity.assign(joint_names_.size(), 0.0); + return speedJ(zero, 0.0, 0.0); } Result MotorRobotArm::startServoMode(const ServoOptions& options) @@ -816,25 +919,40 @@ bool MotorRobotArm::configureAlgorithms_() return true; } -bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory) +MotorRobotArm::TrajectoryExecutionResult +MotorRobotArm::executeMoveLTrajectory_( + const CartesianJointTrajectory& trajectory, + const std::function& cancellation_requested, + const std::uint64_t motion_generation) { if (trajectory.position.empty() || trajectory.velocity.size() != trajectory.position.size() || trajectory.time.size() != trajectory.position.size()) { - return false; + return TrajectoryExecutionResult::Failed; } if (trajectory.position.size() == 1) { - return true; + return cancellationRequested(cancellation_requested) || + motion_generation_.load(std::memory_order_acquire) != + motion_generation + ? TrajectoryExecutionResult::Canceled + : TrajectoryExecutionResult::Completed; } std::vector> motors; motors.reserve(joint_names_.size()); + if (cancellationRequested(cancellation_requested)) { + return TrajectoryExecutionResult::Canceled; + } { std::lock_guard lock(mutex_); + if (motion_generation_.load(std::memory_order_acquire) != + motion_generation) { + return TrajectoryExecutionResult::Canceled; + } for (const auto& joint_name : joint_names_) { auto motor = getMotor_(joint_name); if (!motor) { - return false; + return TrajectoryExecutionResult::Failed; } if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); @@ -849,11 +967,24 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj const auto& position = trajectory.position[i]; const auto& velocity = trajectory.velocity[i]; if (position.size() != motors.size() || velocity.size() != motors.size()) { - return false; + return TrajectoryExecutionResult::Failed; } - for (std::size_t j = 0; j < motors.size(); ++j) { - if (!motors[j]->commandCyclicPosition(position[j], velocity[j])) { - return false; + if (cancellationRequested(cancellation_requested)) { + return TrajectoryExecutionResult::Canceled; + } + { + // Keep the local stop decision and the complete joint frame in the + // same critical section as stopMotion()/stopJ(). + std::lock_guard lock(mutex_); + if (motion_generation_.load(std::memory_order_acquire) != + motion_generation) { + return TrajectoryExecutionResult::Canceled; + } + for (std::size_t j = 0; j < motors.size(); ++j) { + if (!motors[j]->commandCyclicPosition( + position[j], velocity[j])) { + return TrajectoryExecutionResult::Failed; + } } } next_deadline += std::chrono::duration_cast( @@ -861,7 +992,11 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj std::this_thread::sleep_until(next_deadline); } - return true; + return cancellationRequested(cancellation_requested) || + motion_generation_.load(std::memory_order_acquire) != + motion_generation + ? TrajectoryExecutionResult::Canceled + : TrajectoryExecutionResult::Completed; } CartesianVelocityController::Config MotorRobotArm::toCartesianVelocityControllerConfig_( diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp index 5ec14ee0..44148dd5 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp @@ -2,12 +2,17 @@ #include #include +#include #include #include +#include #include +#include +#include #include #include #include +#include #include #include #include @@ -99,6 +104,142 @@ struct ArmMujocoConfigCase { const char* config_file; }; +class BlockingCartesianMotionPlanner final : public CartesianMotionPlanner { +public: + bool configureSpeedL(const config::SpeedLPlannerConfig&, std::size_t) override + { + return true; + } + + bool configureMoveL(const config::MoveLPlannerConfig&) override + { + return true; + } + + bool planMoveL(const CartesianPose&, + const std::vector&, + const std::vector&, + double, + double, + double, + FrameType, + CartesianJointTrajectory&) override + { + return false; + } + + bool speedLStep(const CartesianVelocity&, + double, + const std::vector& q_measured, + const std::vector&, + std::vector& qd_command, + FrameType) override + { + std::unique_lock lock(mutex_); + if (step_count_++ == 0) { + first_step_entered_ = true; + condition_.notify_all(); + condition_.wait(lock, [&] { return release_first_step_; }); + } + qd_command.assign(q_measured.size(), 0.4); + return true; + } + + bool updateSpeedLAcceleration(double) override + { + return true; + } + + CartesianVelocity getSpeedLCommandTwistBase() const override + { + return {}; + } + + bool waitForFirstStep(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return condition_.wait_for( + lock, timeout, [&] { return first_step_entered_; }); + } + + void releaseFirstStep() + { + { + std::lock_guard lock(mutex_); + release_first_step_ = true; + } + condition_.notify_all(); + } + +private: + mutable std::mutex mutex_; + std::condition_variable condition_; + std::size_t step_count_{0}; + bool first_step_entered_{false}; + bool release_first_step_{false}; +}; + +class VelocityCommandRecorder { +public: + Result record(const JointVelocityCommand& velocity, double) + { + { + std::lock_guard lock(mutex_); + commands_.push_back(velocity.velocity); + } + condition_.notify_all(); + return Result::success(); + } + + bool waitForZero(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return condition_.wait_for(lock, timeout, [&] { + return std::any_of(commands_.begin(), commands_.end(), isZero_); + }); + } + + bool waitForNonZeroAfter(const std::size_t index, + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return condition_.wait_for(lock, timeout, [&] { + return index < commands_.size() && + std::any_of(commands_.begin() + index, + commands_.end(), + [](const auto& command) { + return !isZero_(command); + }); + }); + } + + std::size_t size() const + { + std::lock_guard lock(mutex_); + return commands_.size(); + } + + bool allZeroFrom(const std::size_t index) const + { + std::lock_guard lock(mutex_); + return index <= commands_.size() && + std::all_of(commands_.begin() + index, + commands_.end(), isZero_); + } + +private: + static bool isZero_(const std::vector& command) + { + return std::all_of(command.begin(), command.end(), [](const double value) { + return std::abs(value) < 1e-12; + }); + } + + mutable std::mutex mutex_; + std::condition_variable condition_; + std::vector> commands_; +}; + void PrintTo(const ArmMujocoConfigCase& value, std::ostream* os) { *os << value.name << " (" << value.config_file << ")"; @@ -314,6 +455,240 @@ TEST_P(MotorRobotArmMujocoTest, MoveL) EXPECT_LT(outcome.move_l_error, 0.04); } +TEST_P(MotorRobotArmMujocoTest, StopMotionDoesNotWaitForMoveJCancellationCallback) +{ + MotionOptions options; + options.velocity = 0.4; + options.acceleration = 2.0; + + std::mutex cancellation_mutex; + std::condition_variable cancellation_condition; + int cancellation_checks = 0; + bool release_dispatch_check = false; + options.cancellation_requested = [&] { + std::unique_lock lock(cancellation_mutex); + ++cancellation_checks; + cancellation_condition.notify_all(); + if (cancellation_checks == 3) { + cancellation_condition.wait(lock, [&] { + return release_dispatch_check; + }); + } + return false; + }; + + std::vector target(kDof, 0.0); + target[0] = 0.2; + auto motion = std::async(std::launch::async, [&] { + return arm_->moveJ(JointPositionCommand{target}, options); + }); + + bool cancellation_blocked = false; + { + std::unique_lock lock(cancellation_mutex); + cancellation_blocked = cancellation_condition.wait_for( + lock, std::chrono::seconds(2), [&] { + return cancellation_checks >= 3; + }); + } + + auto stop = std::async(std::launch::async, [&] { + return arm_->stopMotion(); + }); + EXPECT_TRUE(cancellation_blocked); + EXPECT_EQ(stop.wait_for(std::chrono::seconds(1)), + std::future_status::ready); + + { + std::lock_guard lock(cancellation_mutex); + release_dispatch_check = true; + } + cancellation_condition.notify_all(); + + const auto motion_result = motion.get(); + const auto stop_result = stop.get(); + EXPECT_EQ(motion_result.code, ArmErrorCode::CommandRejected) + << motion_result.message; + EXPECT_TRUE(stop_result.ok()) << stop_result.message; +} + +TEST_P(MotorRobotArmMujocoTest, StopMotionDoesNotWaitForMoveLCancellationCallback) +{ + const std::vector initial{ + 0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0.0, 0.0}; + MotionOptions joint_options; + joint_options.velocity = 2.8; + joint_options.acceleration = 20.0; + const auto setup = arm_->moveJ( + JointPositionCommand{initial}, joint_options); + ASSERT_TRUE(setup.ok()) << setup.message; + + CartesianPose target = arm_->getTcpPose(); + target.x += 0.05; + MotionOptions options; + options.velocity = 0.4; + options.acceleration = 5.0; + options.jerk = 20.0; + + std::mutex cancellation_mutex; + std::condition_variable cancellation_condition; + int cancellation_checks = 0; + bool release_dispatch_check = false; + options.cancellation_requested = [&] { + std::unique_lock lock(cancellation_mutex); + ++cancellation_checks; + cancellation_condition.notify_all(); + if (cancellation_checks == 3) { + cancellation_condition.wait(lock, [&] { + return release_dispatch_check; + }); + } + return false; + }; + + auto motion = std::async(std::launch::async, [&] { + return arm_->moveL(target, options, FrameType::Base); + }); + + bool cancellation_blocked = false; + { + std::unique_lock lock(cancellation_mutex); + cancellation_blocked = cancellation_condition.wait_for( + lock, std::chrono::seconds(2), [&] { + return cancellation_checks >= 3; + }); + } + + auto stop = std::async(std::launch::async, [&] { + return arm_->stopMotion(); + }); + EXPECT_TRUE(cancellation_blocked); + EXPECT_EQ(stop.wait_for(std::chrono::seconds(1)), + std::future_status::ready); + + { + std::lock_guard lock(cancellation_mutex); + release_dispatch_check = true; + } + cancellation_condition.notify_all(); + + const auto motion_result = motion.get(); + const auto stop_result = stop.get(); + EXPECT_EQ(motion_result.code, ArmErrorCode::CommandRejected) + << motion_result.message; + EXPECT_TRUE(stop_result.ok()) << stop_result.message; +} + +TEST_P(MotorRobotArmMujocoTest, StopMotionCancelsMoveJWithoutExternalCallback) +{ + MotionOptions options; + options.velocity = 0.1; + options.acceleration = 0.5; + + std::vector target(kDof, 0.0); + target[0] = 0.4; + auto motion = std::async(std::launch::async, [&] { + return arm_->moveJ(JointPositionCommand{target}, options); + }); + + waitFor([&] { return arm_->busy(); }, std::chrono::seconds(1)); + const auto stop_result = arm_->stopMotion(); + const auto motion_result = motion.get(); + + EXPECT_TRUE(stop_result.ok()) << stop_result.message; + EXPECT_EQ(motion_result.code, ArmErrorCode::CommandRejected) + << motion_result.message; + EXPECT_FALSE(arm_->busy()); +} + +TEST_P(MotorRobotArmMujocoTest, StopMotionCancelsMoveLWithoutExternalCallback) +{ + const std::vector initial{ + 0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0.0, 0.0}; + MotionOptions joint_options; + joint_options.velocity = 2.8; + joint_options.acceleration = 20.0; + const auto setup = arm_->moveJ( + JointPositionCommand{initial}, joint_options); + ASSERT_TRUE(setup.ok()) << setup.message; + + CartesianPose target = arm_->getTcpPose(); + target.x += 0.08; + MotionOptions options; + options.velocity = 0.1; + options.acceleration = 1.0; + options.jerk = 5.0; + auto motion = std::async(std::launch::async, [&] { + return arm_->moveL(target, options, FrameType::Base); + }); + + waitFor([&] { return arm_->busy(); }, std::chrono::seconds(1)); + const auto stop_result = arm_->stopMotion(); + const auto motion_result = motion.get(); + + EXPECT_TRUE(stop_result.ok()) << stop_result.message; + EXPECT_EQ(motion_result.code, ArmErrorCode::CommandRejected) + << motion_result.message; + EXPECT_FALSE(arm_->busy()); +} + +TEST(CartesianVelocityControllerTest, + ShutdownFencesStaleWriteAndLaterSpeedLRestartsWorker) +{ + constexpr std::size_t dof = 2; + auto planner = std::make_shared(); + VelocityCommandRecorder recorder; + CartesianVelocityController controller( + CartesianVelocityController::Config{}, + planner, + dof, + [](std::vector& q, std::vector& qd) { + q.assign(dof, 0.0); + qd.assign(dof, 0.0); + return true; + }, + [&](const JointVelocityCommand& command, const double acceleration) { + return recorder.record(command, acceleration); + }); + + CartesianVelocity velocity; + velocity.vx = 0.1; + const auto first = controller.speedL( + velocity, 0.5, 0.0, FrameType::Base); + ASSERT_TRUE(first.ok()) << first.message; + const bool first_step_entered = + planner->waitForFirstStep(std::chrono::seconds(1)); + if (!first_step_entered) { + planner->releaseFirstStep(); + controller.shutdown(); + FAIL() << "velocity worker did not enter the blocking planner step"; + } + + auto shutdown = std::async(std::launch::async, [&] { + controller.shutdown(); + }); + EXPECT_TRUE(recorder.waitForZero(std::chrono::seconds(1))); + EXPECT_EQ(shutdown.wait_for(std::chrono::milliseconds(20)), + std::future_status::timeout); + + const auto zero_index = recorder.size(); + planner->releaseFirstStep(); + EXPECT_EQ(shutdown.wait_for(std::chrono::seconds(1)), + std::future_status::ready); + shutdown.get(); + EXPECT_TRUE(recorder.allZeroFrom(zero_index)); + EXPECT_FALSE(controller.busy()); + + const auto restart_index = recorder.size(); + const auto restarted = controller.speedL( + velocity, 0.5, 0.0, FrameType::Base); + EXPECT_TRUE(restarted.ok()) << restarted.message; + EXPECT_TRUE(recorder.waitForNonZeroAfter( + restart_index, std::chrono::seconds(1))); + controller.shutdown(); + EXPECT_FALSE(controller.busy()); +} + TEST_P(MotorRobotArmMujocoTest, SpeedL) { MotorRobotArm& arm = *arm_; diff --git a/cmvr-es/devices/biohead/abstract_biohead.h b/cmvr-es/devices/biohead/abstract_biohead.h index aaf28658..26ac0d35 100644 --- a/cmvr-es/devices/biohead/abstract_biohead.h +++ b/cmvr-es/devices/biohead/abstract_biohead.h @@ -1,6 +1,11 @@ #ifndef ABSTRACT_BIOHEAD_H #define ABSTRACT_BIOHEAD_H #pragma once + +#include +#include +#include + #include "../abstract_device.h" namespace cmvr::device { @@ -58,6 +63,8 @@ namespace cmvr::device { // 抽象头部类 class AbstractBiohead : public AbstractDevice { public: + using OperationalToken = std::uint64_t; + AbstractBiohead() = default; ~AbstractBiohead() override = default; @@ -76,20 +83,140 @@ namespace cmvr::device { virtual void expressionSadness() {}; virtual void expressionYawn() {}; + // Capture under the process-wide StopAll admission gate. Commands + // from an older generation are rejected after operational stop. + OperationalToken beginOperationalActivity() const noexcept + { + std::lock_guard lock(operational_mutex_); + return operational_generation_; + } + + virtual bool setExpressionPoseIfCurrent( + OperationalToken token, + FacialExpressionState& expression_state, + double vel = 0.5, + double acc = 0.1) + { + return runIfOperationalActivityCurrent_(token, [&] { + setExpressionPose(expression_state, vel, acc); + }); + } + + virtual bool streamFacialPoseIfCurrent( + OperationalToken token, + FacialExpressionState& expression_state, + double vel, + double acc) + { + return runIfOperationalActivityCurrent_(token, [&] { + streamFacialPose(expression_state, vel, acc); + }); + } + + virtual bool speakStartIfCurrent(OperationalToken token) + { + return runIfOperationalActivityCurrent_(token, [&] { + speakstart(); + }); + } + + virtual bool expressionHappyIfCurrent(OperationalToken token) + { + return runIfOperationalActivityCurrent_(token, [&] { + expressionHappy(); + }); + } + + virtual bool expressionSurprisedIfCurrent(OperationalToken token) + { + return runIfOperationalActivityCurrent_(token, [&] { + expressionSurprised(); + }); + } + + virtual bool expressionTiredIfCurrent(OperationalToken token) + { + return runIfOperationalActivityCurrent_(token, [&] { + expressionTired(); + }); + } + + virtual bool expressionAngryIfCurrent(OperationalToken token) + { + return runIfOperationalActivityCurrent_(token, [&] { + expressionAngry(); + }); + } + + virtual bool expressionSadnessIfCurrent(OperationalToken token) + { + return runIfOperationalActivityCurrent_(token, [&] { + expressionSadness(); + }); + } + + virtual bool expressionYawnIfCurrent(OperationalToken token) + { + return runIfOperationalActivityCurrent_(token, [&] { + expressionYawn(); + }); + } + + // Stops expression motion and speaking without closing the device. + // True confirms that old activity was fenced and the hold completed. + virtual bool stopOperationalActivity() + { + invalidateOperationalActivities_(); + speakstop(); + (void)runOperationalStop_([&] { eStop(); }); + return false; + } + FacialExpressionState expression_state_; - std::atomic emergency_stop_requested = false; - + protected: + template + bool runIfOperationalActivityCurrent_( + const OperationalToken token, + Operation&& operation) + { + std::lock_guard lock(operational_mutex_); + if (token == 0U || token != operational_generation_) { + return false; + } + std::forward(operation)(); + return true; + } + template + bool runOperationalStop_(Operation&& operation) + { + std::lock_guard lock(operational_mutex_); + std::forward(operation)(); + return true; + } + bool operationalActivityCurrent_( + const OperationalToken token) const noexcept + { + std::lock_guard lock(operational_mutex_); + return token != 0U && token == operational_generation_; + } + void invalidateOperationalActivities_() noexcept + { + std::lock_guard lock(operational_mutex_); + ++operational_generation_; + if (operational_generation_ == 0U) { + ++operational_generation_; + } + } + private: + mutable std::mutex operational_mutex_; + OperationalToken operational_generation_{1U}; }; } // namespace cmvr::device #endif // ABSTRACT_BIOHEAD_H - - - - diff --git a/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h b/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h index 61d301ba..dd0d31a1 100644 --- a/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h +++ b/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h @@ -4,10 +4,12 @@ #include "../../abstract_biohead.h" #include "../../../../hardware/include/esp32_serial_port.h" #include "cmvr/config/biohead_config/biohead_config.pb.h" +#include #include #include #include #include +#include namespace cmvr::device { @@ -19,7 +21,7 @@ namespace cmvr::device { class BioHeadRobot : public AbstractBiohead { public: explicit BioHeadRobot(const config::BioHeadRobotConfig &config); - ~BioHeadRobot() override = default; + ~BioHeadRobot() override; std::string typeName() const override { return "BioHeadRobot"; } bool init() override; @@ -30,7 +32,25 @@ namespace cmvr::device { void streamFacialPose(FacialExpressionState& expression_state, double vel, double acc) override; void speakstart() override; void speakstop() override; - void speakthread(); + bool stopOperationalActivity() override; + + bool setExpressionPoseIfCurrent( + OperationalToken token, + FacialExpressionState& expression_state, + double vel = 0.5, + double acc = 0.1) override; + bool streamFacialPoseIfCurrent( + OperationalToken token, + FacialExpressionState& expression_state, + double vel, + double acc) override; + bool speakStartIfCurrent(OperationalToken token) override; + bool expressionHappyIfCurrent(OperationalToken token) override; + bool expressionSurprisedIfCurrent(OperationalToken token) override; + bool expressionTiredIfCurrent(OperationalToken token) override; + bool expressionAngryIfCurrent(OperationalToken token) override; + bool expressionSadnessIfCurrent(OperationalToken token) override; + bool expressionYawnIfCurrent(OperationalToken token) override; void expressionHappy()override; void expressionSurprised()override; @@ -43,11 +63,23 @@ namespace cmvr::device { private: // 内部方法 void parseConfig(const config::BioHeadRobotConfig &config); - void sendServoCommands( const std::vector& targets, uint16_t duration_ms); + bool sendServoCommands( + const std::vector& targets, + uint16_t duration_ms, + bool force = false); + bool sendRawIfCurrent( + OperationalToken token, + const std::vector& raw_data); uint16_t angleToRaw(double angle); double normalizeToAngle(double normalized, size_t index); - void sendExpression(const std::vector& device_64_angles, const std::vector& device_65_angles, int step_ms); + bool sendExpression( + OperationalToken token, + const std::vector& device_64_angles, + const std::vector& device_65_angles, + int step_ms); + bool startSpeaking(OperationalToken token); + void speakthread(OperationalToken token); @@ -69,8 +101,9 @@ namespace cmvr::device { std::shared_ptr speak_thread_; std::atomic speak_running_{false}; - - + std::mutex speak_mutex_; + std::mutex expression_wait_mutex_; + std::condition_variable expression_wait_cv_; }; diff --git a/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp b/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp index 5e86e722..7fd504a6 100644 --- a/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp +++ b/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp @@ -21,6 +21,11 @@ BioHeadRobot::BioHeadRobot(const config::BioHeadRobotConfig &config) { } +BioHeadRobot::~BioHeadRobot() +{ + speakstop(); +} + bool BioHeadRobot::init() { @@ -112,13 +117,36 @@ double BioHeadRobot::normalizeToAngle(double normalized, size_t index) { } void BioHeadRobot::getState(RobotState &state) { + std::lock_guard lock(stateMutex_); state.error = false; state.joint_positions = current_joints_; } void BioHeadRobot::eStop() { CMVR_LOG(WARNING) << "[BioHeadRobot] Emergency stop: hold current joint positions."; - sendServoCommands(current_joints_, 100); // 快速下发当前角度 + (void)sendServoCommands(last_joints_, 0, true); +} + +bool BioHeadRobot::setExpressionPoseIfCurrent( + const OperationalToken token, + FacialExpressionState& expression_state, + const double vel, + const double acc) +{ + return runIfOperationalActivityCurrent_(token, [&] { + setExpressionPose(expression_state, vel, acc); + }); +} + +bool BioHeadRobot::streamFacialPoseIfCurrent( + const OperationalToken token, + FacialExpressionState& expression_state, + const double vel, + const double acc) +{ + return runIfOperationalActivityCurrent_(token, [&] { + streamFacialPose(expression_state, vel, acc); + }); } void BioHeadRobot::setExpressionPose(FacialExpressionState& expression_state, double vel, double acc) { @@ -160,8 +188,10 @@ void BioHeadRobot::setExpressionPose(FacialExpressionState& expression_state, do for (size_t i = 0; i < joints.size(); ++i) { CMVR_LOG(INFO) << "Joint[" << i << "] = " << joints[i]; // 打印每个关节的角度 } - uint16_t duration = static_cast(1000.0 / vel); - sendServoCommands(joints, duration); + const uint16_t duration = vel > 0.0 + ? static_cast(1000.0 / vel) + : 0U; + (void)sendServoCommands(joints, duration); } @@ -208,17 +238,30 @@ void BioHeadRobot::streamFacialPose(FacialExpressionState& expression_state, dou CMVR_LOG(INFO) << "嘴角3=： " << ": " << joints[15]; CMVR_LOG(INFO) << "嘴角4=： " << ": " << joints[16]; - uint16_t duration = static_cast(1000.0 / vel); - sendServoCommands(joints, duration); + const uint16_t duration = vel > 0.0 + ? static_cast(1000.0 / vel) + : 0U; + (void)sendServoCommands(joints, duration); } void BioHeadRobot::speakstart() { + (void)speakStartIfCurrent(beginOperationalActivity()); +} + +bool BioHeadRobot::speakStartIfCurrent(const OperationalToken token) +{ + return startSpeaking(token); +} + +bool BioHeadRobot::startSpeaking(const OperationalToken token) +{ + std::lock_guard lock(speak_mutex_); if (speak_running_.load()) { CMVR_LOG(INFO) << "[BioHeadRobot] speak thread already running."; - return; + return operationalActivityCurrent_(token); } // 检查 channels 中是否有 65:8 和 65:9 @@ -229,12 +272,9 @@ void BioHeadRobot::speakstart() { } if (!found8 || !found9) { CMVR_LOG(ERROR) << "[BioHeadRobot] Required servo channels not found (addr 65 ch 8/9). speakstart aborted."; - return; + return false; } - // 启动线程 - speak_running_.store(true); - // 清理旧线程（若有） if (speak_thread_ && speak_thread_->joinable()) { try { @@ -245,19 +285,24 @@ void BioHeadRobot::speakstart() { speak_thread_.reset(); } - speak_thread_ = std::make_shared(&BioHeadRobot::speakthread, this); + bool started = false; + const bool current = runIfOperationalActivityCurrent_(token, [&] { + speak_running_.store(true, std::memory_order_release); + speak_thread_ = std::make_shared( + &BioHeadRobot::speakthread, this, token); + started = true; + }); + if (!current || !started) { + speak_running_.store(false, std::memory_order_release); + return false; + } CMVR_LOG(INFO) << "[BioHeadRobot] speak thread started."; + return true; } void BioHeadRobot::speakstop() { - { - if (!speak_running_.load()) { - CMVR_LOG(INFO) << "[BioHeadRobot] speak thread not running."; - return; - } - speak_running_.store(false); - } - // 唤醒线程（如果在 wait 中） + std::lock_guard lock(speak_mutex_); + speak_running_.store(false, std::memory_order_release); // join 并清理线程对象 if (speak_thread_) { @@ -275,7 +320,20 @@ void BioHeadRobot::speakstop() { CMVR_LOG(INFO) << "[BioHeadRobot] speak thread stopped."; } -void BioHeadRobot::speakthread() { +bool BioHeadRobot::stopOperationalActivity() +{ + invalidateOperationalActivities_(); + expression_wait_cv_.notify_all(); + speakstop(); + + bool hold_confirmed = false; + (void)runOperationalStop_([&] { + hold_confirmed = sendServoCommands(last_joints_, 0, true); + }); + return hold_confirmed; +} + +void BioHeadRobot::speakthread(const OperationalToken token) { CMVR_LOG(INFO) << "[BioHeadRobot] speakthread running."; // 固定参数 @@ -313,7 +371,11 @@ void BioHeadRobot::speakthread() { } // 以当前角度为基准 - std::vector base = current_joints_; + std::vector base; + { + std::lock_guard lock(stateMutex_); + base = current_joints_; + } if (base.size() != channels_.size()) { base.resize(channels_.size(), 90.0); } @@ -346,7 +408,8 @@ void BioHeadRobot::speakthread() { double current_random_factor = 0.0; const double random_update_interval = 0.2; // 每0.2秒更新一次随机扰动 - while (speak_running_.load()) { + while (speak_running_.load(std::memory_order_acquire) && + operationalActivityCurrent_(token)) { auto now = std::chrono::steady_clock::now(); double t = std::chrono::duration_cast>(now - start).count(); @@ -452,7 +515,9 @@ void BioHeadRobot::speakthread() { } // 下发 - serial_->sendRawServoData(raw_data); + if (!sendRawIfCurrent(token, raw_data)) { + break; + } // 控制循环频率 std::this_thread::sleep_for(std::chrono::milliseconds(step_ms)); @@ -483,12 +548,23 @@ void BioHeadRobot::speakthread() { } } - serial_->sendRawServoData(restore_data); - CMVR_LOG(INFO) << "[BioHeadRobot] speakthread exiting and restored base pose."; + if (sendRawIfCurrent(token, restore_data)) { + CMVR_LOG(INFO) + << "[BioHeadRobot] speakthread exiting and restored base pose."; + } else { + CMVR_LOG(INFO) + << "[BioHeadRobot] speakthread stopped without a stale restore."; + } + speak_running_.store(false, std::memory_order_release); } -void BioHeadRobot::sendExpression(const std::vector& device_64_angles, const std::vector& device_65_angles, int step_ms) { +bool BioHeadRobot::sendExpression( + const OperationalToken token, + const std::vector& device_64_angles, + const std::vector& device_65_angles, + const int step_ms) +{ std::vector raw_data; // 处理设备64角度 @@ -511,8 +587,20 @@ void BioHeadRobot::sendExpression(const std::vector& device_64_angles, c raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节 } - serial_->sendRawServoData(raw_data); - std::this_thread::sleep_for(std::chrono::seconds(5)); + if (!sendRawIfCurrent(token, raw_data)) { + return false; + } + { + std::unique_lock lock(expression_wait_mutex_); + if (expression_wait_cv_.wait_for( + lock, + std::chrono::seconds(5), + [this, token] { + return !operationalActivityCurrent_(token); + })) { + return false; + } + } // 恢复到原始角度 // 设备64角度（10通道） @@ -542,50 +630,78 @@ void BioHeadRobot::sendExpression(const std::vector& device_64_angles, c raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节 } - serial_->sendRawServoData(raw_data_neutral); + return sendRawIfCurrent(token, raw_data_neutral); } //高兴 void BioHeadRobot::expressionHappy() { + (void)expressionHappyIfCurrent(beginOperationalActivity()); +} +bool BioHeadRobot::expressionHappyIfCurrent(const OperationalToken token) { const std::vector device_64_angles = {90, 90, 90, 90, 80, 125, 100, 60, 90, 90}; const std::vector device_65_angles = {100, 80, 125, 135, 100, 105, 110, 90, 90, 90}; - sendExpression(device_64_angles, device_65_angles, 0); + return sendExpression(token, device_64_angles, device_65_angles, 0); } //惊讶 void BioHeadRobot::expressionSurprised() { + (void)expressionSurprisedIfCurrent(beginOperationalActivity()); +} +bool BioHeadRobot::expressionSurprisedIfCurrent(const OperationalToken token) { const std::vector device_64_angles = {90, 100, 100, 70, 20, 140, 130, 50, 90, 90}; const std::vector device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 70, 110}; - sendExpression(device_64_angles, device_65_angles, 0); + return sendExpression(token, device_64_angles, device_65_angles, 0); } //睡觉 void BioHeadRobot::expressionTired() { + (void)expressionTiredIfCurrent(beginOperationalActivity()); +} +bool BioHeadRobot::expressionTiredIfCurrent(const OperationalToken token) { const std::vector device_64_angles = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90}; const std::vector device_65_angles = {90, 90, 90, 90, 90, 105, 110, 90, 85, 95}; - sendExpression(device_64_angles, device_65_angles, 0); + return sendExpression(token, device_64_angles, device_65_angles, 0); } //愤怒 void BioHeadRobot::expressionAngry() { + (void)expressionAngryIfCurrent(beginOperationalActivity()); +} +bool BioHeadRobot::expressionAngryIfCurrent(const OperationalToken token) { const std::vector device_64_angles = {90, 70, 90, 110, 70, 125, 110, 80, 70, 90}; const std::vector device_65_angles = {100, 80, 130, 130, 70, 55, 50, 125, 90, 90}; - sendExpression(device_64_angles, device_65_angles, 0); + return sendExpression(token, device_64_angles, device_65_angles, 0); } //悲伤 void BioHeadRobot::expressionSadness() { + (void)expressionSadnessIfCurrent(beginOperationalActivity()); +} +bool BioHeadRobot::expressionSadnessIfCurrent(const OperationalToken token) { const std::vector device_64_angles = {90, 70, 90, 110, 70, 125, 110, 80, 90, 90}; const std::vector device_65_angles = {100, 80, 130, 130, 70, 55, 50, 125, 90, 90}; - sendExpression(device_64_angles, device_65_angles, 0); + return sendExpression(token, device_64_angles, device_65_angles, 0); } //打哈欠 void BioHeadRobot::expressionYawn() { + (void)expressionYawnIfCurrent(beginOperationalActivity()); +} +bool BioHeadRobot::expressionYawnIfCurrent(const OperationalToken token) { const std::vector device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90}; const std::vector device_65_angles = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90}; - sendExpression(device_64_angles, device_65_angles, 0); + return sendExpression(token, device_64_angles, device_65_angles, 0); } -void BioHeadRobot::sendServoCommands(const std::vector& targets, uint16_t duration_ms) { +bool BioHeadRobot::sendServoCommands( + const std::vector& targets, + const uint16_t duration_ms, + const bool force) +{ + if (!serial_ || targets.size() != channels_.size() || + targets.size() != min_angles_.size() || + targets.size() != max_angles_.size() || + targets.size() != last_joints_.size()) { + return false; + } std::vector addrs, chs; std::vector raws; @@ -598,17 +714,14 @@ void BioHeadRobot::sendServoCommands(const std::vector& targets, uint16_ continue; } - // 更新 last_joints_，只有当角度变化较大时才更新 - last_joints_[i] = tgt; - // 准备打包数据 addrs.push_back(channels_[i].addr); chs.push_back(channels_[i].channel); raws.push_back(angleToRaw(tgt)); } // 2. 如果没有任何通道需要更新，就直接返回 - if (raws.empty()) { - return; + if (raws.empty() && !force) { + return true; } std::vector raw_data; // 原始格式处理 @@ -640,7 +753,41 @@ void BioHeadRobot::sendServoCommands(const std::vector& targets, uint16_ - serial_->sendRawServoData(raw_data); + const bool sent = serial_->sendRawServoData(raw_data); + if (sent) { + for (std::size_t i = 0; i < targets.size(); ++i) { + last_joints_[i] = + std::clamp(targets[i], min_angles_[i], max_angles_[i]); + } + std::lock_guard lock(stateMutex_); + current_joints_ = last_joints_; + } + return sent; +} + +bool BioHeadRobot::sendRawIfCurrent( + const OperationalToken token, + const std::vector& raw_data) +{ + bool sent = false; + const bool current = runIfOperationalActivityCurrent_(token, [&] { + sent = serial_ && serial_->sendRawServoData(raw_data); + if (!sent || raw_data.size() % 5U != 0U) { + return; + } + for (std::size_t offset = 0; offset < raw_data.size(); offset += 5U) { + for (std::size_t index = 0; index < channels_.size(); ++index) { + if (channels_[index].addr == raw_data[offset] && + channels_[index].channel == raw_data[offset + 1U]) { + last_joints_[index] = raw_data[offset + 2U]; + break; + } + } + } + std::lock_guard lock(stateMutex_); + current_joints_ = last_joints_; + }); + return current && sent; } @@ -651,4 +798,3 @@ uint16_t BioHeadRobot::angleToRaw(double angle) { } // namespace cmvr::device - diff --git a/cmvr-es/devices/camera/abstract_camera.h b/cmvr-es/devices/camera/abstract_camera.h index a3082602..7d28efca 100644 --- a/cmvr-es/devices/camera/abstract_camera.h +++ b/cmvr-es/devices/camera/abstract_camera.h @@ -131,6 +131,12 @@ namespace cmvr::device { virtual bool startStreaming() {return true;} virtual void stopStreaming() {} + virtual bool startOperationalActivity() { return start(); } + // Stops activity started by CameraService::StartCamera without + // tearing down the device lifecycle. Implementations must return true + // only after the camera is quiescent and a later start() can resume it + // without another init(). Unsupported backends fail closed. + virtual bool stopOperationalActivity() { return false; } virtual bool controlPtz(PtzCommand command, bool stop, int speed) { (void)command; (void)stop; diff --git a/cmvr-es/devices/camera/hikvision_camera/include/hikvision_camera.h b/cmvr-es/devices/camera/hikvision_camera/include/hikvision_camera.h index 0fd6e27f..39adf98e 100644 --- a/cmvr-es/devices/camera/hikvision_camera/include/hikvision_camera.h +++ b/cmvr-es/devices/camera/hikvision_camera/include/hikvision_camera.h @@ -36,6 +36,7 @@ public: bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) override; bool startStreaming() override; void stopStreaming() override; + bool stopOperationalActivity() override; bool controlPtz(PtzCommand command, bool stop, int speed) override; bool executeJsonCommand(const std::string& request_json, std::string& response_json) override; bool requestKeyFrame() override; @@ -56,7 +57,7 @@ private: void releaseSdk_(); bool login_(); bool startPreview_(); - void stopPreview_(); + bool stopPreview_(); bool requestKeyFrame_(); void stopRecordingUnlocked_(); void fillIntrinsics_(Rs2Intrinsics& intrinsics) const; diff --git a/cmvr-es/devices/camera/hikvision_camera/src/hikvision_camera.cpp b/cmvr-es/devices/camera/hikvision_camera/src/hikvision_camera.cpp index e2d92821..265d7e62 100644 --- a/cmvr-es/devices/camera/hikvision_camera/src/hikvision_camera.cpp +++ b/cmvr-es/devices/camera/hikvision_camera/src/hikvision_camera.cpp @@ -323,7 +323,7 @@ bool HikvisionCamera::start() if (state_.is_opened) { return true; } - if (!login_()) { + if (user_id_ < 0 && !login_()) { return false; } if (!startPreview_()) { @@ -348,7 +348,7 @@ bool HikvisionCamera::stop() state_.is_streaming = false; stream_count_ = 0; resetStreamState_(); - stopPreview_(); + (void)stopPreview_(); if (user_id_ >= 0) { NET_DVR_Logout(user_id_); user_id_ = -1; @@ -544,6 +544,23 @@ void HikvisionCamera::stopStreaming() } } +bool HikvisionCamera::stopOperationalActivity() +{ + std::lock_guard lock(ctrl_mtx_); + clear_error_(); + + if (stream_count_ != 0 || state_.is_streaming || state_.is_recording) { + return false; + } + if (!stopPreview_()) { + setError_(sdkError_("NET_DVR_StopRealPlay")); + return false; + } + state_.is_opened = false; + return real_handle_ < 0 && !state_.is_streaming && + !state_.is_recording; +} + bool HikvisionCamera::controlPtz(PtzCommand command, bool stop, int speed) { std::lock_guard lock(ctrl_mtx_); @@ -917,7 +934,7 @@ bool HikvisionCamera::startPreview_() return true; } -void HikvisionCamera::stopPreview_() +bool HikvisionCamera::stopPreview_() { const int preview_handle = real_handle_; { @@ -930,9 +947,12 @@ void HikvisionCamera::stopPreview_() awaiting_key_frame_ = false; } if (preview_handle >= 0) { - NET_DVR_StopRealPlay(preview_handle); + if (!NET_DVR_StopRealPlay(preview_handle)) { + return false; + } real_handle_ = -1; } + return true; } void HikvisionCamera::fillIntrinsics_(Rs2Intrinsics& intrinsics) const diff --git a/cmvr-es/devices/camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp b/cmvr-es/devices/camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp index 1aa72441..fc18e9e5 100644 --- a/cmvr-es/devices/camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp +++ b/cmvr-es/devices/camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp @@ -269,11 +269,30 @@ bool testCallbackPublicationLifecycle() CHECK_TRUE(frame.sequence == 0); CHECK_TRUE(frame.codec_config_generation == 3); + camera.stopStreaming(); + CHECK_TRUE(camera.stopOperationalActivity()); + CHECK_TRUE(g_stop_callback_count.load() == 1); + cmvr::device::CameraState stopped_state{}; + camera.getState(stopped_state); + CHECK_TRUE(stopped_state.is_initialized); + CHECK_TRUE(!stopped_state.is_opened); + CHECK_TRUE(!stopped_state.is_streaming); + + // Operational stop keeps the SDK/login lifecycle reusable. start() only + // recreates the preview pipeline and can stream again without init(). + CHECK_TRUE(camera.startOperationalActivity()); + CHECK_TRUE(camera.startStreaming()); + emitIFrame(); + CHECK_TRUE(camera.waitEncodedFrame( + frame, cursor, std::chrono::milliseconds(50))); + CHECK_TRUE(frame.stream_epoch == 3); + camera.stopStreaming(); + // The fake StopRealPlay invokes the SDK callback synchronously. stop() // owns ctrl_mtx_ here, proving the callback neither takes that mutex nor // publishes after the preview handle has been invalidated. CHECK_TRUE(camera.stop()); - CHECK_TRUE(g_stop_callback_count.load() == 1); + CHECK_TRUE(g_stop_callback_count.load() == 2); CHECK_TRUE(!camera.waitEncodedFrame( frame, cursor, std::chrono::milliseconds(10))); return true; diff --git a/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h b/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h index 501b0da8..d8c0bfde 100644 --- a/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h +++ b/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h @@ -50,6 +50,8 @@ public: void getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) override; bool startStreaming() override; void stopStreaming() override; + bool startOperationalActivity() override; + bool stopOperationalActivity() override; bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) override; private: @@ -91,6 +93,8 @@ private: uint64_t last_frame_id_{0}; bool has_last_frame_id_{false}; size_t stream_frame_index_{0}; + std::size_t stream_count_{0}; + bool operational_active_{false}; bool streaming_{false}; std::shared_ptr rgb_encoder_; }; diff --git a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp index 416dc7bb..54afbdfe 100644 --- a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp +++ b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp @@ -150,6 +150,9 @@ bool MujocoCamera::start() bool MujocoCamera::stop() { std::lock_guard lock(mtx_); + operational_active_ = false; + streaming_ = false; + stream_count_ = 0U; state_.is_streaming = false; state_.is_opened = false; destroyOffscreen_(); @@ -213,6 +216,7 @@ bool MujocoCamera::startStreaming() return false; } std::lock_guard lock(mtx_); + ++stream_count_; streaming_ = true; state_.is_streaming = true; return true; @@ -221,10 +225,41 @@ bool MujocoCamera::startStreaming() void MujocoCamera::stopStreaming() { std::lock_guard lock(mtx_); + if (stream_count_ > 0U) { + --stream_count_; + } + if (stream_count_ == 0U) { + streaming_ = false; + state_.is_streaming = operational_active_; + stream_frame_index_ = 0; + rgb_encoder_.reset(); + } +} + +bool MujocoCamera::startOperationalActivity() +{ + if (!start()) { + return false; + } + std::lock_guard lock(mtx_); + operational_active_ = true; + state_.is_streaming = true; + return true; +} + +bool MujocoCamera::stopOperationalActivity() +{ + std::lock_guard lock(mtx_); + if (stream_count_ != 0U || state_.is_recording) { + return false; + } + operational_active_ = false; streaming_ = false; state_.is_streaming = false; + state_.is_opened = false; stream_frame_index_ = 0; rgb_encoder_.reset(); + return true; } bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) diff --git a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera_test.cpp b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera_test.cpp index 67a6c083..a3ab20be 100644 --- a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera_test.cpp +++ b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera_test.cpp @@ -53,4 +53,40 @@ TEST(MujocoCameraTest, CapturesOffscreenRgbdFrame) EXPECT_GT(intrinsics.fy, 0.0f); } +TEST(MujocoCameraTest, OperationalStopCanResumeWithoutReinitializing) +{ + std::uint64_t frame_id = 0; + cmvr::device::MujocoCamera camera( + [&frame_id](std::vector& rgb, + std::vector& depth, + int& width, + int& height, + std::uint64_t& returned_frame_id) { + width = 2; + height = 2; + rgb.assign(12U, 127U); + depth.assign(4U, 1.0F); + returned_frame_id = ++frame_id; + return true; + }); + + ASSERT_TRUE(camera.init()); + ASSERT_TRUE(camera.startOperationalActivity()); + ASSERT_TRUE(camera.startStreaming()); + EXPECT_FALSE(camera.stopOperationalActivity()); + camera.stopStreaming(); + EXPECT_TRUE(camera.stopOperationalActivity()); + + cmvr::device::CameraState state{}; + camera.getState(state); + EXPECT_TRUE(state.is_initialized); + EXPECT_FALSE(state.is_opened); + EXPECT_FALSE(state.is_streaming); + + ASSERT_TRUE(camera.startOperationalActivity()); + camera.getState(state); + EXPECT_TRUE(state.is_opened); + EXPECT_TRUE(state.is_streaming); +} + } // namespace diff --git a/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h b/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h index ec700d96..09bad9e8 100644 --- a/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h +++ b/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h @@ -5,6 +5,10 @@ #ifndef REALSENSE_CAMERA_H #define REALSENSE_CAMERA_H +#include +#include +#include + #include "camera/abstract_camera.h" #include "common/base/ring_buffer.h" #include "devices/camera/common/include/camera_stream_encoder.h" @@ -38,6 +42,7 @@ namespace cmvr::device{ bool startStreaming() override; void stopStreaming() override; + bool stopOperationalActivity() override; Eigen::Vector3f get3DPointFromPixel(int u, int v) override; @@ -45,6 +50,11 @@ namespace cmvr::device{ rs2::frameset get_frameset(bool align); void streaming_worker_(); void recording_worker_(); + bool collectStreamingWorker_(std::chrono::milliseconds timeout); + bool collectRecordingWorker_(std::chrono::milliseconds timeout); + void markStreamingWorkerStopped_() noexcept; + void markRecordingWorkerStopped_() noexcept; + void setWorkerError_(const std::string& message) noexcept; private: int fps_; int width_; @@ -79,6 +89,10 @@ namespace cmvr::device{ std::string current_video_path_; std::mutex ctrl_mtx_{}; + std::mutex stream_lifecycle_mtx_{}; + std::mutex stream_stop_mtx_{}; + std::condition_variable stream_stop_cv_{}; + std::condition_variable recording_stop_cv_{}; std::unique_ptr video_writer_; std::shared_ptr stream_thread_;//采集线程 std::shared_ptr encode_thread_;//采集线程 @@ -102,8 +116,12 @@ namespace cmvr::device{ size_t recordingIndex_ = 0; size_t getImageIndex_ = 0; - bool is_streaming_running = false; - bool is_recording_running = false; + std::atomic stream_requested_{false}; + std::atomic recording_requested_{false}; + std::atomic stream_worker_exited_{true}; + std::atomic recording_worker_exited_{true}; + std::atomic is_streaming_running{false}; + std::atomic is_recording_running{false}; int stream_count_ = 0; cv::Mat latest_depth_; diff --git a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp index 5ab33323..d3b49bda 100644 --- a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp +++ b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp @@ -8,6 +8,10 @@ using namespace std; using namespace cmvr::device; +namespace { +constexpr auto kStreamStopTimeout = std::chrono::seconds(2); +} + // 检查系统中是否存在指定序列号的 RealSense 设备 bool checkRealSenseCamera(const std::string& serialNumber = "") { @@ -307,19 +311,39 @@ bool RealsenseCamera::start() { bool RealsenseCamera::stop() { //先停止录制再关闭摄像头 - if (state_.is_recording) { + bool is_recording = false; + { + std::lock_guard lock(ctrl_mtx_); + is_recording = state_.is_recording; + } + if (is_recording) { try { stopRecording(); } catch (const std::exception& e) { CMVR_LOG(WARNING) << "[RealsenseCamera] (stop): stopRecording failed: " << e.what(); } } - std::lock_guard lock(ctrl_mtx_); - clear_error_(); - if (!state_.is_opened || !state_.is_initialized) { - state_.is_opened = false; - return true; + std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); + { + std::lock_guard lock(ctrl_mtx_); + clear_error_(); + if (!state_.is_opened || !state_.is_initialized) { + state_.is_opened = false; + return true; + } + state_.is_streaming = false; + stream_count_ = 0; + stream_requested_.store(false, std::memory_order_release); } + + if (!collectStreamingWorker_(kStreamStopTimeout)) { + std::lock_guard lock(ctrl_mtx_); + state_.is_error = true; + state_.error_message = "timed out waiting for camera stream to stop"; + return false; + } + + std::lock_guard lock(ctrl_mtx_); try { pipe_.stop(); } catch (const std::exception& e) { @@ -387,6 +411,8 @@ void RealsenseCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) { } void RealsenseCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) { + std::lock_guard control_lock(ctrl_mtx_); + clear_error_(); intrinsics.cx = intrinsics_.ppx; intrinsics.cy = intrinsics_.ppy; intrinsics.fx = intrinsics_.fx; @@ -433,6 +459,8 @@ void RealsenseCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) { } void RealsenseCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) { + std::lock_guard control_lock(ctrl_mtx_); + clear_error_(); intrinsics.cx = intrinsics_.ppx; intrinsics.cy = intrinsics_.ppy; intrinsics.fx = intrinsics_.fx; @@ -485,24 +513,49 @@ void RealsenseCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsic void RealsenseCamera::startRecording(const std::string &video_path) { - std::lock_guard lock(ctrl_mtx_); - clear_error_(); - if (mode_ != VIDEO_MODE) { - state_.is_error = true; - state_.error_message = "startRecording only supports VIDEO_MODE"; - CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message; + std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); + { + std::lock_guard lock(ctrl_mtx_); + clear_error_(); + if (mode_ != VIDEO_MODE) { + state_.is_error = true; + state_.error_message = "startRecording only supports VIDEO_MODE"; + return; + } + if (!state_.is_opened) { + state_.is_error = true; + state_.error_message = "camera not opened"; + return; + } + if (state_.is_recording || + recording_requested_.load(std::memory_order_acquire)) { + state_.is_error = true; + state_.error_message = "already recording"; + return; + } + } + + if (!collectRecordingWorker_(kStreamStopTimeout)) { + setWorkerError_("previous camera recording did not stop"); return; } - if (!state_.is_opened) { - state_.is_error = true; - state_.error_message = "camera not opened"; - CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message; + bool collect_stale_stream = false; + { + std::lock_guard lock(ctrl_mtx_); + collect_stale_stream = stream_thread_ && + stream_worker_exited_.load(std::memory_order_acquire); + } + if (collect_stale_stream && + !collectStreamingWorker_(kStreamStopTimeout)) { + setWorkerError_("previous camera stream did not stop"); return; } - if (state_.is_recording) { + + std::unique_lock lock(ctrl_mtx_); + if (!state_.is_opened || state_.is_recording) { state_.is_error = true; - state_.error_message = "already recording"; - CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message; + state_.error_message = state_.is_recording + ? "already recording" : "camera not opened"; return; } @@ -516,8 +569,11 @@ void RealsenseCamera::startRecording(const std::string &video_path) { stream_ = nullptr; format_context_ = nullptr; state_.is_recording = false; + recording_requested_.store(false, std::memory_order_release); + current_video_path_.clear(); }; + bool created_stream_worker = false; try { current_video_path_ = video_path; std::string temp_path = current_video_path_ + ".temp"; // 临时文件 @@ -604,63 +660,67 @@ void RealsenseCamera::startRecording(const std::string &video_path) { cleanup_recording_resources(); return; } - //不在录像也不在流传输，但是采集线程没有退出时。 - if (!state_.is_streaming && !state_.is_recording) { - if (stream_thread_) { - if (stream_thread_->joinable()) { - stream_thread_->join(); - is_streaming_running = false; - } - stream_thread_.reset(); - } - } // 开启录像 state_.is_recording = true; + recording_requested_.store(true, std::memory_order_release); //开启流采集线程 if (!stream_thread_) { + stream_worker_exited_.store(false, std::memory_order_release); stream_thread_ = make_shared(&RealsenseCamera::streaming_worker_, this); - //延时100ms，等待流线程获取图像 - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + created_stream_worker = true; } // 启动录像线程 frame_count_ = 0; - if (recording_thread_) { - if (recording_thread_->joinable()) { - recording_thread_->join(); - is_recording_running = false; - } - recording_thread_.reset(); - } + recording_worker_exited_.store(false, std::memory_order_release); recording_thread_ = make_shared(&RealsenseCamera::recording_worker_, this); } catch (const std::exception& e) { + recording_requested_.store(false, std::memory_order_release); + recording_worker_exited_.store(true, std::memory_order_release); + recording_stop_cv_.notify_all(); + if (!stream_thread_) { + stream_worker_exited_.store(true, std::memory_order_release); + stream_stop_cv_.notify_all(); + } cleanup_recording_resources(); state_.is_error = true; state_.error_message = "[RealsenseCamera] (startRecording): " + std::string(e.what()); CMVR_LOG(ERROR) << state_.error_message; + lock.unlock(); + if (created_stream_worker && + !collectStreamingWorker_(kStreamStopTimeout)) { + setWorkerError_( + "failed to collect camera stream after recording start failure"); + } } } void RealsenseCamera::stopRecording() { - std::lock_guard lock(ctrl_mtx_); - clear_error_(); - if (mode_ != VIDEO_MODE) { - state_.is_error = true; - state_.error_message = "stopRecording only supports VIDEO_MODE"; - CMVR_LOG(ERROR) << "[RealsenseCamera] (stopRecording): " << state_.error_message; - return; + std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); + bool has_recording_worker = false; + { + std::lock_guard lock(ctrl_mtx_); + clear_error_(); + if (mode_ != VIDEO_MODE) { + state_.is_error = true; + state_.error_message = "stopRecording only supports VIDEO_MODE"; + return; + } + has_recording_worker = static_cast(recording_thread_); + if (!state_.is_recording && !has_recording_worker) { + return; + } + recording_requested_.store(false, std::memory_order_release); } - if (!state_.is_recording) { - CMVR_LOG(WARNING) << "[RealsenseCamera] (stopRecording): not recording"; - return; + if (has_recording_worker && + !collectRecordingWorker_(kStreamStopTimeout)) { + setWorkerError_("timed out waiting for camera recording to stop"); + throw std::runtime_error( + "timed out waiting for camera recording to stop"); } - // 1. 停止录像线程 + std::unique_lock lock(ctrl_mtx_); state_.is_recording = false; - if (recording_thread_ && recording_thread_->joinable()) { - recording_thread_->join(); - recording_thread_.reset(); - } // 2. 清理FFmpeg资源 if (packet_) { @@ -682,14 +742,26 @@ void RealsenseCamera::stopRecording() { stream_ = nullptr; // 3. 重命名临时文件为目标文件 - std::string temp_path = current_video_path_ + ".temp"; - if (rename(temp_path.c_str(), current_video_path_.c_str()) != 0) { + const std::string completed_video_path = current_video_path_; + const std::string temp_path = completed_video_path + ".temp"; + if (!completed_video_path.empty() && + rename(temp_path.c_str(), completed_video_path.c_str()) != 0) { state_.is_error = true; - state_.error_message = "failed to rename temp file: " + temp_path + " -> " + current_video_path_; + state_.error_message = "failed to rename temp file: " + temp_path + + " -> " + completed_video_path; CMVR_LOG(ERROR) << "[RealsenseCamera] (stopRecording): " << state_.error_message; - return; } current_video_path_.clear(); + + const bool collect_stream = stream_count_ == 0 && + static_cast(stream_thread_); + lock.unlock(); + if (collect_stream && + !collectStreamingWorker_(kStreamStopTimeout)) { + setWorkerError_("timed out waiting for camera stream to stop"); + throw std::runtime_error( + "timed out waiting for camera stream to stop"); + } } void RealsenseCamera::pauseRecording() { @@ -718,10 +790,11 @@ void RealsenseCamera::streaming_worker_() { const int frame_interval = 1000 / fps_; bool success = false; - is_streaming_running = true; + is_streaming_running.store(true, std::memory_order_release); // 处于流传输或者录像状态时就不退出线程 - while (state_.is_streaming || state_.is_recording) { + while (stream_requested_.load(std::memory_order_acquire) || + recording_requested_.load(std::memory_order_acquire)) { // 记录当前帧处理开始时间 auto frame_start_time = std::chrono::high_resolution_clock::now(); @@ -730,15 +803,13 @@ void RealsenseCamera::streaming_worker_() { rs2::frame color_frame = frames.get_color_frame(); rs2::frame depth_frame = frames.get_depth_frame(); if (!color_frame) { - state_.is_error = true; - state_.error_message = "missing color frame"; - CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_: " << state_.error_message; + setWorkerError_("missing color frame"); + recording_requested_.store(false, std::memory_order_release); break; } if (stream_mode_ == RGBD_MODE && !depth_frame) { - state_.is_error = true; - state_.error_message = "missing depth frame in RGBD mode"; - CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_: " << state_.error_message; + setWorkerError_("missing depth frame in RGBD mode"); + recording_requested_.store(false, std::memory_order_release); break; } @@ -809,7 +880,7 @@ void RealsenseCamera::streaming_worker_() { } } - is_streaming_running = false; + markStreamingWorkerStopped_(); // 线程结束时清空队列 stream_frame_buffer_->clear(); recordingIndex_ = 0; @@ -820,20 +891,21 @@ void RealsenseCamera::streaming_worker_() { // 线程结束时清空队列 stream_frame_buffer_->clear(); // 确保线程状态正确更新 - is_streaming_running = false; - state_.is_error = true; - state_.error_message = e.what(); - CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_ error:" << state_.error_message; + recording_requested_.store(false, std::memory_order_release); + setWorkerError_(e.what()); + markStreamingWorkerStopped_(); + CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_ error:" + << e.what(); } } void RealsenseCamera::recording_worker_() { - is_recording_running = true; + is_recording_running.store(true, std::memory_order_release); const int frame_interval = 1000 / fps_; bool is_first_key = false; try { //保证当前采集线程正常运行 - while (state_.is_recording && is_streaming_running) { + while (recording_requested_.load(std::memory_order_acquire)) { // 等待缓冲区有数据 if (stream_frame_buffer_->empty()) { std::this_thread::sleep_for(std::chrono::milliseconds(frame_interval)); @@ -898,11 +970,12 @@ void RealsenseCamera::recording_worker_() { av_write_trailer(format_context_); } catch (const std::exception& e) { + setWorkerError_(e.what()); CMVR_LOG(ERROR) << "Recording thread error: " << e.what(); } - is_recording_running = false; - state_.is_recording = false; + recording_requested_.store(false, std::memory_order_release); + markRecordingWorkerStopped_(); } void RealsenseCamera::getEncodedFrame(StreamFrameData& frame_data, size_t& index) { @@ -936,37 +1009,222 @@ bool RealsenseCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& bool RealsenseCamera::startStreaming() { - std::lock_guard lock(ctrl_mtx_); - //不在录像也不在流传输，但是采集线程没有退出时。 - if (!state_.is_streaming && !state_.is_recording) { - if (stream_thread_) { - if (stream_thread_->joinable()) { - stream_thread_->join(); - is_streaming_running = false; + std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); + + { + std::lock_guard lock(ctrl_mtx_); + clear_error_(); + if (!state_.is_opened) { + state_.is_error = true; + state_.error_message = "camera not opened"; + return false; + } + if (stream_count_ > 0) { + if (!stream_thread_ || + stream_worker_exited_.load(std::memory_order_acquire)) { + state_.is_error = true; + state_.error_message = "camera stream worker exited"; + return false; } - stream_thread_.reset(); + ++stream_count_; + state_.is_streaming = true; + stream_requested_.store(true, std::memory_order_release); + return true; + } + if (stream_thread_ && + !stream_worker_exited_.load(std::memory_order_acquire)) { + ++stream_count_; + state_.is_streaming = true; + stream_requested_.store(true, std::memory_order_release); + return true; } } - //开启流采集线程 - if (!stream_thread_) { - state_.is_streaming = true; - stream_thread_ = make_shared(&RealsenseCamera::streaming_worker_, this); - //延时100ms，等待流线程获取图像 - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + if (!collectStreamingWorker_(kStreamStopTimeout)) { + setWorkerError_("previous camera stream did not stop"); + return false; } - stream_count_++; + + std::lock_guard lock(ctrl_mtx_); + if (!state_.is_opened) { + state_.is_error = true; + state_.error_message = "camera not opened"; + return false; + } + state_.is_streaming = true; + stream_requested_.store(true, std::memory_order_release); + stream_worker_exited_.store(false, std::memory_order_release); + try { + stream_thread_ = make_shared(&RealsenseCamera::streaming_worker_, this); + } catch (const std::exception& error) { + stream_requested_.store(false, std::memory_order_release); + stream_worker_exited_.store(true, std::memory_order_release); + stream_stop_cv_.notify_all(); + state_.is_streaming = false; + state_.is_error = true; + state_.error_message = + std::string("failed to start camera stream worker: ") + + error.what(); + return false; + } + stream_count_ = 1; return true; } void RealsenseCamera::stopStreaming() { - std::lock_guard lock(ctrl_mtx_); - stream_count_--; - if (stream_count_ == 0) + std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); { - // 当前已经没有正在使用的流了，编码采集线程状态修改 + std::lock_guard lock(ctrl_mtx_); + if (stream_count_ == 0) { + return; + } + --stream_count_; + if (stream_count_ != 0) { + return; + } state_.is_streaming = false; + stream_requested_.store(false, std::memory_order_release); + if (recording_requested_.load(std::memory_order_acquire)) { + return; + } + } + if (!collectStreamingWorker_(kStreamStopTimeout)) { + setWorkerError_("timed out waiting for camera stream to stop"); + throw std::runtime_error("timed out waiting for camera stream to stop"); + } +} + +bool RealsenseCamera::stopOperationalActivity() +{ + std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); + { + std::lock_guard lock(ctrl_mtx_); + clear_error_(); + + if (stream_count_ != 0 || state_.is_streaming || + state_.is_recording || + recording_requested_.load(std::memory_order_acquire)) { + return false; + } + stream_requested_.store(false, std::memory_order_release); + recording_requested_.store(false, std::memory_order_release); + } + + if (!collectRecordingWorker_(kStreamStopTimeout)) { + setWorkerError_("timed out waiting for camera recording to stop"); + return false; + } + if (!collectStreamingWorker_(kStreamStopTimeout)) { + setWorkerError_("timed out waiting for camera stream to stop"); + return false; + } + + std::lock_guard lock(ctrl_mtx_); + if (state_.is_opened) { + try { + pipe_.stop(); + } catch (const std::exception& error) { + state_.is_error = true; + state_.error_message = + std::string("failed to stop operational pipeline: ") + + error.what(); + return false; + } + } + align_.reset(); + pipe_ = rs2::pipeline(); + state_.is_opened = false; + return !state_.is_streaming && !state_.is_recording; +} + +bool RealsenseCamera::collectStreamingWorker_( + const std::chrono::milliseconds timeout) +{ + std::shared_ptr worker; + { + std::lock_guard lock(ctrl_mtx_); + worker = stream_thread_; + } + if (!worker) { + return true; + } + + { + std::unique_lock lock(stream_stop_mtx_); + if (!stream_stop_cv_.wait_for(lock, timeout, [this] { + return stream_worker_exited_.load(std::memory_order_acquire); + })) { + return false; + } + } + if (worker->joinable()) { + worker->join(); + } + std::lock_guard lock(ctrl_mtx_); + if (stream_thread_ == worker) { + stream_thread_.reset(); + } + return true; +} + +void RealsenseCamera::markStreamingWorkerStopped_() noexcept +{ + is_streaming_running.store(false, std::memory_order_release); + stream_worker_exited_.store(true, std::memory_order_release); + stream_stop_cv_.notify_all(); +} + +bool RealsenseCamera::collectRecordingWorker_( + const std::chrono::milliseconds timeout) +{ + std::shared_ptr worker; + { + std::lock_guard lock(ctrl_mtx_); + worker = recording_thread_; + } + if (!worker) { + return true; + } + + { + std::unique_lock lock(stream_stop_mtx_); + if (!recording_stop_cv_.wait_for(lock, timeout, [this] { + return recording_worker_exited_.load( + std::memory_order_acquire); + })) { + return false; + } + } + if (worker->joinable()) { + worker->join(); + } + std::lock_guard lock(ctrl_mtx_); + if (recording_thread_ == worker) { + recording_thread_.reset(); + } + return true; +} + +void RealsenseCamera::markRecordingWorkerStopped_() noexcept +{ + { + std::lock_guard lock(ctrl_mtx_); + state_.is_recording = false; + } + is_recording_running.store(false, std::memory_order_release); + recording_worker_exited_.store(true, std::memory_order_release); + recording_stop_cv_.notify_all(); +} + +void RealsenseCamera::setWorkerError_(const std::string& message) noexcept +{ + try { + std::lock_guard lock(ctrl_mtx_); + state_.is_error = true; + state_.error_message = message; + } catch (...) { + // Error reporting from a worker must never terminate the process. } } diff --git a/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h b/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h index 4560cd10..e8519828 100644 --- a/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h +++ b/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h @@ -5,6 +5,10 @@ #ifndef CMVR_ES_UVC_CAMERA_H #define CMVR_ES_UVC_CAMERA_H +#include +#include +#include + #include "common/base/ring_buffer.h" #include "camera/abstract_camera.h" #include "devices/camera/common/include/camera_stream_encoder.h" @@ -42,9 +46,15 @@ namespace cmvr::device { bool startStreaming() override; void stopStreaming() override; + bool stopOperationalActivity() override; private: void streaming_worker_(); void recording_worker_(); + bool collectStreamingWorker_(std::chrono::milliseconds timeout); + bool collectRecordingWorker_(std::chrono::milliseconds timeout); + void markStreamingWorkerStopped_() noexcept; + void markRecordingWorkerStopped_() noexcept; + void setWorkerError_(const std::string& message) noexcept; int fps_; int width_; @@ -63,6 +73,10 @@ namespace cmvr::device { std::string current_video_path_; std::mutex ctrl_mtx_{}; + std::mutex stream_lifecycle_mtx_{}; + std::mutex stream_stop_mtx_{}; + std::condition_variable stream_stop_cv_{}; + std::condition_variable recording_stop_cv_{}; std::unique_ptr video_writer_; std::shared_ptr stream_thread_; std::shared_ptr recording_thread_; @@ -88,8 +102,12 @@ namespace cmvr::device { size_t recordingIndex_ = 0; size_t getImageIndex_ = 0; - bool is_streaming_running = false; - bool is_recording_running = false; + std::atomic stream_requested_{false}; + std::atomic recording_requested_{false}; + std::atomic stream_worker_exited_{true}; + std::atomic recording_worker_exited_{true}; + std::atomic is_streaming_running{false}; + std::atomic is_recording_running{false}; int stream_count_ = 0; config::UVCCameraConfig camera_; diff --git a/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp b/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp index 5aa7018c..3eb9465a 100644 --- a/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp +++ b/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp @@ -10,6 +10,10 @@ using namespace cmvr::device; #define USE_LIST_IMAGE 1 +namespace { +constexpr auto kStreamStopTimeout = std::chrono::seconds(2); +} + UVCCamera::UVCCamera(const config::UVCCameraConfig& camera):camera_(camera) { id_ = camera_.id(); @@ -174,9 +178,17 @@ bool UVCCamera::start() { bool UVCCamera::stop() { //先停止录制再关闭摄像头 - if (state_.is_recording) { + bool is_recording = false; + { + std::lock_guard lock(ctrl_mtx_); + is_recording = state_.is_recording; + } + if (is_recording) { stopRecording(); } + std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); + bool collect_stream = false; + { std::lock_guard lock(ctrl_mtx_); clear_error_(); try { @@ -186,18 +198,10 @@ bool UVCCamera::stop() { } if (mode_ == VIDEO_MODE){ state_.is_streaming = false; - if (stream_thread_->joinable()) { - stream_thread_->join(); - stream_thread_.reset(); - stream_thread_ = nullptr; - } + stream_count_ = 0; + stream_requested_.store(false, std::memory_order_release); + collect_stream = static_cast(stream_thread_); } - - if (cap_.isOpened()) { - cap_.release(); - } - state_.is_opened = false; - return true; } catch (exception &e) { CMVR_LOG(ERROR) << "[UVCCamera] (stop): " << e.what(); @@ -205,6 +209,20 @@ bool UVCCamera::stop() { state_.error_message = e.what(); return false; } + } + + if (collect_stream && !collectStreamingWorker_(kStreamStopTimeout)) { + std::lock_guard lock(ctrl_mtx_); + state_.is_error = true; + state_.error_message = "timed out waiting for camera stream to stop"; + return false; + } + std::lock_guard lock(ctrl_mtx_); + if (cap_.isOpened()) { + cap_.release(); + } + state_.is_opened = false; + return true; } void UVCCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) @@ -240,6 +258,7 @@ void UVCCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) } void UVCCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) { + std::lock_guard lock(ctrl_mtx_); state_.is_error = true; state_.error_message = "getDepthImage unsupported usage"; CMVR_LOG(ERROR) << "[UVCCamera] (getDepthImage): " << state_.error_message; @@ -247,6 +266,7 @@ void UVCCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) { } void UVCCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) { + std::lock_guard lock(ctrl_mtx_); state_.is_error = true; state_.error_message = "getRGBDImages unsupported usage"; CMVR_LOG(ERROR) << "[UVCCamera] (getRGBDImages): " << state_.error_message; @@ -255,24 +275,52 @@ void UVCCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& int } void UVCCamera::startRecording(const std::string &video_path) { - std::lock_guard lock(ctrl_mtx_); - clear_error_(); - if (mode_ != VIDEO_MODE) { - state_.is_error = true; - state_.error_message = "startRecording only supports VIDEO_MODE"; - CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); + { + std::lock_guard lock(ctrl_mtx_); + clear_error_(); + if (mode_ != VIDEO_MODE) { + state_.is_error = true; + state_.error_message = "startRecording only supports VIDEO_MODE"; + CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + return; + } + if (!state_.is_opened) { + state_.is_error = true; + state_.error_message = "camera not opened"; + CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + return; + } + if (state_.is_recording || + recording_requested_.load(std::memory_order_acquire)) { + state_.is_error = true; + state_.error_message = "already recording"; + CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + return; + } + } + + if (!collectRecordingWorker_(kStreamStopTimeout)) { + setWorkerError_("previous camera recording did not stop"); return; } - if (!state_.is_opened) { - state_.is_error = true; - state_.error_message = "camera not opened"; - CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + bool collect_stale_stream = false; + { + std::lock_guard lock(ctrl_mtx_); + collect_stale_stream = stream_thread_ && + stream_worker_exited_.load(std::memory_order_acquire); + } + if (collect_stale_stream && + !collectStreamingWorker_(kStreamStopTimeout)) { + setWorkerError_("previous camera stream did not stop"); return; } - if (state_.is_recording) { + + std::unique_lock lock(ctrl_mtx_); + if (!state_.is_opened || state_.is_recording) { state_.is_error = true; - state_.error_message = "already recording"; - CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + state_.error_message = state_.is_recording + ? "already recording" : "camera not opened"; return; } @@ -286,8 +334,11 @@ void UVCCamera::startRecording(const std::string &video_path) { stream_ = nullptr; format_context_ = nullptr; state_.is_recording = false; + recording_requested_.store(false, std::memory_order_release); + current_video_path_.clear(); }; + bool created_stream_worker = false; try { current_video_path_ = video_path; std::string temp_path = current_video_path_ + ".temp"; // 临时文件 @@ -374,64 +425,73 @@ void UVCCamera::startRecording(const std::string &video_path) { cleanup_recording_resources(); return; } - //不在录像也不在流传输，但是采集线程没有退出时。 - if (!state_.is_streaming && !state_.is_recording) { - if (stream_thread_) { - if (stream_thread_->joinable()) { - stream_thread_->join(); - is_streaming_running = false; - } - stream_thread_.reset(); - } - } // 开启录像 state_.is_recording = true; + recording_requested_.store(true, std::memory_order_release); //开启流采集线程 if (!stream_thread_) { + stream_worker_exited_.store(false, std::memory_order_release); stream_thread_ = make_shared(&UVCCamera::streaming_worker_, this); - //延时100ms，等待流线程获取图像 - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + created_stream_worker = true; } // 启动录像线程 - if (recording_thread_) { - if (recording_thread_->joinable()) { - recording_thread_->join(); - is_recording_running = false; - } - recording_thread_.reset(); - } frame_count_ = 0; + recording_worker_exited_.store(false, std::memory_order_release); recording_thread_ = make_shared(&UVCCamera::recording_worker_, this); } catch (const std::exception& e) { + recording_requested_.store(false, std::memory_order_release); + recording_worker_exited_.store(true, std::memory_order_release); + recording_stop_cv_.notify_all(); + if (!stream_thread_) { + stream_worker_exited_.store(true, std::memory_order_release); + stream_stop_cv_.notify_all(); + } cleanup_recording_resources(); state_.is_error = true; state_.error_message = "[UVCCamera] (startRecording): " + std::string(e.what()); CMVR_LOG(ERROR) << state_.error_message; + const bool collect_created_stream = created_stream_worker && + !stream_requested_.load(std::memory_order_acquire); + lock.unlock(); + if (collect_created_stream && + !collectStreamingWorker_(kStreamStopTimeout)) { + setWorkerError_( + "failed to collect camera stream after recording start failure"); + } } } void UVCCamera::stopRecording() { - std::lock_guard lock(ctrl_mtx_); - clear_error_(); - if (mode_ != VIDEO_MODE) { - state_.is_error = true; - state_.error_message = "stopRecording only supports VIDEO_MODE"; - CMVR_LOG(ERROR) << "[UVCCamera] (stopRecording): " << state_.error_message; - return; - } - if (!state_.is_recording) { - CMVR_LOG(WARNING) << "[UVCCamera] (stopRecording): not recording"; - return; + std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); + bool has_recording_worker = false; + { + std::lock_guard lock(ctrl_mtx_); + clear_error_(); + if (mode_ != VIDEO_MODE) { + state_.is_error = true; + state_.error_message = "stopRecording only supports VIDEO_MODE"; + CMVR_LOG(ERROR) << "[UVCCamera] (stopRecording): " << state_.error_message; + return; + } + has_recording_worker = static_cast(recording_thread_); + if (!state_.is_recording && !has_recording_worker) { + CMVR_LOG(WARNING) << "[UVCCamera] (stopRecording): not recording"; + return; + } + recording_requested_.store(false, std::memory_order_release); } - // 1. 停止录像线程 - state_.is_recording = false; - if (recording_thread_ && recording_thread_->joinable()) { - recording_thread_->join(); - recording_thread_.reset(); + if (has_recording_worker && + !collectRecordingWorker_(kStreamStopTimeout)) { + setWorkerError_("timed out waiting for camera recording to stop"); + throw std::runtime_error( + "timed out waiting for camera recording to stop"); } + std::unique_lock lock(ctrl_mtx_); + state_.is_recording = false; + // 2. 清理FFmpeg资源 if (packet_) { av_packet_free(&packet_); @@ -451,15 +511,29 @@ void UVCCamera::stopRecording() { } stream_ = nullptr; - // 3. 重命名临时文件为目标文件 - std::string temp_path = current_video_path_ + ".temp"; - if (rename(temp_path.c_str(), current_video_path_.c_str()) != 0) { + // 3. 重命名临时文件为目标文件。即使重命名失败，也必须继续 + // 回收仅由录像使用的采集线程。 + const std::string completed_video_path = current_video_path_; + const std::string temp_path = completed_video_path + ".temp"; + const bool rename_failed = !completed_video_path.empty() && + rename(temp_path.c_str(), completed_video_path.c_str()) != 0; + if (rename_failed) { state_.is_error = true; - state_.error_message = "failed to rename temp file: " + temp_path + " -> " + current_video_path_; + state_.error_message = "failed to rename temp file: " + temp_path + + " -> " + completed_video_path; CMVR_LOG(ERROR) << "[UVCCamera] (stopRecording): " << state_.error_message; - return; } current_video_path_.clear(); + + const bool collect_stream = stream_count_ == 0 && + static_cast(stream_thread_); + lock.unlock(); + if (collect_stream && + !collectStreamingWorker_(kStreamStopTimeout)) { + setWorkerError_("timed out waiting for camera stream to stop"); + throw std::runtime_error( + "timed out waiting for camera stream to stop"); + } } void UVCCamera::pauseRecording() { @@ -477,17 +551,19 @@ void UVCCamera::streaming_worker_() { const int frame_interval = 1000 / fps_; bool success = false; - is_streaming_running = true; + is_streaming_running.store(true, std::memory_order_release); cv::Mat frame; int64_t frame_count = 0; // 处于流传输或者录像状态时就不退出线程 - while (state_.is_streaming || state_.is_recording) { + while (stream_requested_.load(std::memory_order_acquire) || + recording_requested_.load(std::memory_order_acquire)) { // 记录当前帧处理开始时间 auto frame_start_time = std::chrono::high_resolution_clock::now(); if (!cap_.read(frame) || frame.empty()) { - state_.is_error = true; - state_.error_message = "failed to read frame"; - CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_: " << state_.error_message; + setWorkerError_("failed to read frame"); + recording_requested_.store(false, std::memory_order_release); + CMVR_LOG(ERROR) << + "[UVCCamera]streaming_worker_: failed to read frame"; break; } @@ -540,7 +616,7 @@ void UVCCamera::streaming_worker_() { } } - is_streaming_running = false; + markStreamingWorkerStopped_(); // 线程结束时清空队列 stream_frame_buffer_->clear(); recordingIndex_ = 0; @@ -551,20 +627,20 @@ void UVCCamera::streaming_worker_() { // 线程结束时清空队列 stream_frame_buffer_->clear(); // 确保线程状态正确更新 - is_streaming_running = false; - state_.is_error = true; - state_.error_message = e.what(); - CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_ error:" << state_.error_message; + recording_requested_.store(false, std::memory_order_release); + setWorkerError_(e.what()); + markStreamingWorkerStopped_(); + CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_ error:" << e.what(); } } void UVCCamera::recording_worker_() { - is_recording_running = true; + is_recording_running.store(true, std::memory_order_release); const int frame_interval = 1000 / fps_; bool is_first_key = false; try { //保证当前采集线程正常运行 - while (state_.is_recording && is_streaming_running) { + while (recording_requested_.load(std::memory_order_acquire)) { // 等待缓冲区有数据 if (stream_frame_buffer_->empty()) { std::this_thread::sleep_for(std::chrono::milliseconds(frame_interval)); @@ -629,11 +705,12 @@ void UVCCamera::recording_worker_() { av_write_trailer(format_context_); } catch (const std::exception& e) { + setWorkerError_(e.what()); CMVR_LOG(ERROR) << "录像线程错误: " << e.what(); } - is_recording_running = false; - state_.is_recording = false; + recording_requested_.store(false, std::memory_order_release); + markRecordingWorkerStopped_(); } void UVCCamera::getEncodedFrame(StreamFrameData& frame_data, size_t& index) { @@ -667,36 +744,215 @@ bool UVCCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_ bool UVCCamera::startStreaming() { - std::lock_guard lock(ctrl_mtx_); - //不在录像也不在流传输，但是采集线程没有退出时。 - if (!state_.is_streaming && !state_.is_recording) { - if (stream_thread_) { - if (stream_thread_->joinable()) { - stream_thread_->join(); - is_streaming_running = false; + std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); + + { + std::lock_guard lock(ctrl_mtx_); + clear_error_(); + if (!state_.is_opened) { + state_.is_error = true; + state_.error_message = "camera not opened"; + return false; + } + + // A running worker is shared by all streaming leases and recording. + // Adding another lease must not wait for that worker to exit. + if (stream_count_ > 0) { + if (!stream_thread_ || + stream_worker_exited_.load(std::memory_order_acquire)) { + state_.is_error = true; + state_.error_message = "camera stream worker exited"; + return false; } - stream_thread_.reset(); + ++stream_count_; + state_.is_streaming = true; + stream_requested_.store(true, std::memory_order_release); + return true; + } + if (stream_thread_ && + !stream_worker_exited_.load(std::memory_order_acquire)) { + ++stream_count_; + state_.is_streaming = true; + stream_requested_.store(true, std::memory_order_release); + return true; } } - //开启流采集线程 - if (!stream_thread_) { - state_.is_streaming = true; - stream_thread_ = make_shared(&UVCCamera::streaming_worker_, this); - //延时100ms，等待流线程获取图像 - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + if (!collectStreamingWorker_(kStreamStopTimeout)) { + setWorkerError_("previous camera stream did not stop"); + return false; } - stream_count_++; + + std::lock_guard lock(ctrl_mtx_); + if (!state_.is_opened) { + state_.is_error = true; + state_.error_message = "camera not opened"; + return false; + } + state_.is_streaming = true; + stream_requested_.store(true, std::memory_order_release); + stream_worker_exited_.store(false, std::memory_order_release); + try { + stream_thread_ = make_shared(&UVCCamera::streaming_worker_, this); + } catch (const std::exception& error) { + stream_requested_.store(false, std::memory_order_release); + stream_worker_exited_.store(true, std::memory_order_release); + stream_stop_cv_.notify_all(); + state_.is_streaming = false; + state_.is_error = true; + state_.error_message = + std::string("failed to start camera stream worker: ") + + error.what(); + return false; + } + stream_count_ = 1; return true; } void UVCCamera::stopStreaming() { - std::lock_guard lock(ctrl_mtx_); - stream_count_--; - if (stream_count_ == 0) + std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); { - // 当前已经没有正在使用的流了，编码采集线程状态修改 + std::lock_guard lock(ctrl_mtx_); + if (stream_count_ == 0) { + return; + } + --stream_count_; + if (stream_count_ != 0) { + return; + } state_.is_streaming = false; + stream_requested_.store(false, std::memory_order_release); + if (recording_requested_.load(std::memory_order_acquire)) { + return; + } + } + if (!collectStreamingWorker_(kStreamStopTimeout)) { + setWorkerError_("timed out waiting for camera stream to stop"); + throw std::runtime_error("timed out waiting for camera stream to stop"); + } +} + +bool UVCCamera::stopOperationalActivity() +{ + std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); + { + std::lock_guard lock(ctrl_mtx_); + clear_error_(); + + if (stream_count_ != 0 || state_.is_streaming || + state_.is_recording || + recording_requested_.load(std::memory_order_acquire)) { + return false; + } + stream_requested_.store(false, std::memory_order_release); + recording_requested_.store(false, std::memory_order_release); + } + + if (!collectRecordingWorker_(kStreamStopTimeout)) { + setWorkerError_("timed out waiting for camera recording to stop"); + return false; + } + if (!collectStreamingWorker_(kStreamStopTimeout)) { + setWorkerError_("timed out waiting for camera stream to stop"); + return false; + } + + std::lock_guard lock(ctrl_mtx_); + if (cap_.isOpened()) { + cap_.release(); + } + state_.is_opened = false; + return !cap_.isOpened() && !state_.is_streaming && + !state_.is_recording; +} + +bool UVCCamera::collectStreamingWorker_( + const std::chrono::milliseconds timeout) +{ + std::shared_ptr worker; + { + std::lock_guard lock(ctrl_mtx_); + worker = stream_thread_; + } + if (!worker) { + return true; + } + + { + std::unique_lock lock(stream_stop_mtx_); + if (!stream_stop_cv_.wait_for(lock, timeout, [this] { + return stream_worker_exited_.load(std::memory_order_acquire); + })) { + return false; + } + } + if (worker->joinable()) { + worker->join(); + } + std::lock_guard lock(ctrl_mtx_); + if (stream_thread_ == worker) { + stream_thread_.reset(); + } + return true; +} + +void UVCCamera::markStreamingWorkerStopped_() noexcept +{ + is_streaming_running.store(false, std::memory_order_release); + stream_worker_exited_.store(true, std::memory_order_release); + stream_stop_cv_.notify_all(); +} + +bool UVCCamera::collectRecordingWorker_( + const std::chrono::milliseconds timeout) +{ + std::shared_ptr worker; + { + std::lock_guard lock(ctrl_mtx_); + worker = recording_thread_; + } + if (!worker) { + return true; + } + + { + std::unique_lock lock(stream_stop_mtx_); + if (!recording_stop_cv_.wait_for(lock, timeout, [this] { + return recording_worker_exited_.load( + std::memory_order_acquire); + })) { + return false; + } + } + if (worker->joinable()) { + worker->join(); + } + std::lock_guard lock(ctrl_mtx_); + if (recording_thread_ == worker) { + recording_thread_.reset(); + } + return true; +} + +void UVCCamera::markRecordingWorkerStopped_() noexcept +{ + { + std::lock_guard lock(ctrl_mtx_); + state_.is_recording = false; + } + is_recording_running.store(false, std::memory_order_release); + recording_worker_exited_.store(true, std::memory_order_release); + recording_stop_cv_.notify_all(); +} + +void UVCCamera::setWorkerError_(const std::string& message) noexcept +{ + try { + std::lock_guard lock(ctrl_mtx_); + state_.is_error = true; + state_.error_message = message; + } catch (...) { + // Error reporting from a worker must never terminate the process. } } diff --git a/cmvr-es/devices/dexhand/abstract_dexhand.h b/cmvr-es/devices/dexhand/abstract_dexhand.h index 9cdb53fb..a68048e8 100644 --- a/cmvr-es/devices/dexhand/abstract_dexhand.h +++ b/cmvr-es/devices/dexhand/abstract_dexhand.h @@ -156,6 +156,17 @@ namespace cmvr::device { lifecycle == Status::STREAMING; } + // Stops command-driven activity without changing the device lifecycle + // or closing its transport. Implementations must return true only after + // no pre-stop activity can continue. Motion-capable hands without a + // reliable hold/idle command deliberately fail closed. + virtual bool stopOperationalActivity() { return false; } + + // Restores an activity paused by stopOperationalActivity(). This is + // called only after a new command has crossed the system admission + // boundary. Most motion-capable hands need no separate resume command. + virtual bool resumeOperationalActivity() { return true; } + virtual void setAngles(const std::vector& finger_joint_angles) = 0; virtual void setTactilePollingRegion(FingerType finger, TactileRegion region) { setTactilePollingRegions({TactileRegionKey{finger, region}}); diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h b/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h index 163f12c8..91cb479f 100644 --- a/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h @@ -49,6 +49,8 @@ namespace cmvr::device { Status state() const override; std::string lastError() const override; void getState(DexHandState& state) override; + bool stopOperationalActivity() override; + bool resumeOperationalActivity() override; void setAngles(const std::vector& finger_joint_angles) override; void setTactilePollingRegions(const std::vector& regions) override; @@ -122,6 +124,7 @@ namespace cmvr::device { mutable std::mutex polling_mutex_; std::condition_variable polling_cv_; bool requested_polling_{true}; + bool polling_paused_for_stop_all_{false}; std::thread polling_thread_; std::atomic polling_thread_running_{false}; std::chrono::milliseconds poll_interval_{10}; diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp index d15d4cf6..0d4724d2 100644 --- a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp @@ -450,6 +450,28 @@ void PX6AXGen3::getState(DexHandState& state_out) { state_out = std::move(next_state); } +bool PX6AXGen3::stopOperationalActivity() { + { + std::lock_guard lock(polling_mutex_); + polling_paused_for_stop_all_ = true; + } + polling_cv_.notify_all(); + + // The refresh mutex is the bounded device-I/O dispatch boundary. Once it is + // acquired, a pre-stop sensor transaction cannot still be using the wire. + std::lock_guard refresh_lock(refresh_mutex_); + return true; +} + +bool PX6AXGen3::resumeOperationalActivity() { + { + std::lock_guard lock(polling_mutex_); + polling_paused_for_stop_all_ = false; + } + polling_cv_.notify_all(); + return true; +} + void PX6AXGen3::setAngles(const std::vector&) { CMVR_LOG(ERROR) << "PX6AXGen3 is a tactile sensor only and does not support setAngles."; } @@ -589,6 +611,12 @@ void PX6AXGen3::refreshSensorData(const bool read_distributed, const bool read_r } std::lock_guard refresh_lock(refresh_mutex_); + { + std::lock_guard polling_lock(polling_mutex_); + if (polling_paused_for_stop_all_) { + return; + } + } const bool had_valid_snapshot = isSnapshotReady(read_distributed, read_resultant); try { @@ -722,9 +750,10 @@ void PX6AXGen3::pollingLoop() { auto next_poll_deadline = std::chrono::steady_clock::now(); std::unique_lock lock(polling_mutex_); while (polling_thread_running_.load(std::memory_order_acquire)) { - if (!requested_polling_) { + if (!requested_polling_ || polling_paused_for_stop_all_) { polling_cv_.wait(lock, [this]() { - return !polling_thread_running_.load(std::memory_order_acquire) || requested_polling_; + return !polling_thread_running_.load(std::memory_order_acquire) || + (requested_polling_ && !polling_paused_for_stop_all_); }); next_poll_deadline = std::chrono::steady_clock::now(); continue; @@ -746,7 +775,8 @@ void PX6AXGen3::pollingLoop() { } polling_cv_.wait_until(lock, next_poll_deadline, [this]() { - return !polling_thread_running_.load(std::memory_order_acquire); + return !polling_thread_running_.load(std::memory_order_acquire) || + polling_paused_for_stop_all_; }); } } @@ -758,6 +788,15 @@ void PX6AXGen3::ensureSensorReady(const bool allow_background, const bool background_covers_request = (!require_tactile || polls_tactile) && (!require_resultant || polls_resultant); + bool polling_paused = false; + { + std::lock_guard lock(polling_mutex_); + polling_paused = polling_paused_for_stop_all_; + } + if (polling_paused) { + return; + } + const bool background_ready = allow_background && background_covers_request && polling_thread_running_.load(std::memory_order_acquire) && diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt b/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt index 1e522d43..0c340a57 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt @@ -7,3 +7,30 @@ add_library(cmvr_es::device::rh56dftp_dexhand ALIAS rh56dftp_dexhand) target_link_libraries(rh56dftp_dexhand PRIVATE cmvr_es::hardware cmvr_es::proto -lmodbus) install(TARGETS rh56dftp_dexhand LIBRARY DESTINATION lib) + +if(BUILD_TESTING) + add_executable(rh56dftp_dexhand_stop_all_test + tests/rh56dftp_dexhand_stop_all_test.cpp + ) + target_link_libraries(rh56dftp_dexhand_stop_all_test PRIVATE + cmvr_es::device::rh56dftp_dexhand + gtest + gtest_main + pthread + ) + add_test( + NAME rh56dftp_dexhand_stop_all_test + COMMAND rh56dftp_dexhand_stop_all_test + ) + set(_rh56_stop_all_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _rh56_stop_all_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(rh56dftp_dexhand_stop_all_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_rh56_stop_all_test_environment}" + ) +endif() diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h index 388790f8..6e19729b 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h @@ -28,14 +28,17 @@ namespace cmvr::device { class ModbusController { public: ModbusController() = default; - ~ModbusController(); + virtual ~ModbusController(); - bool open(const std::string& ip, int port); - void close(); - bool isOpen() const; + virtual bool open(const std::string& ip, int port); + virtual void close(); + virtual bool isOpen() const; - bool writeRegisters(int address, const uint16_t* values, int count); - bool readRegisterBlock(int start_address, int count, std::vector& values); + virtual bool writeRegisters(int address, const uint16_t* values, int count); + virtual bool readRegisterBlock( + int start_address, + int count, + std::vector& values); private: void closeUnlocked(); @@ -58,6 +61,9 @@ namespace cmvr::device { using RegionMask = std::bitset; explicit RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& cfg); + RH56DFTPDexhand( + const config::RH56DFTPDexHandConfig& cfg, + std::unique_ptr controller); ~RH56DFTPDexhand() override; std::string typeName() const override { return "RH56DFTPDexhand"; } @@ -68,6 +74,8 @@ namespace cmvr::device { Status state() const override; std::string lastError() const override; void getState(DexHandState& state) override; + bool stopOperationalActivity() override; + bool resumeOperationalActivity() override; void setAngles(const std::vector& finger_joint_angles) override; void setTactilePollingRegions(const std::vector& regions) override; @@ -110,6 +118,18 @@ namespace cmvr::device { mutable std::mutex command_mutex_; std::array last_commanded_angles_{}; + // Kept separately from last_commanded_angles_: a failed Modbus block + // write may still have changed a prefix of the device registers. Such + // an attempt must remain visible to StopAll without being reported as + // a successfully accepted command. + std::array pending_angle_target_{}; + bool angle_target_unconfirmed_{false}; + + // Normal command and tactile I/O take this gate in shared mode. + // StopAll first closes admission and then takes it exclusively, which + // drains every operation that crossed the boundary before the stop. + mutable std::shared_mutex operational_gate_; + std::atomic operational_paused_{false}; std::array tactile_buffers_; std::array tactile_buffer_masks_{}; diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp index 44737981..3d0875bd 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp @@ -21,6 +21,11 @@ namespace { using Status = DexHand::Status; constexpr int kAngleSetByteAddress = 1486; + constexpr int kAngleActualByteAddress = 1546; + constexpr int kAngleStoppedTolerance = 5; + constexpr int kAngleStableTolerance = 1; + constexpr int kAngleStopConfirmationSamples = 3; + constexpr auto kAngleStopSampleInterval = std::chrono::milliseconds(10); constexpr int kDefaultPort = 6000; constexpr int kMaxRegistersPerRead = 125; @@ -325,8 +330,16 @@ void ModbusController::closeUnlocked() { } RH56DFTPDexhand::RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& cfg) - : controller_(std::make_unique()), - dexhandCfg_(cfg) { + : RH56DFTPDexhand(cfg, std::make_unique()) { +} + +RH56DFTPDexhand::RH56DFTPDexhand( + const config::RH56DFTPDexHandConfig& cfg, + std::unique_ptr controller) + : controller_(std::move(controller)), dexhandCfg_(cfg) { + if (!controller_) { + throw std::invalid_argument("RH56 Modbus controller is required"); + } id_ = dexhandCfg_.id(); ip_address_ = dexhandCfg_.ip(); if (dexhandCfg_.port() > 0) { @@ -352,6 +365,9 @@ bool RH56DFTPDexhand::init() { } bool RH56DFTPDexhand::start() { + if (!resumeOperationalActivity()) { + return false; + } if (tactile_thread_running_.exchange(true, std::memory_order_acq_rel)) { transitionTo(Status::STREAMING); return true; @@ -387,6 +403,7 @@ bool RH56DFTPDexhand::start() { } bool RH56DFTPDexhand::stop() { + operational_paused_.store(true, std::memory_order_release); tactile_thread_running_.store(false, std::memory_order_release); polling_cv_.notify_all(); @@ -394,8 +411,14 @@ bool RH56DFTPDexhand::stop() { tactile_thread_.join(); } - if (controller_) { - controller_->close(); + { + // Drain command and tactile dispatches before closing their transport. + std::unique_lock operational_lock( + operational_gate_); + operational_paused_.store(true, std::memory_order_release); + if (controller_) { + controller_->close(); + } } if (state() != Status::FAULT) { @@ -433,13 +456,124 @@ void RH56DFTPDexhand::getState(DexHandState& state_out) { state_out = std::move(next_state); } +bool RH56DFTPDexhand::stopOperationalActivity() { + operational_paused_.store(true, std::memory_order_release); + polling_cv_.notify_all(); + + // Taking the gate exclusively confirms that every command write and + // tactile read admitted before StopAll has left the Modbus boundary. + std::unique_lock operational_lock(operational_gate_); + operational_paused_.store(true, std::memory_order_release); + + std::array target{}; + { + std::lock_guard command_lock(command_mutex_); + if (!angle_target_unconfirmed_) { + return true; + } + target = pending_angle_target_; + } + + // RH56 exposes no hold/quick-stop command. The actual-angle registers are + // therefore the only physical confirmation available. Require several + // samples both at the requested target and stable over time; a single + // sample can coincide with a joint crossing the target while still moving. + // Otherwise StopAll stays fail-closed and a later round can retry. + if (!controller_ || !controller_->isOpen()) { + CMVR_LOG(ERROR) + << "[RH56DFTPDexhand] cannot confirm the last angle target: " + "Modbus is not connected"; + return false; + } + std::vector previous_actual; + for (int sample = 0; sample < kAngleStopConfirmationSamples; ++sample) { + if (sample != 0) { + std::this_thread::sleep_for(kAngleStopSampleInterval); + } + + std::vector actual; + if (!controller_->readRegisterBlock( + kAngleActualByteAddress, + static_cast(ANGLE_COMMAND_COUNT), + actual) || + actual.size() != ANGLE_COMMAND_COUNT) { + CMVR_LOG(ERROR) + << "[RH56DFTPDexhand] failed to read actual joint angles " + "while confirming operational stop"; + return false; + } + for (std::size_t index = 0; index < target.size(); ++index) { + if (std::abs(static_cast(actual[index]) - target[index]) > + kAngleStoppedTolerance) { + CMVR_LOG(WARNING) + << "[RH56DFTPDexhand] joint " << index + << " has not reached its pending target; target=" + << target[index] << ", actual=" << actual[index]; + return false; + } + if (!previous_actual.empty() && + std::abs(static_cast(actual[index]) - + static_cast(previous_actual[index])) > + kAngleStableTolerance) { + CMVR_LOG(WARNING) + << "[RH56DFTPDexhand] joint " << index + << " is not stable while confirming operational stop; " + "previous=" + << previous_actual[index] << ", actual=" << actual[index]; + return false; + } + } + previous_actual = std::move(actual); + } + + { + std::lock_guard command_lock(command_mutex_); + angle_target_unconfirmed_ = false; + } + return true; +} + +bool RH56DFTPDexhand::resumeOperationalActivity() { + std::unique_lock operational_lock(operational_gate_); + operational_paused_.store(false, std::memory_order_release); + operational_lock.unlock(); + polling_cv_.notify_all(); + return true; +} + void RH56DFTPDexhand::setAngles(const std::vector& finger_joint_angles) { + if (finger_joint_angles.size() != ANGLE_COMMAND_COUNT) { + CMVR_LOG(ERROR) << "RH56DFTPDexhand expects exactly 6 joint angles."; + return; + } + if (operational_paused_.load(std::memory_order_acquire)) { + CMVR_LOG(WARNING) + << "[RH56DFTPDexhand] angle command rejected while operational " + "activity is paused"; + return; + } + std::shared_lock operational_lock(operational_gate_); + if (operational_paused_.load(std::memory_order_acquire)) { + return; + } const auto registers = encodeAngleCommand(finger_joint_angles); try { if (!ensureConnected()) { return; } + + // Mark the write attempt before crossing the Modbus boundary. A false + // return can represent a partial register write, so only StopAll's + // physical confirmation may clear this state. + { + std::lock_guard lock(command_mutex_); + std::copy( + finger_joint_angles.begin(), + finger_joint_angles.end(), + pending_angle_target_.begin()); + angle_target_unconfirmed_ = true; + } if (!controller_->writeRegisters( kAngleSetByteAddress, registers.data(), @@ -538,6 +672,14 @@ void RH56DFTPDexhand::refreshTactileData(const RegionMask& mask) { if (mask.none()) { return; } + if (operational_paused_.load(std::memory_order_acquire)) { + return; + } + + std::shared_lock operational_lock(operational_gate_); + if (operational_paused_.load(std::memory_order_acquire)) { + return; + } try { if (!ensureConnected()) { @@ -586,9 +728,12 @@ void RH56DFTPDexhand::tactilePollingLoop() { auto next_poll_deadline = std::chrono::steady_clock::now(); std::unique_lock lock(polling_mutex_); while (tactile_thread_running_.load(std::memory_order_acquire)) { - if (requested_polling_mask_.none()) { + if (requested_polling_mask_.none() || + operational_paused_.load(std::memory_order_acquire)) { polling_cv_.wait(lock, [this]() { - return !tactile_thread_running_.load(std::memory_order_acquire) || requested_polling_mask_.any(); + return !tactile_thread_running_.load(std::memory_order_acquire) || + (!operational_paused_.load(std::memory_order_acquire) && + requested_polling_mask_.any()); }); next_poll_deadline = std::chrono::steady_clock::now(); continue; @@ -611,7 +756,9 @@ void RH56DFTPDexhand::tactilePollingLoop() { } polling_cv_.wait_until(lock, next_poll_deadline, [this, mask]() { - return !tactile_thread_running_.load(std::memory_order_acquire) || requested_polling_mask_ != mask; + return !tactile_thread_running_.load(std::memory_order_acquire) || + operational_paused_.load(std::memory_order_acquire) || + requested_polling_mask_ != mask; }); } } @@ -667,6 +814,9 @@ void RH56DFTPDexhand::ensureTactileMaskReady(const RegionMask& mask, const bool if (mask.none()) { return; } + if (operational_paused_.load(std::memory_order_acquire)) { + return; + } const bool background_ready = allow_background && tactile_thread_running_.load(std::memory_order_acquire) && diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/tests/rh56dftp_dexhand_stop_all_test.cpp b/cmvr-es/devices/dexhand/rh56dftp_dexhand/tests/rh56dftp_dexhand_stop_all_test.cpp new file mode 100644 index 00000000..9414588c --- /dev/null +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/tests/rh56dftp_dexhand_stop_all_test.cpp @@ -0,0 +1,348 @@ +#include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace cmvr::device { +namespace { + +using namespace std::chrono_literals; + +class FakeModbusController final : public ModbusController { +public: + bool open(const std::string&, int) override + { + std::lock_guard lock(mutex_); + open_ = true; + return true; + } + + void close() override + { + std::lock_guard lock(mutex_); + open_ = false; + } + + bool isOpen() const override + { + std::lock_guard lock(mutex_); + return open_; + } + + bool writeRegisters( + int, + const uint16_t* values, + const int count) override + { + std::unique_lock lock(mutex_); + ++write_calls_; + write_started_ = true; + condition_.notify_all(); + condition_.wait(lock, [this] { return !block_write_; }); + if (!write_succeeds_) { + return false; + } + actual_angles_.assign(values, values + count); + return true; + } + + bool readRegisterBlock( + const int address, + const int count, + std::vector& values) override + { + std::unique_lock lock(mutex_); + ++read_calls_; + read_started_ = true; + condition_.notify_all(); + condition_.wait(lock, [this] { return !block_read_; }); + const std::vector* source = &actual_angles_; + std::vector sampled_angles; + if (address == 1546 && !actual_angle_samples_.empty()) { + const auto sample = actual_angle_samples_.front(); + actual_angle_samples_.pop_front(); + sampled_angles.assign(sample.begin(), sample.end()); + source = &sampled_angles; + } + values.assign(static_cast(count), 0U); + for (std::size_t index = 0; + index < values.size() && index < source->size(); + ++index) { + values[index] = (*source)[index]; + } + return read_succeeds_; + } + + void setActualAngles(const std::array& values) + { + std::lock_guard lock(mutex_); + actual_angles_.assign(values.begin(), values.end()); + actual_angle_samples_.clear(); + } + + void setActualAngleSamples( + std::deque> samples) + { + std::lock_guard lock(mutex_); + actual_angle_samples_ = std::move(samples); + } + + void setWriteSucceeds(const bool succeeds) + { + std::lock_guard lock(mutex_); + write_succeeds_ = succeeds; + } + + void blockNextRead() + { + std::lock_guard lock(mutex_); + block_read_ = true; + read_started_ = false; + } + + void releaseRead() + { + { + std::lock_guard lock(mutex_); + block_read_ = false; + } + condition_.notify_all(); + } + + bool waitForRead(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return condition_.wait_for( + lock, timeout, [this] { return read_started_; }); + } + + bool waitForReadCalls( + const int expected, + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return condition_.wait_for( + lock, timeout, [this, expected] { return read_calls_ >= expected; }); + } + + void blockNextWrite() + { + std::lock_guard lock(mutex_); + block_write_ = true; + write_started_ = false; + } + + void releaseWrite() + { + { + std::lock_guard lock(mutex_); + block_write_ = false; + } + condition_.notify_all(); + } + + bool waitForWrite(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return condition_.wait_for( + lock, timeout, [this] { return write_started_; }); + } + + int readCalls() const + { + std::lock_guard lock(mutex_); + return read_calls_; + } + + int writeCalls() const + { + std::lock_guard lock(mutex_); + return write_calls_; + } + +private: + mutable std::mutex mutex_; + std::condition_variable condition_; + std::vector actual_angles_{6U, 0U}; + std::deque> actual_angle_samples_; + bool open_{false}; + bool block_read_{false}; + bool block_write_{false}; + bool read_started_{false}; + bool write_started_{false}; + bool read_succeeds_{true}; + bool write_succeeds_{true}; + int read_calls_{0}; + int write_calls_{0}; +}; + +struct TestHand { + TestHand() + { + config.set_id("rh56-test"); + config.set_ip("fake-modbus"); + auto controller = std::make_unique(); + fake = controller.get(); + hand = std::make_unique( + config, std::move(controller)); + EXPECT_TRUE(hand->init()); + } + + ~TestHand() + { + if (hand) { + hand->stop(); + } + } + + config::RH56DFTPDexHandConfig config; + FakeModbusController* fake{nullptr}; + std::unique_ptr hand; +}; + +TEST(RH56DFTPDexhandStopAllTest, IdleTactileDeviceStopsWithoutClosingLifecycle) +{ + TestHand fixture; + + EXPECT_TRUE(fixture.hand->stopOperationalActivity()); + EXPECT_EQ(fixture.hand->state(), AbstractDexHand::Status::INITIALIZED); + EXPECT_TRUE(fixture.fake->isOpen()); + + const int reads_before = fixture.fake->readCalls(); + (void)fixture.hand->getSensorData( + AbstractDexHand::FingerType::INDEX, + AbstractDexHand::TactileRegion::TIP); + EXPECT_EQ(fixture.fake->readCalls(), reads_before); +} + +TEST(RH56DFTPDexhandStopAllTest, UnreachedAngleTargetFailsClosedThenRecovers) +{ + TestHand fixture; + const std::vector target{100, 200, 300, 400, 500, 600}; + + fixture.hand->setAngles(target); + fixture.fake->setActualAngles({0, 0, 0, 0, 0, 0}); + EXPECT_FALSE(fixture.hand->stopOperationalActivity()); + + fixture.fake->setActualAngles({100, 200, 300, 400, 500, 600}); + EXPECT_TRUE(fixture.hand->stopOperationalActivity()); + EXPECT_TRUE(fixture.fake->isOpen()); +} + +TEST(RH56DFTPDexhandStopAllTest, MovingSampleAtTargetDoesNotConfirmStop) +{ + TestHand fixture; + const std::vector target{100, 200, 300, 400, 500, 600}; + + fixture.hand->setAngles(target); + fixture.fake->setActualAngleSamples({ + {100, 200, 300, 400, 500, 600}, + {103, 203, 303, 403, 503, 603}, + {106, 206, 306, 406, 506, 606}, + }); + EXPECT_FALSE(fixture.hand->stopOperationalActivity()); + + fixture.fake->setActualAngles({100, 200, 300, 400, 500, 600}); + EXPECT_TRUE(fixture.hand->stopOperationalActivity()); +} + +TEST(RH56DFTPDexhandStopAllTest, FailedAngleWriteRemainsUnconfirmed) +{ + TestHand fixture; + fixture.fake->setActualAngles({0, 0, 0, 0, 0, 0}); + fixture.fake->setWriteSucceeds(false); + + fixture.hand->setAngles({100, 200, 300, 400, 500, 600}); + + EXPECT_FALSE(fixture.hand->stopOperationalActivity()); +} + +TEST(RH56DFTPDexhandStopAllTest, StopWaitsForAdmittedAngleWrite) +{ + TestHand fixture; + fixture.fake->blockNextWrite(); + const std::vector target{100, 200, 300, 400, 500, 600}; + auto command = std::async(std::launch::async, [&] { + fixture.hand->setAngles(target); + }); + ASSERT_TRUE(fixture.fake->waitForWrite(500ms)); + + auto stop = std::async(std::launch::async, [&] { + return fixture.hand->stopOperationalActivity(); + }); + EXPECT_EQ(stop.wait_for(20ms), std::future_status::timeout); + + fixture.fake->releaseWrite(); + EXPECT_EQ(command.wait_for(500ms), std::future_status::ready); + command.get(); + ASSERT_EQ(stop.wait_for(500ms), std::future_status::ready); + EXPECT_TRUE(stop.get()); + + const int writes_before = fixture.fake->writeCalls(); + fixture.hand->setAngles(target); + EXPECT_EQ(fixture.fake->writeCalls(), writes_before); + EXPECT_TRUE(fixture.hand->resumeOperationalActivity()); + fixture.hand->setAngles(target); + EXPECT_EQ(fixture.fake->writeCalls(), writes_before + 1); +} + +TEST(RH56DFTPDexhandStopAllTest, StopWaitsForAdmittedTactileRead) +{ + TestHand fixture; + fixture.fake->blockNextRead(); + auto read = std::async(std::launch::async, [&] { + return fixture.hand->getSensorData( + AbstractDexHand::FingerType::INDEX, + AbstractDexHand::TactileRegion::TIP); + }); + ASSERT_TRUE(fixture.fake->waitForRead(500ms)); + + auto stop = std::async(std::launch::async, [&] { + return fixture.hand->stopOperationalActivity(); + }); + EXPECT_EQ(stop.wait_for(20ms), std::future_status::timeout); + + fixture.fake->releaseRead(); + EXPECT_EQ(read.wait_for(500ms), std::future_status::ready); + (void)read.get(); + ASSERT_EQ(stop.wait_for(500ms), std::future_status::ready); + EXPECT_TRUE(stop.get()); +} + +TEST(RH56DFTPDexhandStopAllTest, StopDrainsAndPausesBackgroundTactilePolling) +{ + TestHand fixture; + ASSERT_TRUE(fixture.hand->start()); + + const int reads_before_block = fixture.fake->readCalls(); + fixture.fake->blockNextRead(); + ASSERT_TRUE(fixture.fake->waitForReadCalls(reads_before_block + 1, 500ms)); + + auto stop = std::async(std::launch::async, [&] { + return fixture.hand->stopOperationalActivity(); + }); + EXPECT_EQ(stop.wait_for(20ms), std::future_status::timeout); + + fixture.fake->releaseRead(); + ASSERT_EQ(stop.wait_for(500ms), std::future_status::ready); + EXPECT_TRUE(stop.get()); + + const int reads_after_stop = fixture.fake->readCalls(); + std::this_thread::sleep_for(30ms); + EXPECT_EQ(fixture.fake->readCalls(), reads_after_stop); + + ASSERT_TRUE(fixture.hand->resumeOperationalActivity()); + EXPECT_TRUE(fixture.fake->waitForReadCalls(reads_after_stop + 1, 500ms)); +} + +} // namespace +} // namespace cmvr::device diff --git a/cmvr-es/devices/gripper/abstract_gripper.h b/cmvr-es/devices/gripper/abstract_gripper.h index 728d094b..d4dcfed1 100644 --- a/cmvr-es/devices/gripper/abstract_gripper.h +++ b/cmvr-es/devices/gripper/abstract_gripper.h @@ -23,6 +23,11 @@ namespace cmvr::device{ virtual void setPosition(float position, float vel) {} virtual void setForce(float value) {} + // Stops command-driven gripper activity while preserving the device + // lifecycle. Backends must explicitly confirm this contract before + // SystemService::StopAll can report success. + virtual bool stopOperationalActivity() { return false; } + protected: GripperState state_; }; diff --git a/cmvr-es/devices/motor/manager/include/motor_manager.h b/cmvr-es/devices/motor/manager/include/motor_manager.h index 3bbd2cab..2e31f856 100644 --- a/cmvr-es/devices/motor/manager/include/motor_manager.h +++ b/cmvr-es/devices/motor/manager/include/motor_manager.h @@ -46,6 +46,15 @@ public: std::shared_ptr getMotor(const std::string& joint_name) const; const std::unordered_map>& motorsMap() const; + // A MotorRobotArm owns its joints for the lifetime of the arm instance. + // Direct per-motor control must not compete with that group controller. + bool claimArmJoints(const std::string& arm_id, + const std::vector& joint_names, + std::uint64_t& claim_id, + std::string* error = nullptr); + void releaseArmJoints(std::uint64_t claim_id) noexcept; + std::string armOwnerForJoint(const std::string& joint_name) const; + static std::shared_ptr managerFor(const std::string& id); static std::shared_ptr mujocoWorldFor(const std::string& id); static void setActiveJoints(const std::string& motor_manager_id, @@ -85,6 +94,13 @@ private: mutable std::mutex motors_mutex_; std::unordered_map> motors_by_id_; std::unordered_map> motors_by_joint_; + struct ArmJointClaim { + std::string arm_id; + std::vector joint_names; + }; + std::uint64_t next_arm_claim_id_{0}; + std::unordered_map arm_claims_; + std::unordered_map arm_claim_by_joint_; bool initialized_{false}; static std::mutex registry_mutex_; diff --git a/cmvr-es/devices/motor/manager/src/motor_manager.cpp b/cmvr-es/devices/motor/manager/src/motor_manager.cpp index 5e83b043..64fa0aa4 100644 --- a/cmvr-es/devices/motor/manager/src/motor_manager.cpp +++ b/cmvr-es/devices/motor/manager/src/motor_manager.cpp @@ -239,6 +239,120 @@ const std::unordered_map>& MotorMana return motors_by_joint_; } +bool MotorManager::claimArmJoints( + const std::string& arm_id, + const std::vector& joint_names, + std::uint64_t& claim_id, + std::string* error) +{ + claim_id = 0; + if (error) { + error->clear(); + } + if (arm_id.empty() || joint_names.empty()) { + if (error) { + *error = "arm id and joint names are required"; + } + return false; + } + + std::unordered_set unique_joints; + unique_joints.reserve(joint_names.size()); + std::lock_guard lock(motors_mutex_); + for (const auto& joint_name : joint_names) { + if (joint_name.empty() || !unique_joints.insert(joint_name).second) { + if (error) { + *error = joint_name.empty() + ? "arm joint name cannot be empty" + : "arm joint is listed more than once: " + joint_name; + } + return false; + } + if (motors_by_joint_.count(joint_name) == 0U) { + if (error) { + *error = "motor not found for arm joint: " + joint_name; + } + return false; + } + const auto existing = arm_claim_by_joint_.find(joint_name); + if (existing != arm_claim_by_joint_.end()) { + const auto owner = arm_claims_.find(existing->second); + if (error) { + *error = "motor joint is already controlled by RobotArm"; + if (owner != arm_claims_.end()) { + *error += " '" + owner->second.arm_id + "'"; + } + *error += ": " + joint_name; + } + return false; + } + } + + do { + ++next_arm_claim_id_; + } while (next_arm_claim_id_ == 0U || + arm_claims_.count(next_arm_claim_id_) != 0U); + + ArmJointClaim claim; + claim.arm_id = arm_id; + claim.joint_names.assign(unique_joints.begin(), unique_joints.end()); + const auto new_claim_id = next_arm_claim_id_; + arm_claims_.emplace(new_claim_id, std::move(claim)); + try { + for (const auto& joint_name : unique_joints) { + arm_claim_by_joint_.emplace(joint_name, new_claim_id); + } + } catch (...) { + for (auto it = arm_claim_by_joint_.begin(); + it != arm_claim_by_joint_.end();) { + if (it->second == new_claim_id) { + it = arm_claim_by_joint_.erase(it); + } else { + ++it; + } + } + arm_claims_.erase(new_claim_id); + throw; + } + claim_id = new_claim_id; + return true; +} + +void MotorManager::releaseArmJoints(const std::uint64_t claim_id) noexcept +{ + if (claim_id == 0U) { + return; + } + try { + std::lock_guard lock(motors_mutex_); + const auto claim = arm_claims_.find(claim_id); + if (claim == arm_claims_.end()) { + return; + } + for (const auto& joint_name : claim->second.joint_names) { + const auto owner = arm_claim_by_joint_.find(joint_name); + if (owner != arm_claim_by_joint_.end() && + owner->second == claim_id) { + arm_claim_by_joint_.erase(owner); + } + } + arm_claims_.erase(claim); + } catch (...) { + } +} + +std::string MotorManager::armOwnerForJoint( + const std::string& joint_name) const +{ + std::lock_guard lock(motors_mutex_); + const auto owner = arm_claim_by_joint_.find(joint_name); + if (owner == arm_claim_by_joint_.end()) { + return {}; + } + const auto claim = arm_claims_.find(owner->second); + return claim == arm_claims_.end() ? std::string{} : claim->second.arm_id; +} + std::shared_ptr MotorManager::managerFor(const std::string& id) { std::lock_guard lock(registry_mutex_); diff --git a/cmvr-es/devices/speaker/abstract_speaker.h b/cmvr-es/devices/speaker/abstract_speaker.h index ab24542d..cfdb62d9 100644 --- a/cmvr-es/devices/speaker/abstract_speaker.h +++ b/cmvr-es/devices/speaker/abstract_speaker.h @@ -21,6 +21,11 @@ namespace cmvr::device{ virtual int getVolume() const {return 0;} virtual void pause() {} virtual void resume() {} + // Stops the current file or streamed playback without changing the + // device lifecycle. SystemService StopAll and SpeakerService use this + // typed operation; implementations should return only after their + // playback workers can no longer emit audio. + virtual bool stopPlayback() { return false; } virtual bool pushAudioFrame(const AudioStreamFrameData& frame_data) { return false; } virtual void stopStreaming() {} diff --git a/cmvr-es/devices/speaker/ffmpeg_speaker/CMakeLists.txt b/cmvr-es/devices/speaker/ffmpeg_speaker/CMakeLists.txt index 7ff77300..c37d546c 100644 --- a/cmvr-es/devices/speaker/ffmpeg_speaker/CMakeLists.txt +++ b/cmvr-es/devices/speaker/ffmpeg_speaker/CMakeLists.txt @@ -7,3 +7,32 @@ add_library(cmvr_es::device::ffmpeg_speaker ALIAS ffmpeg_speaker) target_link_libraries(ffmpeg_speaker PRIVATE -lpulse-simple -lpulse cmvr_es::proto) install(TARGETS ffmpeg_speaker LIBRARY DESTINATION lib) + +if(BUILD_TESTING) + add_executable(ffmpeg_speaker_lifecycle_test + tests/ffmpeg_speaker_lifecycle_test.cpp + ) + target_link_libraries(ffmpeg_speaker_lifecycle_test + PRIVATE + cmvr_es::device::ffmpeg_speaker + avcodec + avformat + avutil + swresample + ) + add_test( + NAME ffmpeg_speaker_lifecycle_test + COMMAND ffmpeg_speaker_lifecycle_test + ) + set(_ffmpeg_speaker_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _ffmpeg_speaker_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(ffmpeg_speaker_lifecycle_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_ffmpeg_speaker_test_environment}" + ) +endif() diff --git a/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_speaker.h b/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_speaker.h index 124213b3..6c95eb37 100644 --- a/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_speaker.h +++ b/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_speaker.h @@ -32,6 +32,7 @@ namespace cmvr::device { bool init() override; bool start() override; bool stop() override; + bool stopPlayback() override; void play(const std::string& audio_path) override; void setVolume(int volume) override; int getVolume() const override; @@ -45,6 +46,7 @@ namespace cmvr::device { bool initPulseDevice_(); bool initAudioParams_(const std::string& audio_path); private: + bool stopPlayback_(bool deinitialize); void decode_audio_(); void play_audio_(); bool startStreamingPlayback_(const AudioStreamFrameData& frame_data); diff --git a/cmvr-es/devices/speaker/ffmpeg_speaker/src/ffmpeg_speaker.cpp b/cmvr-es/devices/speaker/ffmpeg_speaker/src/ffmpeg_speaker.cpp index 125d9e04..a0c25b3b 100644 --- a/cmvr-es/devices/speaker/ffmpeg_speaker/src/ffmpeg_speaker.cpp +++ b/cmvr-es/devices/speaker/ffmpeg_speaker/src/ffmpeg_speaker.cpp @@ -34,6 +34,7 @@ ffmpegSpeaker::~ffmpegSpeaker() { is_stopping_ = true; { std::lock_guard lock(mtx_); + state_.is_initialized = false; state_.is_running = false; state_.is_decoding = false; state_.is_paused = false; @@ -94,7 +95,6 @@ void ffmpegSpeaker::resetPlayState() // 清空所有帧 } - state_.is_initialized = false; is_streaming_input_ = false; audio_path_.clear(); @@ -102,11 +102,22 @@ void ffmpegSpeaker::resetPlayState() } bool ffmpegSpeaker::stop() { + return stopPlayback_(true); +} + +bool ffmpegSpeaker::stopPlayback() { + return stopPlayback_(false); +} + +bool ffmpegSpeaker::stopPlayback_(const bool deinitialize) { std::lock_guard stop_lock(stop_mtx_); is_stopping_ = true; { lock_guard lock(mtx_); + if (deinitialize) { + state_.is_initialized = false; + } state_.is_running = false; state_.is_decoding = false; state_.is_paused = false; diff --git a/cmvr-es/devices/speaker/ffmpeg_speaker/tests/ffmpeg_speaker_lifecycle_test.cpp b/cmvr-es/devices/speaker/ffmpeg_speaker/tests/ffmpeg_speaker_lifecycle_test.cpp new file mode 100644 index 00000000..432a82a6 --- /dev/null +++ b/cmvr-es/devices/speaker/ffmpeg_speaker/tests/ffmpeg_speaker_lifecycle_test.cpp @@ -0,0 +1,54 @@ +#include "include/ffmpeg_speaker.h" + +#include + +namespace { + +bool check(const bool condition, const char* expression, const int line) +{ + if (condition) { + return true; + } + std::cerr << "CHECK failed at line " << line << ": " << expression << '\n'; + return false; +} + +#define CHECK_TRUE(expression) \ + do { \ + if (!check(static_cast(expression), #expression, __LINE__)) { \ + return 1; \ + } \ + } while (false) + +} // namespace + +int main() +{ + cmvr::config::FFMpegSpeakerConfig config; + config.set_id("lifecycle-test-speaker"); + cmvr::device::ffmpegSpeaker speaker(config); + cmvr::device::SpeakerState state{}; + + CHECK_TRUE(speaker.init()); + speaker.getState(state); + CHECK_TRUE(state.is_initialized); + + speaker.resetPlayState(); + speaker.getState(state); + CHECK_TRUE(state.is_initialized); + + CHECK_TRUE(speaker.stopPlayback()); + speaker.getState(state); + CHECK_TRUE(state.is_initialized); + + speaker.stopStreaming(); + speaker.getState(state); + CHECK_TRUE(state.is_initialized); + + CHECK_TRUE(speaker.stop()); + speaker.getState(state); + CHECK_TRUE(!state.is_initialized); + + std::cout << "ffmpeg_speaker_lifecycle_test: PASS\n"; + return 0; +} diff --git a/cmvr-es/manager/README.md b/cmvr-es/manager/README.md index 7ef1429b..b30bc369 100644 --- a/cmvr-es/manager/README.md +++ b/cmvr-es/manager/README.md @@ -30,7 +30,8 @@ - DeviceManager 构造不会自动调用全部设备的 `start()`； - 当前主退出路径没有调用 `DeviceManager::stop()`； -- `SystemService/StopAll` 会调用 DeviceManager stop； +- `SystemService/StopAll` 只停止当前运动、控制和媒体活动，不调用 + `DeviceManager::stop()`，成功返回后可继续接受新命令； - `DeviceManager::destroyInstance()` 不调用设备 stop，销毁前必须先显式停止； - `TaskManager::destroyInstance()` 会调用 `stopRunTask()`，但 manager 未处于 running 状态时该调用会直接返回； - DeviceManager 和 TaskManager 都是首次配置生效的单例，不支持热加载。 diff --git a/cmvr-es/manager/control_authority/include/control_authority_manager.h b/cmvr-es/manager/control_authority/include/control_authority_manager.h index bda89ad5..ceba5057 100644 --- a/cmvr-es/manager/control_authority/include/control_authority_manager.h +++ b/cmvr-es/manager/control_authority/include/control_authority_manager.h @@ -2,7 +2,9 @@ #define CMVR_ES_CONTROL_AUTHORITY_MANAGER_H #include +#include #include +#include #include #include #include @@ -28,6 +30,8 @@ struct ControlAcquireResult { std::string detail; }; +class ControlDispatchGuard; + // Process-wide, transport-independent control ownership. The generation in a // token prevents a delayed release from an old network session from releasing // a newer lease on the same arm. @@ -58,13 +62,40 @@ public: const std::string& owner_id, Duration ttl); + // Waits for normal lease handlers displaced by the current safety barrier + // to release their tokens and for their in-flight dispatches to finish. + // Returns false on timeout or when safety_token is no longer a holder of + // the current entry. + bool waitForPreemptedRelease( + const ControlLeaseToken& safety_token, + Duration timeout); + // Permanently blocks the resource only if the expected normal lease is - // still current. Quarantine does not allocate and can only be removed by - // an explicit revoke/clear. + // still current. A later safety holder may clear this fail-closed state + // only after it has independently confirmed the preempted handler exited. bool quarantineIfCurrent( const ControlLeaseToken& expected_token) noexcept; + + // Abandons a safety token while retaining its barrier. A retired token can + // no longer be validated, released, or used as a recovery authority. This + // lets a failed stop path discard local token ownership without silently + // reopening the resource. + bool retireSafetyHolder( + const ControlLeaseToken& safety_token) noexcept; + + // Clears retired safety holders and a tokenless quarantine after a newer, + // active safety holder has confirmed every preempted normal handler has + // exited. Other active safety holders are deliberately preserved. + bool recoverRetiredSafetyHolders( + const ControlLeaseToken& recovery_token) noexcept; + bool renew(const ControlLeaseToken& token, Duration ttl); bool validate(const ControlLeaseToken& token); + + // Use only around a bounded device-command submission. Never retain this + // guard while waiting for physical motion or another long-running task. + ControlDispatchGuard tryBeginDispatch( + const ControlLeaseToken& token); void release(const ControlLeaseToken& token) noexcept; // Safety/control paths which do not possess a lease use this query to @@ -78,23 +109,62 @@ public: void clear() noexcept; private: - struct Entry { + friend class ControlDispatchGuard; + + struct SafetyHolder { std::string owner_id; - std::uint64_t generation{0}; - std::chrono::steady_clock::time_point deadline; - bool preemptible{true}; - bool quarantined{false}; - std::unordered_map safety_holders; + bool retired{false}; }; + struct Entry; + static void quarantine_(Entry& entry) noexcept; bool expired_(const Entry& entry) const noexcept; + static bool isSafetyHolder_( + const Entry& entry, + const ControlLeaseToken& token) noexcept; + static bool isActiveSafetyHolder_( + const Entry& entry, + const ControlLeaseToken& token) noexcept; + static bool canErase_(const Entry& entry) noexcept; + static void invalidateToDispatchFence_(Entry& entry) noexcept; + void endDispatch_(const std::shared_ptr& entry) noexcept; std::mutex mutex_; - std::unordered_map entries_; + std::condition_variable release_cv_; + std::unordered_map> entries_; std::uint64_t next_generation_{0}; }; +// Tracks one bounded backend dispatch without retaining the process-wide +// authority lock. Safety preemption invalidates the lease immediately, while +// waitForPreemptedRelease() joins both the displaced handler and its in-flight +// dispatches before the safety operation reaches the device. +class ControlDispatchGuard final { +public: + ControlDispatchGuard() noexcept = default; + ~ControlDispatchGuard() noexcept; + + ControlDispatchGuard(ControlDispatchGuard&& other) noexcept; + ControlDispatchGuard& operator=(ControlDispatchGuard&& other) noexcept; + + ControlDispatchGuard(const ControlDispatchGuard&) = delete; + ControlDispatchGuard& operator=(const ControlDispatchGuard&) = delete; + + bool acquired() const noexcept { return entry_ != nullptr; } + +private: + friend class ControlAuthorityManager; + + ControlDispatchGuard( + ControlAuthorityManager* manager, + std::shared_ptr entry) noexcept; + void reset_() noexcept; + + ControlAuthorityManager* manager_{nullptr}; + std::shared_ptr entry_; +}; + } // namespace cmvr::control #endif // CMVR_ES_CONTROL_AUTHORITY_MANAGER_H diff --git a/cmvr-es/manager/control_authority/src/control_authority_manager.cpp b/cmvr-es/manager/control_authority/src/control_authority_manager.cpp index 00c89730..614c32d4 100644 --- a/cmvr-es/manager/control_authority/src/control_authority_manager.cpp +++ b/cmvr-es/manager/control_authority/src/control_authority_manager.cpp @@ -1,10 +1,65 @@ #include "manager/control_authority/include/control_authority_manager.h" -#include #include namespace cmvr::control { +struct ControlAuthorityManager::Entry { + std::string resource_id; + std::string owner_id; + std::uint64_t generation{0}; + std::chrono::steady_clock::time_point deadline; + bool preemptible{true}; + bool quarantined{false}; + bool quarantined_normal_pending{false}; + bool dispatch_fence_only{false}; + std::uint64_t in_flight_dispatches{0}; + std::unordered_map safety_holders; + std::unordered_map + preempted_normal_holders; +}; + +ControlDispatchGuard::ControlDispatchGuard( + ControlAuthorityManager* const manager, + std::shared_ptr entry) noexcept + : manager_(manager), + entry_(std::move(entry)) +{ +} + +ControlDispatchGuard::~ControlDispatchGuard() noexcept +{ + reset_(); +} + +ControlDispatchGuard::ControlDispatchGuard( + ControlDispatchGuard&& other) noexcept + : manager_(std::exchange(other.manager_, nullptr)), + entry_(std::move(other.entry_)) +{ +} + +ControlDispatchGuard& ControlDispatchGuard::operator=( + ControlDispatchGuard&& other) noexcept +{ + if (this != &other) { + reset_(); + manager_ = std::exchange(other.manager_, nullptr); + entry_ = std::move(other.entry_); + } + return *this; +} + +void ControlDispatchGuard::reset_() noexcept +{ + if (entry_ == nullptr) { + return; + } + auto entry = std::move(entry_); + auto* const manager = std::exchange(manager_, nullptr); + manager->endDispatch_(entry); +} + ControlAuthorityManager& ControlAuthorityManager::instance() { static ControlAuthorityManager manager; @@ -24,12 +79,26 @@ ControlAcquireResult ControlAuthorityManager::tryAcquire( std::lock_guard lock(mutex_); const auto existing = entries_.find(resource_id); if (existing != entries_.end()) { - if (!expired_(existing->second)) { + auto& entry = *existing->second; + if (!expired_(entry)) { + if (entry.dispatch_fence_only) { + return { + false, + {}, + "control resource still has an in-flight dispatch"}; + } return { false, {}, "control resource is already leased by " + - existing->second.owner_id}; + entry.owner_id}; + } + if (entry.in_flight_dispatches != 0U) { + invalidateToDispatchFence_(entry); + return { + false, + {}, + "control resource still has an in-flight dispatch"}; } entries_.erase(existing); } @@ -38,15 +107,13 @@ ControlAcquireResult ControlAuthorityManager::tryAcquire( token.resource_id = resource_id; token.owner_id = owner_id; token.generation = ++next_generation_; - entries_.emplace( - resource_id, - Entry{ - owner_id, - token.generation, - std::chrono::steady_clock::now() + ttl, - true, - false, - {}}); + + auto entry = std::make_shared(); + entry->resource_id = resource_id; + entry->owner_id = owner_id; + entry->generation = token.generation; + entry->deadline = std::chrono::steady_clock::now() + ttl; + entries_.emplace(resource_id, std::move(entry)); return {true, std::move(token), {}}; } @@ -62,47 +129,71 @@ ControlAcquireResult ControlAuthorityManager::preemptAcquire( std::lock_guard lock(mutex_); const auto existing = entries_.find(resource_id); - if (existing != entries_.end()) { - if (!expired_(existing->second) && - !existing->second.preemptible) { - ControlLeaseToken token; - token.resource_id = resource_id; - token.owner_id = owner_id; - token.generation = ++next_generation_; - existing->second.safety_holders.emplace( - token.generation, token.owner_id); - return {true, std::move(token), {}}; - } + if (existing != entries_.end() && + !existing->second->preemptible && + !existing->second->dispatch_fence_only) { + ControlLeaseToken token; + token.resource_id = resource_id; + token.owner_id = owner_id; + token.generation = ++next_generation_; + existing->second->safety_holders.emplace( + token.generation, + SafetyHolder{token.owner_id, false}); + return {true, std::move(token), {}}; } - const bool has_existing = existing != entries_.end(); const bool replacing_normal = - has_existing && existing->second.preemptible; + existing != entries_.end() && existing->second->preemptible; try { ControlLeaseToken token; token.resource_id = resource_id; token.owner_id = owner_id; token.generation = ++next_generation_; - Entry replacement{ - owner_id, - token.generation, - std::chrono::steady_clock::time_point::max(), - false, - false, - {{token.generation, owner_id}}}; - if (has_existing) { - static_assert( - std::is_nothrow_move_assignable_v, - "safety barrier replacement must not throw"); - existing->second = std::move(replacement); + std::unordered_map safety_holders; + safety_holders.emplace( + token.generation, + SafetyHolder{owner_id, false}); + std::string safety_owner = owner_id; + std::unordered_map + preempted_normal_holders; + if (replacing_normal) { + preempted_normal_holders.emplace( + existing->second->generation, + existing->second->owner_id); + } + + std::shared_ptr entry; + if (existing == entries_.end()) { + entry = std::make_shared(); + entry->resource_id = resource_id; + entry->owner_id.swap(safety_owner); + entry->generation = token.generation; + entry->deadline = + std::chrono::steady_clock::time_point::max(); + entry->preemptible = false; + entry->safety_holders.swap(safety_holders); + entry->preempted_normal_holders.swap( + preempted_normal_holders); + entries_.emplace(resource_id, entry); } else { - entries_.emplace(resource_id, std::move(replacement)); + entry = existing->second; + entry->owner_id.swap(safety_owner); + entry->generation = token.generation; + entry->deadline = + std::chrono::steady_clock::time_point::max(); + entry->preemptible = false; + entry->quarantined = false; + entry->quarantined_normal_pending = false; + entry->dispatch_fence_only = false; + entry->safety_holders.swap(safety_holders); + entry->preempted_normal_holders.swap( + preempted_normal_holders); } return {true, std::move(token), {}}; } catch (...) { - if (replacing_normal && existing->second.preemptible) { - quarantine_(existing->second); + if (replacing_normal && existing->second->preemptible) { + quarantine_(*existing->second); } throw; } @@ -121,10 +212,10 @@ ControlAcquireResult ControlAuthorityManager::preemptAcquireIfCurrent( std::lock_guard lock(mutex_); const auto existing = entries_.find(expected_token.resource_id); if (existing == entries_.end() || - expired_(existing->second) || - !existing->second.preemptible || - existing->second.owner_id != expected_token.owner_id || - existing->second.generation != expected_token.generation) { + expired_(*existing->second) || + !existing->second->preemptible || + existing->second->owner_id != expected_token.owner_id || + existing->second->generation != expected_token.generation) { return { false, {}, @@ -136,27 +227,70 @@ ControlAcquireResult ControlAuthorityManager::preemptAcquireIfCurrent( token.resource_id = expected_token.resource_id; token.owner_id = owner_id; token.generation = ++next_generation_; - Entry replacement{ - owner_id, - token.generation, - std::chrono::steady_clock::time_point::max(), - false, - false, - {{token.generation, owner_id}}}; - static_assert( - std::is_nothrow_move_assignable_v, - "safety barrier replacement must not throw"); - existing->second = std::move(replacement); + std::unordered_map safety_holders; + safety_holders.emplace( + token.generation, + SafetyHolder{owner_id, false}); + std::string safety_owner = owner_id; + std::unordered_map + preempted_normal_holders; + preempted_normal_holders.emplace( + existing->second->generation, + existing->second->owner_id); + + auto& entry = *existing->second; + entry.owner_id.swap(safety_owner); + entry.generation = token.generation; + entry.deadline = std::chrono::steady_clock::time_point::max(); + entry.preemptible = false; + entry.quarantined = false; + entry.quarantined_normal_pending = false; + entry.dispatch_fence_only = false; + entry.safety_holders.swap(safety_holders); + entry.preempted_normal_holders.swap( + preempted_normal_holders); return {true, std::move(token), {}}; } catch (...) { - if (existing->second.preemptible) { - quarantine_(existing->second); + if (existing->second->preemptible) { + quarantine_(*existing->second); } throw; } } +bool ControlAuthorityManager::waitForPreemptedRelease( + const ControlLeaseToken& safety_token, + const Duration timeout) +{ + if (!safety_token.valid() || timeout < Duration::zero()) { + return false; + } + + std::unique_lock lock(mutex_); + const auto currentState = [this, &safety_token]() { + const auto found = entries_.find(safety_token.resource_id); + if (found == entries_.end() || + !isActiveSafetyHolder_(*found->second, safety_token)) { + return -1; + } + return !found->second->quarantined_normal_pending && + found->second->preempted_normal_holders.empty() && + found->second->in_flight_dispatches == 0U + ? 1 + : 0; + }; + + if (currentState() < 0) { + return false; + } + release_cv_.wait_for( + lock, + timeout, + [¤tState]() { return currentState() != 0; }); + return currentState() == 1; +} + bool ControlAuthorityManager::quarantineIfCurrent( const ControlLeaseToken& expected_token) noexcept { @@ -165,16 +299,76 @@ bool ControlAuthorityManager::quarantineIfCurrent( } try { std::lock_guard lock(mutex_); - const auto existing = - entries_.find(expected_token.resource_id); + const auto existing = entries_.find(expected_token.resource_id); if (existing == entries_.end() || - expired_(existing->second) || - !existing->second.preemptible || - existing->second.owner_id != expected_token.owner_id || - existing->second.generation != expected_token.generation) { + expired_(*existing->second) || + !existing->second->preemptible || + existing->second->owner_id != expected_token.owner_id || + existing->second->generation != expected_token.generation) { return false; } - quarantine_(existing->second); + quarantine_(*existing->second); + return true; + } catch (...) { + return false; + } +} + +bool ControlAuthorityManager::retireSafetyHolder( + const ControlLeaseToken& safety_token) noexcept +{ + if (!safety_token.valid()) { + return false; + } + try { + std::lock_guard lock(mutex_); + const auto existing = entries_.find(safety_token.resource_id); + if (existing == entries_.end() || + existing->second->preemptible || + existing->second->dispatch_fence_only) { + return false; + } + const auto holder = existing->second->safety_holders.find( + safety_token.generation); + if (holder == existing->second->safety_holders.end() || + holder->second.owner_id != safety_token.owner_id) { + return false; + } + holder->second.retired = true; + release_cv_.notify_all(); + return true; + } catch (...) { + return false; + } +} + +bool ControlAuthorityManager::recoverRetiredSafetyHolders( + const ControlLeaseToken& recovery_token) noexcept +{ + if (!recovery_token.valid()) { + return false; + } + try { + std::lock_guard lock(mutex_); + const auto existing = entries_.find(recovery_token.resource_id); + if (existing == entries_.end() || + !isActiveSafetyHolder_(*existing->second, recovery_token) || + existing->second->quarantined_normal_pending || + !existing->second->preempted_normal_holders.empty() || + existing->second->in_flight_dispatches != 0U) { + return false; + } + + for (auto holder = existing->second->safety_holders.begin(); + holder != existing->second->safety_holders.end();) { + if (holder->second.retired) { + holder = existing->second->safety_holders.erase(holder); + } else { + ++holder; + } + } + existing->second->quarantined = false; + release_cv_.notify_all(); return true; } catch (...) { return false; @@ -190,24 +384,29 @@ bool ControlAuthorityManager::renew( } std::lock_guard lock(mutex_); const auto found = entries_.find(token.resource_id); - if (found == entries_.end() || expired_(found->second)) { - if (found != entries_.end() && expired_(found->second)) { + if (found == entries_.end()) { + return false; + } + if (expired_(*found->second)) { + if (found->second->in_flight_dispatches != 0U) { + invalidateToDispatchFence_(*found->second); + } else { entries_.erase(found); } return false; } - if (!found->second.preemptible) { - const auto holder = - found->second.safety_holders.find(token.generation); - return holder != found->second.safety_holders.end() && - holder->second == token.owner_id; + if (!found->second->preemptible) { + const auto holder = found->second->safety_holders.find( + token.generation); + return holder != found->second->safety_holders.end() && + holder->second.owner_id == token.owner_id && + !holder->second.retired; } - if (found->second.owner_id != token.owner_id || - found->second.generation != token.generation) { + if (found->second->owner_id != token.owner_id || + found->second->generation != token.generation) { return false; } - found->second.deadline = - std::chrono::steady_clock::now() + ttl; + found->second->deadline = std::chrono::steady_clock::now() + ttl; return true; } @@ -222,18 +421,51 @@ bool ControlAuthorityManager::validate( if (found == entries_.end()) { return false; } - if (expired_(found->second)) { - entries_.erase(found); + if (expired_(*found->second)) { + if (found->second->in_flight_dispatches != 0U) { + invalidateToDispatchFence_(*found->second); + } else { + entries_.erase(found); + } return false; } - if (!found->second.preemptible) { - const auto holder = - found->second.safety_holders.find(token.generation); - return holder != found->second.safety_holders.end() && - holder->second == token.owner_id; + if (!found->second->preemptible) { + const auto holder = found->second->safety_holders.find( + token.generation); + return holder != found->second->safety_holders.end() && + holder->second.owner_id == token.owner_id && + !holder->second.retired; } - return found->second.owner_id == token.owner_id && - found->second.generation == token.generation; + return found->second->owner_id == token.owner_id && + found->second->generation == token.generation; +} + +ControlDispatchGuard ControlAuthorityManager::tryBeginDispatch( + const ControlLeaseToken& token) +{ + if (!token.valid()) { + return {}; + } + std::lock_guard lock(mutex_); + const auto found = entries_.find(token.resource_id); + if (found == entries_.end()) { + return {}; + } + if (expired_(*found->second)) { + if (found->second->in_flight_dispatches != 0U) { + invalidateToDispatchFence_(*found->second); + } else { + entries_.erase(found); + } + return {}; + } + if (!found->second->preemptible || + found->second->owner_id != token.owner_id || + found->second->generation != token.generation) { + return {}; + } + ++found->second->in_flight_dispatches; + return ControlDispatchGuard(this, found->second); } void ControlAuthorityManager::release( @@ -248,21 +480,54 @@ void ControlAuthorityManager::release( if (found == entries_.end()) { return; } - if (!found->second.preemptible) { - const auto holder = - found->second.safety_holders.find(token.generation); - if (holder == found->second.safety_holders.end() || - holder->second != token.owner_id) { + auto& entry = *found->second; + if (!entry.preemptible) { + if (entry.dispatch_fence_only) { return; } - found->second.safety_holders.erase(holder); - if (found->second.safety_holders.empty() && - !found->second.quarantined) { + const auto holder = entry.safety_holders.find(token.generation); + if (holder != entry.safety_holders.end() && + holder->second.owner_id == token.owner_id) { + if (holder->second.retired) { + return; + } + entry.safety_holders.erase(holder); + if (canErase_(entry)) { + entries_.erase(found); + } + release_cv_.notify_all(); + return; + } + + const auto preempted = entry.preempted_normal_holders.find( + token.generation); + if (preempted != entry.preempted_normal_holders.end() && + preempted->second == token.owner_id) { + entry.preempted_normal_holders.erase(preempted); + if (canErase_(entry)) { + entries_.erase(found); + } + release_cv_.notify_all(); + return; + } + + if (entry.quarantined_normal_pending && + entry.owner_id == token.owner_id && + entry.generation == token.generation) { + entry.quarantined_normal_pending = false; + if (canErase_(entry)) { + entries_.erase(found); + } + release_cv_.notify_all(); + } + } else if (entry.owner_id == token.owner_id && + entry.generation == token.generation) { + if (entry.in_flight_dispatches != 0U) { + invalidateToDispatchFence_(entry); + } else { entries_.erase(found); } - } else if (found->second.owner_id == token.owner_id && - found->second.generation == token.generation) { - entries_.erase(found); + release_cv_.notify_all(); } } catch (...) { } @@ -279,7 +544,11 @@ bool ControlAuthorityManager::isLeased( if (found == entries_.end()) { return false; } - if (expired_(found->second)) { + if (expired_(*found->second)) { + if (found->second->in_flight_dispatches != 0U) { + invalidateToDispatchFence_(*found->second); + return true; + } entries_.erase(found); return false; } @@ -291,7 +560,16 @@ void ControlAuthorityManager::revoke( { try { std::lock_guard lock(mutex_); - entries_.erase(resource_id); + const auto found = entries_.find(resource_id); + if (found == entries_.end()) { + return; + } + if (found->second->in_flight_dispatches != 0U) { + invalidateToDispatchFence_(*found->second); + } else { + entries_.erase(found); + } + release_cv_.notify_all(); } catch (...) { } } @@ -300,24 +578,105 @@ void ControlAuthorityManager::clear() noexcept { try { std::lock_guard lock(mutex_); - entries_.clear(); + for (auto entry = entries_.begin(); entry != entries_.end();) { + if (entry->second->in_flight_dispatches != 0U) { + invalidateToDispatchFence_(*entry->second); + ++entry; + } else { + entry = entries_.erase(entry); + } + } + release_cv_.notify_all(); } catch (...) { } } -bool ControlAuthorityManager::expired_( - const Entry& entry) const noexcept +bool ControlAuthorityManager::expired_(const Entry& entry) const noexcept { return !entry.quarantined && std::chrono::steady_clock::now() >= entry.deadline; } +bool ControlAuthorityManager::isSafetyHolder_( + const Entry& entry, + const ControlLeaseToken& token) noexcept +{ + if (entry.preemptible || entry.dispatch_fence_only) { + return false; + } + const auto holder = entry.safety_holders.find(token.generation); + return holder != entry.safety_holders.end() && + holder->second.owner_id == token.owner_id; +} + +bool ControlAuthorityManager::isActiveSafetyHolder_( + const Entry& entry, + const ControlLeaseToken& token) noexcept +{ + if (entry.preemptible || entry.dispatch_fence_only) { + return false; + } + const auto holder = entry.safety_holders.find(token.generation); + return holder != entry.safety_holders.end() && + holder->second.owner_id == token.owner_id && + !holder->second.retired; +} + +bool ControlAuthorityManager::canErase_(const Entry& entry) noexcept +{ + if (entry.in_flight_dispatches != 0U) { + return false; + } + if (entry.dispatch_fence_only) { + return true; + } + return !entry.preemptible && + entry.safety_holders.empty() && + entry.preempted_normal_holders.empty() && + !entry.quarantined_normal_pending && + !entry.quarantined; +} + +void ControlAuthorityManager::invalidateToDispatchFence_( + Entry& entry) noexcept +{ + entry.owner_id.clear(); + entry.generation = 0U; + entry.deadline = std::chrono::steady_clock::time_point::max(); + entry.preemptible = false; + entry.quarantined = false; + entry.quarantined_normal_pending = false; + entry.dispatch_fence_only = true; + entry.safety_holders.clear(); + entry.preempted_normal_holders.clear(); +} + +void ControlAuthorityManager::endDispatch_( + const std::shared_ptr& entry) noexcept +{ + try { + std::lock_guard lock(mutex_); + if (entry->in_flight_dispatches == 0U) { + return; + } + --entry->in_flight_dispatches; + const auto found = entries_.find(entry->resource_id); + if (found != entries_.end() && found->second == entry && + canErase_(*entry)) { + entries_.erase(found); + } + release_cv_.notify_all(); + } catch (...) { + } +} + void ControlAuthorityManager::quarantine_(Entry& entry) noexcept { - entry.deadline = - std::chrono::steady_clock::time_point::max(); + entry.quarantined_normal_pending = entry.preemptible; + entry.deadline = std::chrono::steady_clock::time_point::max(); entry.preemptible = false; entry.quarantined = true; + entry.dispatch_fence_only = false; } } // namespace cmvr::control diff --git a/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp b/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp index c95cef21..02d92671 100644 --- a/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp +++ b/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp @@ -1,6 +1,7 @@ #include "manager/control_authority/include/control_authority_manager.h" #include +#include #include #include @@ -93,6 +94,536 @@ TEST_F(ControlAuthorityManagerTest, EXPECT_FALSE(manager.isLeased("right_arm")); } +TEST_F(ControlAuthorityManagerTest, + SafetyPreemptionReturnsWhileDispatchIsInFlightAndWaitsForBoth) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 1s); + ASSERT_TRUE(control.acquired); + + auto dispatch = manager.tryBeginDispatch(control.token); + ASSERT_TRUE(dispatch.acquired()); + auto pending_barrier = std::async( + std::launch::async, + [&manager]() { + return manager.preemptAcquire( + "right_arm", "stop-operation", 1s); + }); + + const auto preempt_status = pending_barrier.wait_for(100ms); + if (preempt_status != std::future_status::ready) { + dispatch = {}; + const auto cleanup_barrier = pending_barrier.get(); + if (cleanup_barrier.acquired) { + manager.release(cleanup_barrier.token); + } + FAIL() << "safety preemption waited for an in-flight dispatch"; + return; + } + + const auto acquired_barrier = pending_barrier.get(); + ASSERT_TRUE(acquired_barrier.acquired) + << acquired_barrier.detail; + EXPECT_FALSE(manager.validate(control.token)); + EXPECT_FALSE(manager.tryBeginDispatch(control.token).acquired()); + + EXPECT_FALSE(manager.waitForPreemptedRelease( + acquired_barrier.token, 10ms)); + manager.release(control.token); + EXPECT_FALSE(manager.waitForPreemptedRelease( + acquired_barrier.token, 10ms)); + + dispatch = {}; + EXPECT_TRUE(manager.waitForPreemptedRelease( + acquired_barrier.token, 10ms)); + manager.release(acquired_barrier.token); +} + +TEST_F(ControlAuthorityManagerTest, + DispatchOnOneResourceDoesNotDelaySafetyPreemptionOnAnother) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto right_control = + manager.tryAcquire("right_arm", "right-move", 1s); + const auto left_control = + manager.tryAcquire("left_arm", "left-move", 1s); + ASSERT_TRUE(right_control.acquired); + ASSERT_TRUE(left_control.acquired); + + auto right_dispatch = manager.tryBeginDispatch(right_control.token); + ASSERT_TRUE(right_dispatch.acquired()); + auto pending_left_barrier = std::async( + std::launch::async, + [&manager]() { + return manager.preemptAcquire( + "left_arm", "left-stop", 1s); + }); + + const auto preempt_status = pending_left_barrier.wait_for(100ms); + if (preempt_status != std::future_status::ready) { + right_dispatch = {}; + const auto cleanup_barrier = pending_left_barrier.get(); + if (cleanup_barrier.acquired) { + manager.release(cleanup_barrier.token); + } + FAIL() << "one resource's dispatch blocked another resource's stop"; + return; + } + + const auto left_barrier = pending_left_barrier.get(); + ASSERT_TRUE(left_barrier.acquired) << left_barrier.detail; + manager.release(left_control.token); + EXPECT_TRUE(manager.waitForPreemptedRelease( + left_barrier.token, 0ms)); + manager.release(left_barrier.token); + + EXPECT_TRUE(right_dispatch.acquired()); + right_dispatch = {}; + manager.release(right_control.token); +} + +TEST_F(ControlAuthorityManagerTest, + DispatchGuardDestructorReleasesTheInFlightFence) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 1s); + ASSERT_TRUE(control.acquired); + + ControlAcquireResult barrier; + { + auto dispatch = manager.tryBeginDispatch(control.token); + ASSERT_TRUE(dispatch.acquired()); + barrier = manager.preemptAcquire( + "right_arm", "stop-operation", 1s); + ASSERT_TRUE(barrier.acquired) << barrier.detail; + manager.release(control.token); + EXPECT_FALSE(manager.waitForPreemptedRelease( + barrier.token, 0ms)); + } + + EXPECT_TRUE(manager.waitForPreemptedRelease(barrier.token, 0ms)); + manager.release(barrier.token); +} + +TEST_F(ControlAuthorityManagerTest, + DispatchGuardMoveAssignmentReleasesOnlyItsPreviousFence) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto right_control = + manager.tryAcquire("right_arm", "right-move", 1s); + const auto left_control = + manager.tryAcquire("left_arm", "left-move", 1s); + ASSERT_TRUE(right_control.acquired); + ASSERT_TRUE(left_control.acquired); + + auto right_dispatch = manager.tryBeginDispatch(right_control.token); + auto left_dispatch = manager.tryBeginDispatch(left_control.token); + ASSERT_TRUE(right_dispatch.acquired()); + ASSERT_TRUE(left_dispatch.acquired()); + const auto right_barrier = manager.preemptAcquire( + "right_arm", "right-stop", 1s); + const auto left_barrier = manager.preemptAcquire( + "left_arm", "left-stop", 1s); + ASSERT_TRUE(right_barrier.acquired) << right_barrier.detail; + ASSERT_TRUE(left_barrier.acquired) << left_barrier.detail; + manager.release(right_control.token); + manager.release(left_control.token); + + right_dispatch = std::move(left_dispatch); + EXPECT_TRUE(right_dispatch.acquired()); + EXPECT_FALSE(left_dispatch.acquired()); + EXPECT_TRUE(manager.waitForPreemptedRelease( + right_barrier.token, 0ms)); + EXPECT_FALSE(manager.waitForPreemptedRelease( + left_barrier.token, 0ms)); + + right_dispatch = {}; + EXPECT_TRUE(manager.waitForPreemptedRelease( + left_barrier.token, 0ms)); + manager.release(right_barrier.token); + manager.release(left_barrier.token); +} + +TEST_F(ControlAuthorityManagerTest, + RevokeKeepsAnInFlightDispatchFencedFromNormalSuccessors) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 1s); + ASSERT_TRUE(control.acquired); + auto dispatch = manager.tryBeginDispatch(control.token); + ASSERT_TRUE(dispatch.acquired()); + + manager.revoke("right_arm"); + EXPECT_FALSE(manager.validate(control.token)); + EXPECT_TRUE(manager.isLeased("right_arm")); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "successor", 1s).acquired); + manager.release(control.token); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "successor", 1s).acquired); + + dispatch = {}; + const auto successor = + manager.tryAcquire("right_arm", "successor", 1s); + ASSERT_TRUE(successor.acquired) << successor.detail; + manager.release(successor.token); +} + +TEST_F(ControlAuthorityManagerTest, + ClearKeepsInFlightDispatchesWhileErasingIdleEntries) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto active = + manager.tryAcquire("right_arm", "move-session", 1s); + ASSERT_TRUE(active.acquired); + ASSERT_TRUE(manager.tryAcquire("idle_arm", "idle-session", 1s).acquired); + auto dispatch = manager.tryBeginDispatch(active.token); + ASSERT_TRUE(dispatch.acquired()); + + manager.clear(); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "successor", 1s).acquired); + const auto idle_successor = + manager.tryAcquire("idle_arm", "idle-successor", 1s); + ASSERT_TRUE(idle_successor.acquired) << idle_successor.detail; + manager.release(idle_successor.token); + + dispatch = {}; + const auto active_successor = + manager.tryAcquire("right_arm", "successor", 1s); + ASSERT_TRUE(active_successor.acquired) << active_successor.detail; + manager.release(active_successor.token); +} + +TEST_F(ControlAuthorityManagerTest, + ExpiredLeaseKeepsInFlightDispatchFencedFromNormalSuccessors) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 10ms); + ASSERT_TRUE(control.acquired); + auto dispatch = manager.tryBeginDispatch(control.token); + ASSERT_TRUE(dispatch.acquired()); + std::this_thread::sleep_for(20ms); + + EXPECT_FALSE(manager.validate(control.token)); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "successor", 1s).acquired); + manager.release(control.token); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "successor", 1s).acquired); + + dispatch = {}; + const auto successor = + manager.tryAcquire("right_arm", "successor", 1s); + ASSERT_TRUE(successor.acquired) << successor.detail; + manager.release(successor.token); +} + +TEST_F(ControlAuthorityManagerTest, + DispatchGuardSurvivesAuthorityMapRehash) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 1s); + ASSERT_TRUE(control.acquired); + auto dispatch = manager.tryBeginDispatch(control.token); + ASSERT_TRUE(dispatch.acquired()); + + for (int index = 0; index < 512; ++index) { + ASSERT_TRUE(manager.tryAcquire( + "rehash-resource-" + std::to_string(index), + "rehash-owner", + 1s).acquired); + } + + const auto barrier = manager.preemptAcquire( + "right_arm", "stop-operation", 1s); + ASSERT_TRUE(barrier.acquired) << barrier.detail; + manager.release(control.token); + EXPECT_FALSE(manager.waitForPreemptedRelease(barrier.token, 0ms)); + dispatch = {}; + EXPECT_TRUE(manager.waitForPreemptedRelease(barrier.token, 0ms)); + manager.release(barrier.token); +} + +TEST_F(ControlAuthorityManagerTest, + StaleLeaseCannotBeginDispatchAfterSafetyPreemption) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 1s); + ASSERT_TRUE(control.acquired); + const auto barrier = manager.preemptAcquire( + "right_arm", "stop-operation", 1s); + ASSERT_TRUE(barrier.acquired) << barrier.detail; + + EXPECT_FALSE(manager.tryBeginDispatch(control.token).acquired()); + manager.release(barrier.token); +} + +TEST_F(ControlAuthorityManagerTest, + ReleasedLastSafetyBarrierKeepsPreemptedHandlerFenced) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 1s); + ASSERT_TRUE(control.acquired); + const auto barrier = manager.preemptAcquire( + "right_arm", "stop-operation", 1s); + ASSERT_TRUE(barrier.acquired) << barrier.detail; + + manager.release(barrier.token); + EXPECT_TRUE(manager.isLeased("right_arm")); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "new-move", 1s).acquired); + + manager.release(control.token); + EXPECT_FALSE(manager.isLeased("right_arm")); + EXPECT_TRUE( + manager.tryAcquire("right_arm", "new-move", 1s).acquired); +} + +TEST_F(ControlAuthorityManagerTest, + SafetyBarrierWaitsForPreemptedNormalLeaseRelease) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 1s); + ASSERT_TRUE(control.acquired); + const auto barrier = manager.preemptAcquire( + "right_arm", "stop-operation", 1s); + ASSERT_TRUE(barrier.acquired) << barrier.detail; + + auto wait_result = std::async( + std::launch::async, + [&manager, token = barrier.token]() { + return manager.waitForPreemptedRelease(token, 1s); + }); + EXPECT_EQ( + wait_result.wait_for(30ms), + std::future_status::timeout); + + manager.release(control.token); + EXPECT_TRUE(wait_result.get()); + EXPECT_TRUE(manager.validate(barrier.token)); + manager.release(barrier.token); +} + +TEST_F(ControlAuthorityManagerTest, + ConditionalSafetyBarrierTracksPreemptedNormalLeaseRelease) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 1s); + ASSERT_TRUE(control.acquired); + auto dispatch = manager.tryBeginDispatch(control.token); + ASSERT_TRUE(dispatch.acquired()); + const auto barrier = manager.preemptAcquireIfCurrent( + control.token, "timed-out-action", 1s); + ASSERT_TRUE(barrier.acquired) << barrier.detail; + + EXPECT_FALSE( + manager.waitForPreemptedRelease(barrier.token, 10ms)); + dispatch = {}; + EXPECT_FALSE( + manager.waitForPreemptedRelease(barrier.token, 10ms)); + manager.release(control.token); + EXPECT_TRUE( + manager.waitForPreemptedRelease(barrier.token, 10ms)); + manager.release(barrier.token); +} + +TEST_F(ControlAuthorityManagerTest, + ReleasedSafetyHolderStopsWaitingWithoutAffectingOtherHolder) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 1s); + ASSERT_TRUE(control.acquired); + const auto first_barrier = manager.preemptAcquire( + "right_arm", "first-stop", 1s); + ASSERT_TRUE(first_barrier.acquired) << first_barrier.detail; + const auto second_barrier = manager.preemptAcquire( + "right_arm", "second-stop", 1s); + ASSERT_TRUE(second_barrier.acquired) << second_barrier.detail; + + auto first_wait = std::async( + std::launch::async, + [&manager, token = first_barrier.token]() { + return manager.waitForPreemptedRelease(token, 1s); + }); + auto second_wait = std::async( + std::launch::async, + [&manager, token = second_barrier.token]() { + return manager.waitForPreemptedRelease(token, 1s); + }); + EXPECT_EQ(first_wait.wait_for(30ms), std::future_status::timeout); + EXPECT_EQ(second_wait.wait_for(30ms), std::future_status::timeout); + + manager.release(first_barrier.token); + EXPECT_FALSE(first_wait.get()); + EXPECT_EQ(second_wait.wait_for(30ms), std::future_status::timeout); + manager.release(control.token); + EXPECT_TRUE(second_wait.get()); + manager.release(second_barrier.token); +} + +TEST_F(ControlAuthorityManagerTest, + PreemptedReleaseRequiresExactNormalLeaseToken) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 1s); + ASSERT_TRUE(control.acquired); + const auto barrier = manager.preemptAcquire( + "right_arm", "stop-operation", 1s); + ASSERT_TRUE(barrier.acquired) << barrier.detail; + + auto forged = control.token; + forged.owner_id = "different-owner"; + manager.release(forged); + EXPECT_FALSE( + manager.waitForPreemptedRelease(barrier.token, 10ms)); + + manager.release(control.token); + EXPECT_TRUE( + manager.waitForPreemptedRelease(barrier.token, 0ms)); + manager.release(barrier.token); +} + +TEST_F(ControlAuthorityManagerTest, + ClearWakesPreemptedReleaseWaiterAndInvalidatesSafetyHolder) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 1s); + ASSERT_TRUE(control.acquired); + const auto barrier = manager.preemptAcquire( + "right_arm", "stop-operation", 1s); + ASSERT_TRUE(barrier.acquired) << barrier.detail; + + auto wait_result = std::async( + std::launch::async, + [&manager, token = barrier.token]() { + return manager.waitForPreemptedRelease(token, 1s); + }); + EXPECT_EQ( + wait_result.wait_for(30ms), + std::future_status::timeout); + + manager.clear(); + EXPECT_FALSE(wait_result.get()); + manager.release(control.token); +} + +TEST_F(ControlAuthorityManagerTest, + RevokeWakesPreemptedReleaseWaiterAndInvalidatesSafetyHolder) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 1s); + ASSERT_TRUE(control.acquired); + const auto barrier = manager.preemptAcquire( + "right_arm", "stop-operation", 1s); + ASSERT_TRUE(barrier.acquired) << barrier.detail; + + auto wait_result = std::async( + std::launch::async, + [&manager, token = barrier.token]() { + return manager.waitForPreemptedRelease(token, 1s); + }); + EXPECT_EQ( + wait_result.wait_for(30ms), + std::future_status::timeout); + + manager.revoke("right_arm"); + EXPECT_FALSE(wait_result.get()); + manager.release(control.token); +} + +TEST_F(ControlAuthorityManagerTest, + ExpiredNormalLeaseStillRequiresHandlerReleaseAfterPreemption) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 10ms); + ASSERT_TRUE(control.acquired); + std::this_thread::sleep_for(20ms); + + const auto barrier = manager.preemptAcquire( + "right_arm", "stop-operation", 1s); + ASSERT_TRUE(barrier.acquired) << barrier.detail; + EXPECT_FALSE( + manager.waitForPreemptedRelease(barrier.token, 10ms)); + + manager.release(control.token); + EXPECT_TRUE( + manager.waitForPreemptedRelease(barrier.token, 0ms)); + manager.release(barrier.token); +} + +TEST_F(ControlAuthorityManagerTest, + QuarantinedFallbackTracksOriginalNormalHandlerRelease) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 1s); + ASSERT_TRUE(control.acquired); + ASSERT_TRUE(manager.quarantineIfCurrent(control.token)); + + const auto barrier = manager.preemptAcquire( + "right_arm", "stop-operation", 1s); + ASSERT_TRUE(barrier.acquired) << barrier.detail; + EXPECT_FALSE( + manager.waitForPreemptedRelease(barrier.token, 10ms)); + + manager.release(control.token); + EXPECT_TRUE( + manager.waitForPreemptedRelease(barrier.token, 0ms)); + manager.release(barrier.token); + EXPECT_TRUE(manager.isLeased("right_arm")); + + manager.revoke("right_arm"); +} + +TEST_F(ControlAuthorityManagerTest, + WaitRejectsStaleSafetyTokenForSuccessorEntry) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto old_control = + manager.tryAcquire("right_arm", "old-move", 1s); + ASSERT_TRUE(old_control.acquired); + const auto old_barrier = manager.preemptAcquire( + "right_arm", "old-stop", 1s); + ASSERT_TRUE(old_barrier.acquired) << old_barrier.detail; + manager.release(old_barrier.token); + + EXPECT_FALSE( + manager.tryAcquire("right_arm", "new-move", 1s).acquired); + manager.release(old_control.token); + + const auto successor = + manager.tryAcquire("right_arm", "new-move", 1s); + ASSERT_TRUE(successor.acquired); + const auto current_barrier = manager.preemptAcquire( + "right_arm", "new-stop", 1s); + ASSERT_TRUE(current_barrier.acquired) << current_barrier.detail; + + EXPECT_FALSE(manager.waitForPreemptedRelease( + old_barrier.token, 0ms)); + EXPECT_FALSE(manager.waitForPreemptedRelease( + current_barrier.token, 10ms)); + + manager.release(successor.token); + EXPECT_TRUE(manager.waitForPreemptedRelease( + current_barrier.token, 0ms)); + manager.release(current_barrier.token); +} + TEST_F(ControlAuthorityManagerTest, ConditionalSafetyBarrierPreemptsMatchingCurrentLease) { @@ -127,6 +658,9 @@ TEST_F(ControlAuthorityManagerTest, "right_arm", "direct-stop", 100ms); ASSERT_TRUE(direct_stop.acquired) << direct_stop.detail; manager.release(direct_stop.token); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "move-session", 100ms).acquired); + manager.release(old.token); const auto successor = manager.tryAcquire("right_arm", "move-session", 100ms); ASSERT_TRUE(successor.acquired); @@ -163,6 +697,8 @@ TEST_F(ControlAuthorityManagerTest, EXPECT_TRUE(manager.validate(existing_barrier.token)); manager.release(existing_barrier.token); + EXPECT_TRUE(manager.isLeased("right_arm")); + manager.release(control.token); EXPECT_FALSE(manager.isLeased("right_arm")); EXPECT_TRUE( manager.tryAcquire("right_arm", "new-move", 100ms) @@ -226,6 +762,137 @@ TEST_F(ControlAuthorityManagerTest, EXPECT_FALSE(manager.isLeased("right_arm")); } +TEST_F(ControlAuthorityManagerTest, + RetiredSafetyHolderStaysFailClosedUntilConfirmedRecovery) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 1s); + ASSERT_TRUE(control.acquired); + const auto failed_stop = manager.preemptAcquire( + "right_arm", "failed-stop", 1s); + ASSERT_TRUE(failed_stop.acquired) << failed_stop.detail; + + manager.release(control.token); + ASSERT_TRUE(manager.waitForPreemptedRelease( + failed_stop.token, 0ms)); + ASSERT_TRUE(manager.retireSafetyHolder(failed_stop.token)); + EXPECT_FALSE(manager.validate(failed_stop.token)); + + // A delayed destructor release cannot undo the retained fail-closed state. + manager.release(failed_stop.token); + EXPECT_TRUE(manager.isLeased("right_arm")); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "new-move", 1s).acquired); + + const auto recovery = manager.preemptAcquire( + "right_arm", "confirmed-recovery", 1s); + ASSERT_TRUE(recovery.acquired) << recovery.detail; + ASSERT_TRUE(manager.waitForPreemptedRelease(recovery.token, 0ms)); + ASSERT_TRUE(manager.recoverRetiredSafetyHolders(recovery.token)); + EXPECT_TRUE(manager.validate(recovery.token)); + + manager.release(recovery.token); + EXPECT_FALSE(manager.isLeased("right_arm")); +} + +TEST_F(ControlAuthorityManagerTest, + RecoveryWaitsForPreemptedNormalHandlerToRelease) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 1s); + ASSERT_TRUE(control.acquired); + const auto failed_stop = manager.preemptAcquire( + "right_arm", "failed-stop", 1s); + ASSERT_TRUE(failed_stop.acquired) << failed_stop.detail; + ASSERT_TRUE(manager.retireSafetyHolder(failed_stop.token)); + + const auto recovery = manager.preemptAcquire( + "right_arm", "confirmed-recovery", 1s); + ASSERT_TRUE(recovery.acquired) << recovery.detail; + EXPECT_FALSE(manager.recoverRetiredSafetyHolders(recovery.token)); + EXPECT_FALSE(manager.waitForPreemptedRelease(recovery.token, 10ms)); + + manager.release(control.token); + ASSERT_TRUE(manager.waitForPreemptedRelease(recovery.token, 0ms)); + ASSERT_TRUE(manager.recoverRetiredSafetyHolders(recovery.token)); + manager.release(recovery.token); + EXPECT_FALSE(manager.isLeased("right_arm")); +} + +TEST_F(ControlAuthorityManagerTest, + RecoveryPreservesEveryOtherActiveSafetyHolder) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto retired = manager.preemptAcquire( + "right_arm", "failed-stop", 1s); + ASSERT_TRUE(retired.acquired) << retired.detail; + ASSERT_TRUE(manager.retireSafetyHolder(retired.token)); + + const auto independent_stop = manager.preemptAcquire( + "right_arm", "independent-stop", 1s); + const auto recovery = manager.preemptAcquire( + "right_arm", "confirmed-recovery", 1s); + ASSERT_TRUE(independent_stop.acquired) << independent_stop.detail; + ASSERT_TRUE(recovery.acquired) << recovery.detail; + + ASSERT_TRUE(manager.recoverRetiredSafetyHolders(recovery.token)); + EXPECT_TRUE(manager.validate(independent_stop.token)); + EXPECT_TRUE(manager.validate(recovery.token)); + EXPECT_FALSE(manager.validate(retired.token)); + + manager.release(recovery.token); + EXPECT_TRUE(manager.isLeased("right_arm")); + EXPECT_TRUE(manager.validate(independent_stop.token)); + manager.release(independent_stop.token); + EXPECT_FALSE(manager.isLeased("right_arm")); +} + +TEST_F(ControlAuthorityManagerTest, + RetiredOrForgedSafetyTokenCannotAuthorizeRecovery) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto retired = manager.preemptAcquire( + "right_arm", "failed-stop", 1s); + ASSERT_TRUE(retired.acquired) << retired.detail; + ASSERT_TRUE(manager.retireSafetyHolder(retired.token)); + EXPECT_FALSE(manager.recoverRetiredSafetyHolders(retired.token)); + + const auto recovery = manager.preemptAcquire( + "right_arm", "confirmed-recovery", 1s); + ASSERT_TRUE(recovery.acquired) << recovery.detail; + auto forged = recovery.token; + forged.owner_id = "different-owner"; + EXPECT_FALSE(manager.recoverRetiredSafetyHolders(forged)); + EXPECT_TRUE(manager.isLeased("right_arm")); + + ASSERT_TRUE(manager.recoverRetiredSafetyHolders(recovery.token)); + manager.release(recovery.token); + EXPECT_FALSE(manager.isLeased("right_arm")); +} + +TEST_F(ControlAuthorityManagerTest, + ConfirmedRecoveryCanClearTokenlessQuarantine) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 1s); + ASSERT_TRUE(control.acquired); + ASSERT_TRUE(manager.quarantineIfCurrent(control.token)); + + const auto recovery = manager.preemptAcquire( + "right_arm", "confirmed-recovery", 1s); + ASSERT_TRUE(recovery.acquired) << recovery.detail; + EXPECT_FALSE(manager.recoverRetiredSafetyHolders(recovery.token)); + + manager.release(control.token); + ASSERT_TRUE(manager.waitForPreemptedRelease(recovery.token, 0ms)); + ASSERT_TRUE(manager.recoverRetiredSafetyHolders(recovery.token)); + manager.release(recovery.token); + EXPECT_FALSE(manager.isLeased("right_arm")); +} + TEST_F(ControlAuthorityManagerTest, ExpiryAndRenewUseMonotonicLocalTime) { auto& manager = ControlAuthorityManager::instance(); diff --git a/cmvr-es/manager/device_manager/include/device_manager.h b/cmvr-es/manager/device_manager/include/device_manager.h index 8d1087cc..259e4429 100644 --- a/cmvr-es/manager/device_manager/include/device_manager.h +++ b/cmvr-es/manager/device_manager/include/device_manager.h @@ -18,6 +18,12 @@ namespace cmvr::device { + struct DeviceInventoryEntry { + std::string id; + DeviceKind kind = DeviceKind::Unknown; + std::shared_ptr device; + }; + class DeviceManager { public: DeviceManager(const DeviceManager&) = delete; @@ -36,6 +42,9 @@ namespace cmvr::device { void registerDevice(const std::shared_ptr& device); void registerDevice(const std::string& device_id, const std::shared_ptr& device); std::shared_ptr getDeviceBase(const std::string& device_id); + // Copies only manager-owned metadata and shared ownership. No device + // methods are called, so a blocked driver cannot delay this snapshot. + std::vector inventorySnapshot() const; DeviceManagerSnapshot snapshot() const; std::string version() const; diff --git a/cmvr-es/manager/device_manager/src/device_manager.cpp b/cmvr-es/manager/device_manager/src/device_manager.cpp index 123797f9..6856d03e 100644 --- a/cmvr-es/manager/device_manager/src/device_manager.cpp +++ b/cmvr-es/manager/device_manager/src/device_manager.cpp @@ -377,6 +377,24 @@ std::shared_ptr DeviceManager::getDeviceBase(const std::string& return it->second.device; } +std::vector DeviceManager::inventorySnapshot() const +{ + std::vector result; + { + std::shared_lock lock(devices_mutex_); + result.reserve(devices_.size()); + for (const auto& [id, record] : devices_) { + result.push_back({id, record.kind, record.device}); + } + } + + std::sort(result.begin(), result.end(), + [](const auto& lhs, const auto& rhs) { + return lhs.id < rhs.id; + }); + return result; +} + void DeviceManager::getDeviceList(std::list>& device_list){ device_list.clear(); std::shared_lock lock(devices_mutex_); 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 a5191824..e7c5f4e9 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 @@ -5,8 +5,12 @@ #include "devices/microphone/abstract_microphone.h" #include +#include +#include #include +#include #include +#include #include #include #include @@ -23,6 +27,7 @@ namespace { using cmvr::device::AbstractDevice; using cmvr::device::DeviceHealthSnapshot; using cmvr::device::DeviceHealthState; +using cmvr::device::DeviceInventoryEntry; using cmvr::device::DeviceKind; using cmvr::device::DeviceManager; using cmvr::device::DeviceManagerSnapshot; @@ -123,6 +128,51 @@ public: std::atomic health_calls{0}; }; +class BlockingHealthDevice final : public AbstractDevice { +public: + explicit BlockingHealthDevice(std::string id) + : AbstractDevice(std::move(id)) + { + } + + DeviceKind kind() const noexcept override { return DeviceKind::Arm; } + std::string typeName() const override { return "BlockingHealthDevice"; } + + DeviceHealthSnapshot healthSnapshot() override + { + std::unique_lock lock(mutex_); + ++health_calls; + health_entered_ = true; + condition_.notify_all(); + condition_.wait(lock, [this] { return release_health_; }); + return {DeviceHealthState::Healthy, {}}; + } + + bool waitForHealthCall(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return condition_.wait_for( + lock, timeout, [this] { return health_entered_; }); + } + + void releaseHealthCall() + { + { + std::lock_guard lock(mutex_); + release_health_ = true; + } + condition_.notify_all(); + } + + std::atomic health_calls{0}; + +private: + std::mutex mutex_; + std::condition_variable condition_; + bool health_entered_{false}; + bool release_health_{false}; +}; + const ManagedDeviceSnapshot* findDevice(const DeviceManagerSnapshot& snapshot, const std::string& id) { @@ -144,6 +194,16 @@ bool isSorted(const DeviceManagerSnapshot& snapshot) return true; } +bool isSorted(const std::vector& inventory) +{ + for (std::size_t i = 1; i < inventory.size(); ++i) { + if (inventory[i].id < inventory[i - 1].id) { + return false; + } + } + return true; +} + bool testCategoryHealthAdapters() { MemoryCamera camera; @@ -374,6 +434,54 @@ bool testConcurrentSnapshotAndRegistration() return true; } +bool testInventorySnapshotDoesNotWaitForDeviceHealth() +{ + DeviceManager::destroyInstance(); + cmvr::config::DeviceManagerConfig config; + auto& manager = DeviceManager::getInstance(config); + auto blocking_device = + 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(); + }); + if (!blocking_device->waitForHealthCall(std::chrono::seconds(2))) { + blocking_device->releaseHealthCall(); + health_future.wait(); + return false; + } + + auto inventory_future = std::async(std::launch::async, [&manager] { + return manager.inventorySnapshot(); + }); + if (inventory_future.wait_for(std::chrono::milliseconds(250)) != + std::future_status::ready) { + blocking_device->releaseHealthCall(); + inventory_future.wait(); + health_future.wait(); + return false; + } + + const auto inventory = inventory_future.get(); + const bool inventory_valid = + inventory.size() == 2 && isSorted(inventory) && + inventory[0].id == "a_camera" && + inventory[0].kind == DeviceKind::Camera && + inventory[0].device == other_device && + inventory[1].id == "blocked_health_arm" && + inventory[1].kind == DeviceKind::Arm && + inventory[1].device == blocking_device && + blocking_device->health_calls.load() == 1; + + blocking_device->releaseHealthCall(); + health_future.get(); + return inventory_valid && blocking_device->health_calls.load() == 1; +} + } // namespace int main() @@ -382,7 +490,8 @@ int main() const bool success = testCategoryHealthAdapters() && testConfiguredAndDynamicSnapshots() && - testConcurrentSnapshotAndRegistration(); + testConcurrentSnapshotAndRegistration() && + testInventorySnapshotDoesNotWaitForDeviceHealth(); 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 6a3634fd..b7fb4487 100644 --- a/cmvr-es/manager/media_source_hub/CMakeLists.txt +++ b/cmvr-es/manager/media_source_hub/CMakeLists.txt @@ -2,6 +2,10 @@ if(CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR) cmake_minimum_required(VERSION 3.22) project(cmvr_media_source_hub LANGUAGES CXX) enable_testing() + add_subdirectory( + ${CMAKE_CURRENT_SOURCE_DIR}/../../service/stop_all + ${CMAKE_CURRENT_BINARY_DIR}/stop_all + ) endif() add_library(media_source_hub STATIC @@ -13,6 +17,10 @@ target_include_directories(media_source_hub PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../.. ) +target_link_libraries(media_source_hub + PUBLIC + cmvr_es::stop_all_admission_gate +) add_library(cmvr_es::media_source_hub ALIAS media_source_hub) diff --git a/cmvr-es/manager/media_source_hub/include/media_source_hub.h b/cmvr-es/manager/media_source_hub/include/media_source_hub.h index c7dcb553..c09bf4d0 100644 --- a/cmvr-es/manager/media_source_hub/include/media_source_hub.h +++ b/cmvr-es/manager/media_source_hub/include/media_source_hub.h @@ -15,6 +15,10 @@ #include "common/base/ring_buffer.h" #include "common/media/media_frame.h" +namespace cmvr::service { +class StopAllAdmissionGate; +} + namespace cmvr::media { // MediaSourceHub owns no protocol-specific state. A device or capture adapter registers @@ -41,6 +45,10 @@ public: // stop() is the synchronous publication barrier for the last lease and // must unblock and join the source producer before returning. std::function stop; + // Optional confirmed variant used by operational StopAll. Returning + // false keeps the source quarantined so a later StopAll can retry it. + // When omitted, a non-throwing stop() call is treated as confirmation. + std::function stop_confirmed; std::function request_key_frame; }; @@ -84,7 +92,10 @@ public: bool active_{false}; }; - MediaSourceHub(); + // Pass the process-wide StopAll gate for a hub whose sources are part of + // whole-machine operational stopping. Test/private hubs may remain local. + explicit MediaSourceHub( + service::StopAllAdmissionGate* admission_gate = nullptr); ~MediaSourceHub(); MediaSourceHub(const MediaSourceHub&) = delete; @@ -100,6 +111,11 @@ public: bool hasSource(const std::string& track_id) const; std::vector listTracks() const; + // Returns a stable, sorted snapshot of physical source IDs. The snapshot + // includes sources temporarily removed from the public track map while a + // stop callback is in progress, so StopAll can discover orphaned activity + // without consulting DeviceManager. + std::vector trackedSourceIds() const; size_t subscriberCount(const std::string& track_id) const; // Protocol adapters can request an IDR after a discontinuity without knowing the @@ -111,14 +127,32 @@ public: StartPosition start_position = StartPosition::NEXT_PUBLISHED, CancelPredicate cancelled = {}); + // Stops and unregisters every source whose registered descriptor belongs + // to source_id. Sources for other physical devices remain registered and + // keep running. A failed source is restored for a later retry. + bool stopSourcesForDevice( + const std::string& source_id, + std::vector* failures = nullptr); + + // Stops and unregisters every source that was registered before this call's + // stop phase began. Outstanding subscriptions are invalidated and blocked + // waitRead calls are awakened. Registrations concurrent with the stop wait + // for that phase to finish and are retained, so sources can be ensured and + // subscribed again after this method returns. + bool stopAllSources(std::vector* failures = nullptr); + // Stops all registered sources and invalidates outstanding subscriptions. The // subscriptions remain destructible and their waitRead calls are awakened. // A cooperative in-progress start is cancelled; a callback that violates the // cancellation contract is quarantined with retained state rather than blocking - // shutdown or risking a use-after-free. + // shutdown or risking a use-after-free. Equivalent to stopAllSources(). void shutdown(); private: + bool stopSources( + const std::optional& source_id, + std::vector* failures); + struct Impl; std::shared_ptr impl_; }; 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 36b21653..56f4cd02 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 @@ -1,5 +1,7 @@ #include "manager/media_source_hub/include/device_media_source_adapter.h" +#include "service/stop_all/include/stop_all_admission_gate.h" + #include #include #include @@ -123,7 +125,7 @@ struct PumpState : public std::enable_shared_from_this> { : device(std::move(device_ptr)) {} virtual ~PumpState() { - stop(); + (void)stop(); } bool begin( @@ -152,7 +154,9 @@ struct PumpState : public std::enable_shared_from_this> { // A previous worker must always be collected before a new capture lease starts. std::thread stale_worker = std::move(worker); lock.unlock(); - collectThread(std::move(stale_worker)); + if (!collectThread(std::move(stale_worker))) { + return false; + } lock.lock(); } @@ -213,7 +217,7 @@ struct PumpState : public std::enable_shared_from_this> { return true; } - void stop() noexcept { + bool stop() noexcept { std::thread thread; bool stop_streaming = false; { @@ -224,24 +228,28 @@ struct PumpState : public std::enable_shared_from_this> { sink = {}; thread = std::move(worker); } + bool stopped = true; if (stop_streaming) { - stopDeviceStreaming(); + stopped = stopDeviceStreaming(); } if (thread.joinable()) { - collectThread(std::move(thread)); + stopped = collectThread(std::move(thread)) && stopped; } + return stopped; } - static void collectThread(std::thread thread) noexcept { + static bool collectThread(std::thread thread) noexcept { if (!thread.joinable()) { - return; + return true; } try { if (thread.get_id() == std::this_thread::get_id()) { thread.detach(); + return false; } else { thread.join(); } + return true; } catch (const std::exception& error) { CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to collect media pump: " << error.what(); @@ -253,22 +261,25 @@ struct PumpState : public std::enable_shared_from_this> { // platform error occurred; there is no recoverable ownership path. } } + return false; } } virtual void run() = 0; - void stopDeviceStreaming() noexcept { + bool stopDeviceStreaming() noexcept { try { if (device) { device->stopStreaming(); } + return true; } catch (const std::exception& error) { CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to stop media source: " << error.what(); } catch (...) { CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to stop media source"; } + return false; } std::shared_ptr device; @@ -282,7 +293,7 @@ struct PumpState : public std::enable_shared_from_this> { struct CameraPump final : PumpState { CameraPump(std::shared_ptr camera, std::string id) : PumpState(std::move(camera)), track_id(std::move(id)) {} - ~CameraPump() override { stop(); } + ~CameraPump() override { (void)stop(); } void run() override { size_t cursor = 0; @@ -459,7 +470,7 @@ struct CameraPump final : PumpState { struct MicrophonePump final : PumpState { MicrophonePump(std::shared_ptr microphone, std::string id) : PumpState(std::move(microphone)), track_id(std::move(id)) {} - ~MicrophonePump() override { stop(); } + ~MicrophonePump() override { (void)stop(); } void run() override { size_t cursor = 0; @@ -607,7 +618,7 @@ TrackDescriptorPtr initialTrack( } // namespace MediaSourceHub& globalMediaSourceHub() { - static MediaSourceHub hub; + static MediaSourceHub hub(&service::globalStopAllAdmissionGate()); return hub; } @@ -638,7 +649,7 @@ bool ensureCameraMediaSource( const MediaSourceHub::CancelPredicate& cancelled) { return pump->begin(sink, cancelled); }; - callbacks.stop = [pump] { pump->stop(); }; + callbacks.stop_confirmed = [pump] { return pump->stop(); }; callbacks.request_key_frame = [camera] { return camera->requestKeyFrame(); }; const bool registered = hub.registerSource( initialTrack(track_id, camera->id(), MediaKind::VIDEO), @@ -670,7 +681,7 @@ bool ensureMicrophoneMediaSource( const MediaSourceHub::CancelPredicate& cancelled) { return pump->begin(sink, cancelled); }; - callbacks.stop = [pump] { pump->stop(); }; + callbacks.stop_confirmed = [pump] { return pump->stop(); }; const bool registered = hub.registerSource( initialTrack(track_id, microphone->id(), MediaKind::AUDIO), std::move(callbacks), diff --git a/cmvr-es/manager/media_source_hub/src/media_source_hub.cpp b/cmvr-es/manager/media_source_hub/src/media_source_hub.cpp index cace2727..5942024d 100644 --- a/cmvr-es/manager/media_source_hub/src/media_source_hub.cpp +++ b/cmvr-es/manager/media_source_hub/src/media_source_hub.cpp @@ -6,8 +6,11 @@ #include #include #include +#include #include +#include "service/stop_all/include/stop_all_admission_gate.h" + namespace cmvr::media { struct MediaSourceHub::SourceState final : public std::enable_shared_from_this { @@ -28,11 +31,14 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_thisid), + source_id(initial_descriptor->source_id), descriptor(std::move(initial_descriptor)), callbacks(std::move(source_callbacks)), - ring(ring_capacity) {} + ring(ring_capacity), + admission_gate(source_admission_gate) {} FrameSink makeSink() { const std::weak_ptr weak_source = shared_from_this(); @@ -85,11 +91,18 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_this callback_lock(callback_mutex); try { - if (callbacks.stop) callbacks.stop(); + if (callbacks.stop_confirmed) { + return callbacks.stop_confirmed(); + } + if (callbacks.stop) { + callbacks.stop(); + } + return true; } catch (...) { + return false; } } @@ -117,9 +130,10 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_this lock(lifecycle_mutex); - if (lifecycle == Lifecycle::STOPPING) { + stop_unconfirmed = !stopped; + if (stopped && lifecycle == Lifecycle::STOPPING) { lifecycle = Lifecycle::STOPPED; } lifecycle_condition.notify_all(); @@ -130,12 +144,29 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_this lock(lifecycle_mutex); - while (lifecycle == Lifecycle::STOPPING) { - if (!registered || isCancelled(cancelled)) return false; - lifecycle_condition.wait_for(lock, std::chrono::milliseconds(10)); + std::optional + admission; + std::unique_lock lock(lifecycle_mutex, std::defer_lock); + for (;;) { + if (admission_gate) { + admission.emplace(admission_gate->lockAdmission()); + } + lock.lock(); + if (!registered || isCancelled(cancelled) || + (admission && !admission->accepting())) { + return false; + } + if (lifecycle != Lifecycle::STOPPING) { + break; + } + + // A device stop may block, so never wait for it while retaining + // the process-wide admission lock. + admission.reset(); + lifecycle_condition.wait_for( + lock, std::chrono::milliseconds(10)); + lock.unlock(); } - if (!registered || isCancelled(cancelled)) return false; if (lifecycle == Lifecycle::RUNNING) { ++subscriber_count; @@ -178,6 +209,10 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_thiscompleted) { if (isCancelled(cancelled)) { if (attempt->waiters != 0U) --attempt->waiters; @@ -207,9 +242,10 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_this lock(lifecycle_mutex); registered = false; ring.close(); @@ -280,25 +320,41 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_thiscancel_requested.store(true, std::memory_order_release); } lifecycle_condition.notify_all(); - return; + return false; } if (lifecycle == Lifecycle::STOPPING) { + if (stop_unconfirmed) { + lock.unlock(); + const bool stopped = invokeStop(); + lock.lock(); + stop_unconfirmed = !stopped; + if (stopped) { + lifecycle = Lifecycle::STOPPED; + } + lifecycle_condition.notify_all(); + return stopped; + } + lifecycle_condition.wait(lock, [this] { + return lifecycle != Lifecycle::STOPPING; + }); lifecycle_condition.notify_all(); - return; + return lifecycle == Lifecycle::STOPPED && !stop_unconfirmed; } if (lifecycle == Lifecycle::STOPPED) { lifecycle_condition.notify_all(); - return; + return !stop_unconfirmed; } lifecycle = Lifecycle::STOPPING; lock.unlock(); - invokeStop(); + const bool stopped = invokeStop(); lock.lock(); - if (lifecycle == Lifecycle::STOPPING) { + stop_unconfirmed = !stopped; + if (stopped && lifecycle == Lifecycle::STOPPING) { lifecycle = Lifecycle::STOPPED; } lifecycle_condition.notify_all(); + return stopped; } bool validForSubscription() const { @@ -334,9 +390,11 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_this start_attempt; }; struct MediaSourceHub::Impl final { + explicit Impl(service::StopAllAdmissionGate* source_admission_gate) + : admission_gate(source_admission_gate) {} + mutable std::mutex mutex; + std::condition_variable stop_condition; + bool stop_all_in_progress{false}; + std::unordered_set device_stops_in_progress; + std::unordered_set track_stops_in_progress; + std::unordered_map tracked_source_counts; std::unordered_map> sources; + service::StopAllAdmissionGate* const admission_gate; }; MediaSourceHub::Subscription::Subscription( @@ -425,8 +493,9 @@ void MediaSourceHub::Subscription::reset() { source_.reset(); } -MediaSourceHub::MediaSourceHub() - : impl_(std::make_shared()) {} +MediaSourceHub::MediaSourceHub( + service::StopAllAdmissionGate* admission_gate) + : impl_(std::make_shared(admission_gate)) {} MediaSourceHub::~MediaSourceHub() { shutdown(); @@ -444,13 +513,47 @@ bool MediaSourceHub::registerSource( std::shared_ptr source; try { source = std::make_shared( - std::move(initial_descriptor), std::move(callbacks), ring_capacity); + std::move(initial_descriptor), std::move(callbacks), ring_capacity, + impl_->admission_gate); } catch (...) { return false; } - std::lock_guard lock(impl_->mutex); - return impl_->sources.emplace(source->track_id, std::move(source)).second; + std::optional admission; + std::unique_lock lock(impl_->mutex, std::defer_lock); + for (;;) { + if (impl_->admission_gate) { + admission.emplace(impl_->admission_gate->lockAdmission()); + } + lock.lock(); + if (admission && !admission->accepting()) { + return false; + } + const bool can_register = + !impl_->stop_all_in_progress && + impl_->device_stops_in_progress.count(source->source_id) == 0U && + impl_->track_stops_in_progress.count(source->track_id) == 0U; + if (can_register) { + break; + } + + // Hub-local stops may invoke arbitrary device callbacks. Wait for + // them without delaying process-wide StopAll admission. + admission.reset(); + impl_->stop_condition.wait(lock, [this, &source] { + return !impl_->stop_all_in_progress && + impl_->device_stops_in_progress.count(source->source_id) == 0U && + impl_->track_stops_in_progress.count(source->track_id) == 0U; + }); + lock.unlock(); + } + const std::string source_id = source->source_id; + const bool inserted = + impl_->sources.emplace(source->track_id, std::move(source)).second; + if (inserted) { + ++impl_->tracked_source_counts[source_id]; + } + return inserted; } bool MediaSourceHub::unregisterSource(const std::string& track_id) { @@ -475,7 +578,13 @@ bool MediaSourceHub::unregisterSource(const std::string& track_id) { std::lock_guard lock(impl_->mutex); const auto it = impl_->sources.find(track_id); if (it != impl_->sources.end() && it->second == source) { + const std::string source_id = source->source_id; impl_->sources.erase(it); + const auto count_it = impl_->tracked_source_counts.find(source_id); + if (count_it != impl_->tracked_source_counts.end() && + --count_it->second == 0U) { + impl_->tracked_source_counts.erase(count_it); + } return true; } return false; @@ -516,6 +625,25 @@ std::vector MediaSourceHub::listTracks() const { return descriptors; } +std::vector MediaSourceHub::trackedSourceIds() const { + if (!impl_) { + return {}; + } + + std::vector source_ids; + { + std::lock_guard lock(impl_->mutex); + source_ids.reserve(impl_->tracked_source_counts.size()); + for (const auto& [source_id, count] : impl_->tracked_source_counts) { + if (count != 0U) { + source_ids.push_back(source_id); + } + } + } + std::sort(source_ids.begin(), source_ids.end()); + return source_ids; +} + size_t MediaSourceHub::subscriberCount(const std::string& track_id) const { if (!impl_) { return 0; @@ -573,24 +701,103 @@ MediaSourceHub::Subscription MediaSourceHub::subscribe( return Subscription(std::move(source), std::move(cursor)); } -void MediaSourceHub::shutdown() { +bool MediaSourceHub::stopSourcesForDevice( + const std::string& source_id, + std::vector* failures) { + if (source_id.empty()) { + if (failures) { + failures->clear(); + } + return true; + } + return stopSources(source_id, failures); +} + +bool MediaSourceHub::stopAllSources(std::vector* failures) { + return stopSources(std::nullopt, failures); +} + +bool MediaSourceHub::stopSources( + const std::optional& source_id, + std::vector* failures) { + if (failures) { + failures->clear(); + } if (!impl_) { - return; + return true; + } + + std::unordered_map> sources; + { + std::unique_lock lock(impl_->mutex); + if (source_id) { + impl_->stop_condition.wait(lock, [this, &source_id] { + return !impl_->stop_all_in_progress && + impl_->device_stops_in_progress.count(*source_id) == 0U; + }); + impl_->device_stops_in_progress.insert(*source_id); + for (auto source_it = impl_->sources.begin(); + source_it != impl_->sources.end();) { + if (source_it->second->source_id != *source_id) { + ++source_it; + continue; + } + impl_->track_stops_in_progress.insert(source_it->first); + sources.emplace(source_it->first, std::move(source_it->second)); + source_it = impl_->sources.erase(source_it); + } + } else { + impl_->stop_condition.wait(lock, [this] { + return !impl_->stop_all_in_progress && + impl_->device_stops_in_progress.empty(); + }); + impl_->stop_all_in_progress = true; + sources.swap(impl_->sources); + } + } + + std::unordered_map> quarantined; + for (const auto& [track_id, source] : sources) { + if (!source->shutdown()) { + quarantined.emplace(track_id, source); + if (failures) { + failures->push_back(track_id); + } + } } - std::vector> sources; { std::lock_guard lock(impl_->mutex); - sources.reserve(impl_->sources.size()); - for (auto& [track_id, source] : impl_->sources) { - (void)track_id; - sources.push_back(std::move(source)); + for (auto& [track_id, source] : quarantined) { + impl_->sources.emplace(track_id, std::move(source)); + } + for (const auto& [track_id, source] : sources) { + if (quarantined.count(track_id) != 0U) { + continue; + } + const auto count_it = + impl_->tracked_source_counts.find(source->source_id); + if (count_it != impl_->tracked_source_counts.end() && + --count_it->second == 0U) { + impl_->tracked_source_counts.erase(count_it); + } + } + if (source_id) { + for (const auto& [track_id, source] : sources) { + (void)source; + impl_->track_stops_in_progress.erase(track_id); + } + impl_->device_stops_in_progress.erase(*source_id); + } else { + impl_->stop_all_in_progress = false; } - impl_->sources.clear(); - } - for (const auto& source : sources) { - source->shutdown(); } + impl_->stop_condition.notify_all(); + return quarantined.empty(); +} + +void MediaSourceHub::shutdown() { + (void)stopAllSources(); } } // namespace cmvr::media diff --git a/cmvr-es/manager/media_source_hub/tests/media_source_hub_test.cpp b/cmvr-es/manager/media_source_hub/tests/media_source_hub_test.cpp index b96cea9b..59afca7d 100644 --- a/cmvr-es/manager/media_source_hub/tests/media_source_hub_test.cpp +++ b/cmvr-es/manager/media_source_hub/tests/media_source_hub_test.cpp @@ -1,4 +1,5 @@ #include "manager/media_source_hub/include/media_source_hub.h" +#include "service/stop_all/include/stop_all_admission_gate.h" #include #include @@ -43,10 +44,12 @@ int failures = 0; TrackDescriptorPtr makeVideoDescriptor( const Codec codec, const uint64_t generation, - std::vector codec_config = {}) { + std::vector codec_config = {}, + std::string track_id = "camera.front.video", + std::string source_id = "camera.front") { TrackDescriptor::Config config; - config.id = "camera.front.video"; - config.source_id = "camera.front"; + config.id = std::move(track_id); + config.source_id = std::move(source_id); config.kind = MediaKind::VIDEO; config.codec = codec; config.payload_format = codec == Codec::UNKNOWN ? PayloadFormat::UNKNOWN : PayloadFormat::ANNEX_B; @@ -504,6 +507,648 @@ void testHubFailedStartAndShutdown() { CHECK_TRUE(!live.waitRead(50ms).has_value()); } +void testHubStopAllSourcesAllowsReregistration() { + MediaSourceHub hub; + const auto first_descriptor = makeVideoDescriptor(Codec::H264, 1); + std::atomic first_stop_count{0}; + + MediaSourceHub::SourceCallbacks first_callbacks; + first_callbacks.start = [](const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate&) { + return true; + }; + first_callbacks.stop = [&] { ++first_stop_count; }; + CHECK_TRUE(hub.registerSource(first_descriptor, std::move(first_callbacks), 2)); + auto old_subscription = hub.subscribe(first_descriptor->id); + CHECK_TRUE(old_subscription.valid()); + + auto blocked_read = std::async(std::launch::async, [&] { + return old_subscription.waitRead(2s); + }); + CHECK_TRUE(hub.stopAllSources()); + + CHECK_TRUE(first_stop_count.load(std::memory_order_acquire) == 1); + CHECK_TRUE(!hub.hasSource(first_descriptor->id)); + CHECK_TRUE(!old_subscription.valid()); + CHECK_TRUE(blocked_read.wait_for(500ms) == std::future_status::ready); + if (blocked_read.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(!blocked_read.get().has_value()); + } + + const auto second_descriptor = makeVideoDescriptor(Codec::H264, 2); + std::atomic second_start_count{0}; + std::atomic second_stop_count{0}; + MediaSourceHub::SourceCallbacks second_callbacks; + second_callbacks.start = [&](const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate&) { + ++second_start_count; + return true; + }; + second_callbacks.stop = [&] { ++second_stop_count; }; + + CHECK_TRUE(hub.registerSource(second_descriptor, std::move(second_callbacks), 2)); + auto new_subscription = hub.subscribe(second_descriptor->id); + CHECK_TRUE(new_subscription.valid()); + CHECK_TRUE(new_subscription.descriptor()->generation == 2); + CHECK_TRUE(second_start_count.load(std::memory_order_acquire) == 1); + + old_subscription.reset(); + CHECK_TRUE(second_stop_count.load(std::memory_order_acquire) == 0); + new_subscription.reset(); + CHECK_TRUE(second_stop_count.load(std::memory_order_acquire) == 1); +} + +void testStopAllSourcesReportsAndRetriesUnconfirmedStop() { + MediaSourceHub hub; + const auto descriptor = makeVideoDescriptor(Codec::H264, 1); + std::atomic stop_attempts{0}; + + MediaSourceHub::SourceCallbacks callbacks; + callbacks.start = [](const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate&) { + return true; + }; + callbacks.stop_confirmed = [&] { + return ++stop_attempts >= 2; + }; + CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 2)); + auto subscription = hub.subscribe(descriptor->id); + CHECK_TRUE(subscription.valid()); + + std::vector stop_failures; + CHECK_TRUE(!hub.stopAllSources(&stop_failures)); + CHECK_TRUE(stop_failures.size() == 1); + CHECK_TRUE(stop_failures.front() == descriptor->id); + CHECK_TRUE(hub.hasSource(descriptor->id)); + CHECK_TRUE(!subscription.valid()); + + stop_failures.clear(); + CHECK_TRUE(hub.stopAllSources(&stop_failures)); + CHECK_TRUE(stop_failures.empty()); + CHECK_TRUE(!hub.hasSource(descriptor->id)); + CHECK_TRUE(stop_attempts.load(std::memory_order_acquire) == 2); +} + +void testStopSourcesForDeviceIsSelectiveAndRetriesFailures() { + MediaSourceHub hub; + const auto front_video = makeVideoDescriptor( + Codec::H264, 1, {}, "front.video", "camera.front"); + const auto front_depth = makeVideoDescriptor( + Codec::H264, 1, {}, "front.depth", "camera.front"); + const auto rear_video = makeVideoDescriptor( + Codec::H264, 1, {}, "rear.video", "camera.rear"); + + std::atomic front_video_stops{0}; + std::atomic front_depth_stops{0}; + std::atomic rear_stops{0}; + auto register_source = [&]( + const TrackDescriptorPtr& descriptor, + std::function stop_confirmed) { + MediaSourceHub::SourceCallbacks callbacks; + callbacks.start = []( + const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate&) { return true; }; + callbacks.stop_confirmed = std::move(stop_confirmed); + return hub.registerSource(descriptor, std::move(callbacks), 2); + }; + + CHECK_TRUE(register_source(front_video, [&] { + return ++front_video_stops >= 2; + })); + CHECK_TRUE(register_source(front_depth, [&] { + ++front_depth_stops; + return true; + })); + CHECK_TRUE(register_source(rear_video, [&] { + ++rear_stops; + return true; + })); + CHECK_TRUE( + hub.trackedSourceIds() == + (std::vector{"camera.front", "camera.rear"})); + + auto front_video_subscription = hub.subscribe(front_video->id); + auto front_depth_subscription = hub.subscribe(front_depth->id); + auto rear_subscription = hub.subscribe(rear_video->id); + CHECK_TRUE(front_video_subscription.valid()); + CHECK_TRUE(front_depth_subscription.valid()); + CHECK_TRUE(rear_subscription.valid()); + + std::vector stop_failures; + CHECK_TRUE(!hub.stopSourcesForDevice("camera.front", &stop_failures)); + CHECK_TRUE(stop_failures.size() == 1U); + CHECK_TRUE(stop_failures.front() == front_video->id); + CHECK_TRUE(hub.hasSource(front_video->id)); + CHECK_TRUE(!hub.hasSource(front_depth->id)); + CHECK_TRUE(hub.hasSource(rear_video->id)); + CHECK_TRUE( + hub.trackedSourceIds() == + (std::vector{"camera.front", "camera.rear"})); + CHECK_TRUE(!front_video_subscription.valid()); + CHECK_TRUE(!front_depth_subscription.valid()); + CHECK_TRUE(rear_subscription.valid()); + CHECK_TRUE(rear_stops.load(std::memory_order_acquire) == 0); + + CHECK_TRUE(hub.stopSourcesForDevice("camera.front", &stop_failures)); + CHECK_TRUE(stop_failures.empty()); + CHECK_TRUE(!hub.hasSource(front_video->id)); + CHECK_TRUE(hub.hasSource(rear_video->id)); + CHECK_TRUE( + hub.trackedSourceIds() == + (std::vector{"camera.rear"})); + CHECK_TRUE(front_video_stops.load(std::memory_order_acquire) == 2); + CHECK_TRUE(front_depth_stops.load(std::memory_order_acquire) == 1); + CHECK_TRUE(rear_stops.load(std::memory_order_acquire) == 0); + + rear_subscription.reset(); + CHECK_TRUE(rear_stops.load(std::memory_order_acquire) == 1); +} + +void testDeviceStopsRunConcurrentlyAndSerializeMatchingRegistration() { + MediaSourceHub hub; + const auto first = makeVideoDescriptor( + Codec::H264, 1, {}, "first.video", "camera.first"); + const auto second = makeVideoDescriptor( + Codec::H264, 1, {}, "second.video", "camera.second"); + + std::atomic first_stop_entered{false}; + std::atomic second_stop_entered{false}; + std::atomic release_stops{false}; + auto register_blocking_source = [&]( + const TrackDescriptorPtr& descriptor, + std::atomic& entered) { + MediaSourceHub::SourceCallbacks callbacks; + callbacks.start = []( + const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate&) { return true; }; + callbacks.stop = [&entered, &release_stops] { + entered.store(true, std::memory_order_release); + while (!release_stops.load(std::memory_order_acquire)) { + std::this_thread::sleep_for(1ms); + } + }; + return hub.registerSource(descriptor, std::move(callbacks), 2); + }; + + CHECK_TRUE(register_blocking_source(first, first_stop_entered)); + CHECK_TRUE(register_blocking_source(second, second_stop_entered)); + auto first_subscription = hub.subscribe(first->id); + auto second_subscription = hub.subscribe(second->id); + CHECK_TRUE(first_subscription.valid()); + CHECK_TRUE(second_subscription.valid()); + + auto first_stop = std::async(std::launch::async, [&] { + return hub.stopSourcesForDevice(first->source_id); + }); + const auto first_deadline = std::chrono::steady_clock::now() + 500ms; + while (!first_stop_entered.load(std::memory_order_acquire) && + std::chrono::steady_clock::now() < first_deadline) { + std::this_thread::sleep_for(1ms); + } + CHECK_TRUE(first_stop_entered.load(std::memory_order_acquire)); + CHECK_TRUE( + hub.trackedSourceIds() == + (std::vector{"camera.first", "camera.second"})); + + auto second_stop = std::async(std::launch::async, [&] { + return hub.stopSourcesForDevice(second->source_id); + }); + const auto second_deadline = std::chrono::steady_clock::now() + 500ms; + while (!second_stop_entered.load(std::memory_order_acquire) && + std::chrono::steady_clock::now() < second_deadline) { + std::this_thread::sleep_for(1ms); + } + CHECK_TRUE(second_stop_entered.load(std::memory_order_acquire)); + + MediaSourceHub::SourceCallbacks replacement_callbacks; + replacement_callbacks.start = []( + const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate&) { return true; }; + replacement_callbacks.stop = [] {}; + auto matching_registration = std::async(std::launch::async, [&] { + return hub.registerSource( + makeVideoDescriptor( + Codec::H264, + 2, + {}, + "first.replacement", + "camera.first"), + std::move(replacement_callbacks), + 2); + }); + CHECK_TRUE( + matching_registration.wait_for(20ms) == std::future_status::timeout); + + release_stops.store(true, std::memory_order_release); + CHECK_TRUE(first_stop.wait_for(500ms) == std::future_status::ready); + CHECK_TRUE(second_stop.wait_for(500ms) == std::future_status::ready); + if (first_stop.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(first_stop.get()); + } + if (second_stop.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(second_stop.get()); + } + CHECK_TRUE( + matching_registration.wait_for(500ms) == std::future_status::ready); + if (matching_registration.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(matching_registration.get()); + } + CHECK_TRUE(hub.hasSource("first.replacement")); + CHECK_TRUE( + hub.trackedSourceIds() == + (std::vector{"camera.first"})); +} + +void testStopAllWaitsForDeviceStopAndRetainsItsConcurrentRegistrationRule() { + MediaSourceHub hub; + const auto first = makeVideoDescriptor( + Codec::H264, 1, {}, "first.video", "camera.first"); + const auto other = makeVideoDescriptor( + Codec::H264, 1, {}, "other.video", "camera.other"); + std::atomic first_stop_entered{false}; + std::atomic release_first_stop{false}; + std::atomic other_stops{0}; + + MediaSourceHub::SourceCallbacks first_callbacks; + first_callbacks.start = []( + const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate&) { return true; }; + first_callbacks.stop = [&] { + first_stop_entered.store(true, std::memory_order_release); + while (!release_first_stop.load(std::memory_order_acquire)) { + std::this_thread::sleep_for(1ms); + } + }; + CHECK_TRUE(hub.registerSource(first, std::move(first_callbacks), 2)); + auto first_subscription = hub.subscribe(first->id); + CHECK_TRUE(first_subscription.valid()); + + auto device_stop = std::async(std::launch::async, [&] { + return hub.stopSourcesForDevice(first->source_id); + }); + const auto stop_deadline = std::chrono::steady_clock::now() + 500ms; + while (!first_stop_entered.load(std::memory_order_acquire) && + std::chrono::steady_clock::now() < stop_deadline) { + std::this_thread::sleep_for(1ms); + } + CHECK_TRUE(first_stop_entered.load(std::memory_order_acquire)); + + auto stop_all = std::async(std::launch::async, [&] { + return hub.stopAllSources(); + }); + CHECK_TRUE(stop_all.wait_for(20ms) == std::future_status::timeout); + + MediaSourceHub::SourceCallbacks other_callbacks; + other_callbacks.start = []( + const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate&) { return true; }; + other_callbacks.stop = [&] { ++other_stops; }; + CHECK_TRUE(hub.registerSource(other, std::move(other_callbacks), 2)); + auto other_subscription = hub.subscribe(other->id); + CHECK_TRUE(other_subscription.valid()); + + release_first_stop.store(true, std::memory_order_release); + CHECK_TRUE(device_stop.wait_for(500ms) == std::future_status::ready); + if (device_stop.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(device_stop.get()); + } + CHECK_TRUE(stop_all.wait_for(500ms) == std::future_status::ready); + if (stop_all.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(stop_all.get()); + } + CHECK_TRUE(!hub.hasSource(first->id)); + CHECK_TRUE(!hub.hasSource(other->id)); + CHECK_TRUE(hub.trackedSourceIds().empty()); + CHECK_TRUE(!other_subscription.valid()); + CHECK_TRUE(other_stops.load(std::memory_order_acquire) == 1); +} + +void testConcurrentRegistrationWaitsForStopAllSources() { + MediaSourceHub hub; + const auto descriptor = makeVideoDescriptor(Codec::H264, 1); + std::atomic stop_entered{false}; + std::atomic release_stop{false}; + std::atomic old_stop_count{0}; + + MediaSourceHub::SourceCallbacks old_callbacks; + old_callbacks.start = [](const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate&) { + return true; + }; + old_callbacks.stop = [&] { + stop_entered.store(true, std::memory_order_release); + while (!release_stop.load(std::memory_order_acquire)) { + std::this_thread::sleep_for(1ms); + } + ++old_stop_count; + }; + CHECK_TRUE(hub.registerSource(descriptor, std::move(old_callbacks), 2)); + auto old_subscription = hub.subscribe(descriptor->id); + CHECK_TRUE(old_subscription.valid()); + + auto stop_all = std::async( + std::launch::async, [&] { return hub.stopAllSources(); }); + const auto stop_deadline = std::chrono::steady_clock::now() + 500ms; + while (!stop_entered.load(std::memory_order_acquire) && + std::chrono::steady_clock::now() < stop_deadline) { + std::this_thread::sleep_for(1ms); + } + CHECK_TRUE(stop_entered.load(std::memory_order_acquire)); + + std::atomic new_start_count{0}; + MediaSourceHub::SourceCallbacks new_callbacks; + new_callbacks.start = [&](const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate&) { + ++new_start_count; + return true; + }; + new_callbacks.stop = [] {}; + auto registration = std::async(std::launch::async, [&] { + return hub.registerSource( + makeVideoDescriptor(Codec::H264, 2), std::move(new_callbacks), 2); + }); + + CHECK_TRUE(registration.wait_for(20ms) == std::future_status::timeout); + release_stop.store(true, std::memory_order_release); + CHECK_TRUE(stop_all.wait_for(500ms) == std::future_status::ready); + if (stop_all.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(stop_all.get()); + } + CHECK_TRUE(registration.wait_for(500ms) == std::future_status::ready); + const bool registered = registration.wait_for(0ms) == std::future_status::ready && + registration.get(); + CHECK_TRUE(registered); + CHECK_TRUE(old_stop_count.load(std::memory_order_acquire) == 1); + CHECK_TRUE(hub.hasSource(descriptor->id)); + + auto new_subscription = hub.subscribe(descriptor->id); + CHECK_TRUE(new_subscription.valid()); + CHECK_TRUE(new_subscription.descriptor()->generation == 2); + CHECK_TRUE(new_start_count.load(std::memory_order_acquire) == 1); +} + +void testSystemStopAllAdmissionFencesRegistrationAndStartup() { + cmvr::service::StopAllAdmissionGate admission_gate; + MediaSourceHub hub(&admission_gate); + const auto dormant = makeVideoDescriptor( + Codec::H264, 1, {}, "dormant.video", "camera.dormant"); + const auto new_source = makeVideoDescriptor( + Codec::H264, 1, {}, "new.video", "camera.new"); + std::atomic dormant_starts{0}; + std::atomic new_starts{0}; + + MediaSourceHub::SourceCallbacks dormant_callbacks; + dormant_callbacks.start = [&]( + const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate&) { + ++dormant_starts; + return true; + }; + dormant_callbacks.stop = [] {}; + CHECK_TRUE(hub.registerSource( + dormant, std::move(dormant_callbacks), 2)); + + const auto stop_ticket = admission_gate.beginStopAll(); + CHECK_TRUE(stop_ticket.valid()); + + MediaSourceHub::SourceCallbacks rejected_callbacks; + rejected_callbacks.start = [&]( + const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate&) { + ++new_starts; + return true; + }; + rejected_callbacks.stop = [] {}; + CHECK_TRUE(!hub.registerSource( + new_source, std::move(rejected_callbacks), 2)); + auto rejected_subscription = hub.subscribe(dormant->id); + CHECK_TRUE(!rejected_subscription.valid()); + CHECK_TRUE(dormant_starts.load(std::memory_order_acquire) == 0); + CHECK_TRUE(new_starts.load(std::memory_order_acquire) == 0); + + CHECK_TRUE(admission_gate.finishStopAll(stop_ticket, true)); + + MediaSourceHub::SourceCallbacks recovered_callbacks; + recovered_callbacks.start = [&]( + const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate&) { + ++new_starts; + return true; + }; + recovered_callbacks.stop = [] {}; + CHECK_TRUE(hub.registerSource( + new_source, std::move(recovered_callbacks), 2)); + + auto dormant_subscription = hub.subscribe(dormant->id); + auto new_subscription = hub.subscribe(new_source->id); + CHECK_TRUE(dormant_subscription.valid()); + CHECK_TRUE(new_subscription.valid()); + CHECK_TRUE(dormant_starts.load(std::memory_order_acquire) == 1); + CHECK_TRUE(new_starts.load(std::memory_order_acquire) == 1); +} + +void testSystemStopAllRejectsRegistrationWaitingForLocalStop() { + cmvr::service::StopAllAdmissionGate admission_gate; + MediaSourceHub hub(&admission_gate); + const auto old_source = makeVideoDescriptor( + Codec::H264, 1, {}, "old.video", "camera.shared"); + const auto replacement = makeVideoDescriptor( + Codec::H264, 2, {}, "replacement.video", "camera.shared"); + std::atomic stop_entered{false}; + std::atomic release_stop{false}; + + MediaSourceHub::SourceCallbacks old_callbacks; + old_callbacks.start = []( + const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate&) { return true; }; + old_callbacks.stop = [&] { + stop_entered.store(true, std::memory_order_release); + while (!release_stop.load(std::memory_order_acquire)) { + std::this_thread::sleep_for(1ms); + } + }; + CHECK_TRUE(hub.registerSource( + old_source, std::move(old_callbacks), 2)); + auto old_subscription = hub.subscribe(old_source->id); + CHECK_TRUE(old_subscription.valid()); + + auto local_stop = std::async(std::launch::async, [&] { + return hub.stopSourcesForDevice(old_source->source_id); + }); + const auto stop_deadline = std::chrono::steady_clock::now() + 500ms; + while (!stop_entered.load(std::memory_order_acquire) && + std::chrono::steady_clock::now() < stop_deadline) { + std::this_thread::sleep_for(1ms); + } + CHECK_TRUE(stop_entered.load(std::memory_order_acquire)); + + auto make_replacement_callbacks = [] { + MediaSourceHub::SourceCallbacks callbacks; + callbacks.start = []( + const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate&) { return true; }; + callbacks.stop = [] {}; + return callbacks; + }; + auto waiting_registration = std::async(std::launch::async, [&] { + return hub.registerSource( + replacement, make_replacement_callbacks(), 2); + }); + CHECK_TRUE( + waiting_registration.wait_for(20ms) == + std::future_status::timeout); + + const auto stop_ticket = admission_gate.beginStopAll(); + release_stop.store(true, std::memory_order_release); + CHECK_TRUE(local_stop.wait_for(500ms) == std::future_status::ready); + if (local_stop.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(local_stop.get()); + } + CHECK_TRUE( + waiting_registration.wait_for(500ms) == + std::future_status::ready); + if (waiting_registration.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(!waiting_registration.get()); + } + CHECK_TRUE(!hub.hasSource(replacement->id)); + + CHECK_TRUE(admission_gate.finishStopAll(stop_ticket, true)); + CHECK_TRUE(hub.registerSource( + replacement, make_replacement_callbacks(), 2)); + auto recovered = hub.subscribe(replacement->id); + CHECK_TRUE(recovered.valid()); +} + +void testSystemStopAllRejectsSubscriptionWaitingForLocalStop() { + cmvr::service::StopAllAdmissionGate admission_gate; + MediaSourceHub hub(&admission_gate); + const auto descriptor = makeVideoDescriptor( + Codec::H264, 1, {}, "waiting.video", "camera.waiting"); + std::atomic stop_entered{false}; + std::atomic release_stop{false}; + std::atomic starts{0}; + + MediaSourceHub::SourceCallbacks callbacks; + callbacks.start = [&]( + const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate&) { + ++starts; + return true; + }; + callbacks.stop = [&] { + stop_entered.store(true, std::memory_order_release); + while (!release_stop.load(std::memory_order_acquire)) { + std::this_thread::sleep_for(1ms); + } + }; + CHECK_TRUE(hub.registerSource( + descriptor, std::move(callbacks), 2)); + auto active = hub.subscribe(descriptor->id); + CHECK_TRUE(active.valid()); + + auto local_stop = std::async(std::launch::async, [&] { + active.reset(); + }); + const auto stop_deadline = std::chrono::steady_clock::now() + 500ms; + while (!stop_entered.load(std::memory_order_acquire) && + std::chrono::steady_clock::now() < stop_deadline) { + std::this_thread::sleep_for(1ms); + } + CHECK_TRUE(stop_entered.load(std::memory_order_acquire)); + + auto waiting_subscription = std::async(std::launch::async, [&] { + return hub.subscribe(descriptor->id); + }); + CHECK_TRUE( + waiting_subscription.wait_for(20ms) == + std::future_status::timeout); + + const auto stop_ticket = admission_gate.beginStopAll(); + release_stop.store(true, std::memory_order_release); + CHECK_TRUE(local_stop.wait_for(500ms) == std::future_status::ready); + if (local_stop.wait_for(0ms) == std::future_status::ready) { + local_stop.get(); + } + CHECK_TRUE( + waiting_subscription.wait_for(500ms) == + std::future_status::ready); + if (waiting_subscription.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(!waiting_subscription.get().valid()); + } + CHECK_TRUE(starts.load(std::memory_order_acquire) == 1); + + CHECK_TRUE(admission_gate.finishStopAll(stop_ticket, true)); + auto recovered = hub.subscribe(descriptor->id); + CHECK_TRUE(recovered.valid()); + CHECK_TRUE(starts.load(std::memory_order_acquire) == 2); +} + +void testStopAllSourcesCancelsStartingSourceBeforeReuse() { + MediaSourceHub hub; + const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1); + std::atomic old_start_entered{false}; + std::atomic release_old_start{false}; + std::atomic old_stop_count{0}; + + MediaSourceHub::SourceCallbacks old_callbacks; + old_callbacks.start = [&](const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate& cancelled) { + old_start_entered.store(true, std::memory_order_release); + while (!release_old_start.load(std::memory_order_acquire)) { + if (cancelled()) { + break; + } + std::this_thread::sleep_for(1ms); + } + // Deliberately report a late success to exercise the abandoned-start + // stop path after stopAllSources has unregistered this source. + return true; + }; + old_callbacks.stop = [&] { ++old_stop_count; }; + CHECK_TRUE(hub.registerSource(descriptor, std::move(old_callbacks), 2)); + + auto old_subscription = std::async(std::launch::async, [&] { + return hub.subscribe(descriptor->id); + }); + const auto start_deadline = std::chrono::steady_clock::now() + 500ms; + while (!old_start_entered.load(std::memory_order_acquire) && + std::chrono::steady_clock::now() < start_deadline) { + std::this_thread::sleep_for(1ms); + } + CHECK_TRUE(old_start_entered.load(std::memory_order_acquire)); + + CHECK_TRUE(!hub.stopAllSources()); + CHECK_TRUE(hub.hasSource(descriptor->id)); + CHECK_TRUE(old_subscription.wait_for(500ms) == std::future_status::ready); + if (old_subscription.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(!old_subscription.get().valid()); + } + + release_old_start.store(true, std::memory_order_release); + const auto old_stop_deadline = std::chrono::steady_clock::now() + 500ms; + while (old_stop_count.load(std::memory_order_acquire) != 1 && + std::chrono::steady_clock::now() < old_stop_deadline) { + std::this_thread::sleep_for(1ms); + } + CHECK_TRUE(old_stop_count.load(std::memory_order_acquire) == 1); + CHECK_TRUE(hub.stopAllSources()); + + std::atomic new_start_count{0}; + MediaSourceHub::SourceCallbacks new_callbacks; + new_callbacks.start = [&](const MediaSourceHub::FrameSink&, + const MediaSourceHub::CancelPredicate&) { + ++new_start_count; + return true; + }; + new_callbacks.stop = [] {}; + CHECK_TRUE(hub.registerSource( + makeVideoDescriptor(Codec::H264, 2), std::move(new_callbacks), 2)); + auto new_subscription = hub.subscribe(descriptor->id); + CHECK_TRUE(new_subscription.valid()); + CHECK_TRUE(new_start_count.load(std::memory_order_acquire) == 1); + CHECK_TRUE(new_subscription.valid()); +} + void testKeyFrameRequestIsOrderedBeforeStop() { MediaSourceHub hub; const auto descriptor = makeVideoDescriptor(Codec::H264, 1); @@ -670,6 +1315,16 @@ int main() { testHubLifecycleAndDescriptorRefresh(); testSubscriptionDiscardPending(); testHubFailedStartAndShutdown(); + testHubStopAllSourcesAllowsReregistration(); + testStopAllSourcesReportsAndRetriesUnconfirmedStop(); + testStopSourcesForDeviceIsSelectiveAndRetriesFailures(); + testDeviceStopsRunConcurrentlyAndSerializeMatchingRegistration(); + testStopAllWaitsForDeviceStopAndRetainsItsConcurrentRegistrationRule(); + testConcurrentRegistrationWaitsForStopAllSources(); + testSystemStopAllAdmissionFencesRegistrationAndStartup(); + testSystemStopAllRejectsRegistrationWaitingForLocalStop(); + testSystemStopAllRejectsSubscriptionWaitingForLocalStop(); + testStopAllSourcesCancelsStartingSourceBeforeReuse(); testKeyFrameRequestIsOrderedBeforeStop(); testHubCancelsBlockedStartWithoutBlockingShutdown(); testHubQuarantinesNonCooperativeStart(); diff --git a/cmvr-es/manager/task_manager/CMakeLists.txt b/cmvr-es/manager/task_manager/CMakeLists.txt index 12768c1d..6f4185a8 100644 --- a/cmvr-es/manager/task_manager/CMakeLists.txt +++ b/cmvr-es/manager/task_manager/CMakeLists.txt @@ -11,6 +11,7 @@ target_link_libraries(task_manager PRIVATE cmvr_es::common cmvr_es::device_manager + cmvr_es::stop_all_admission_gate ) add_library(cmvr_es::task_manager ALIAS task_manager) @@ -22,6 +23,7 @@ if(BUILD_TESTING) ) target_link_libraries(task_manager_lifecycle_test PRIVATE cmvr_es::task_manager + cmvr_es::stop_all_admission_gate gtest gtest_main pthread diff --git a/cmvr-es/manager/task_manager/include/task_manager.h b/cmvr-es/manager/task_manager/include/task_manager.h index 339f95c6..aa80e503 100644 --- a/cmvr-es/manager/task_manager/include/task_manager.h +++ b/cmvr-es/manager/task_manager/include/task_manager.h @@ -8,6 +8,7 @@ #include #include #include +#include #include "task/task.h" #include "task/touch_screen_task/include/touch_screen_task.h" @@ -23,6 +24,10 @@ namespace cmvr::task { static TaskManager& getInstance(const config::TaskManagerConfig& cfg); static TaskManager& getInstance(); static void destroyInstance(); + static std::vector> + activitySnapshotIfInitialized(); + static bool stopAllActivitiesIfInitialized( + std::vector* failures = nullptr); ~TaskManager(); @@ -34,6 +39,11 @@ namespace cmvr::task { std::shared_ptr getTask(const std::string& task_id) const; std::shared_ptr getTouchScreenTask(const std::string& task_id = "touch_screen") const; + // Stops command-driven operational activity without stopping the + // scheduler or destroying task/device lifecycle state. + bool stopAllActivities(std::vector* failures = nullptr); + std::vector> activitySnapshot() const; + private: explicit TaskManager(const config::TaskManagerConfig& cfg); diff --git a/cmvr-es/manager/task_manager/src/task_manager.cpp b/cmvr-es/manager/task_manager/src/task_manager.cpp index cb34859d..85ba6df9 100644 --- a/cmvr-es/manager/task_manager/src/task_manager.cpp +++ b/cmvr-es/manager/task_manager/src/task_manager.cpp @@ -1,5 +1,6 @@ #include "manager/task_manager/include/task_manager.h" +#include #include #include #include @@ -8,6 +9,7 @@ #include "common/base/logging/logger.h" #include "common/config/config_files.h" +#include "service/stop_all/include/stop_all_admission_gate.h" #include "task/task_factory.h" using namespace cmvr; @@ -119,11 +121,46 @@ TaskManager& TaskManager::getInstance() void TaskManager::destroyInstance() { - std::lock_guard lock(init_mutex_); - if (instance_) { - instance_->stopRunTask(); + std::shared_ptr instance; + { + std::lock_guard lock(init_mutex_); + instance = instance_; } - instance_.reset(); + if (instance) { + // Task shutdown may wait for an in-flight SystemService handler. That + // handler can query the process-wide task snapshot, so never retain + // init_mutex_ while stopping tasks or joining service workers. + instance->stopRunTask(); + } + { + std::lock_guard lock(init_mutex_); + if (instance_ == instance) { + instance_.reset(); + } + } +} + +bool TaskManager::stopAllActivitiesIfInitialized( + std::vector* failures) +{ + std::shared_ptr manager; + { + std::lock_guard lock(init_mutex_); + manager = instance_; + } + return !manager || manager->stopAllActivities(failures); +} + +std::vector> +TaskManager::activitySnapshotIfInitialized() +{ + std::shared_ptr manager; + { + std::lock_guard lock(init_mutex_); + manager = instance_; + } + return manager ? manager->activitySnapshot() + : std::vector>{}; } std::shared_ptr TaskManager::getTouchScreenTask(const std::string& task_id) const @@ -147,9 +184,73 @@ std::shared_ptr TaskManager::getTask(const std::string& task_id) const return it->second; } +bool TaskManager::stopAllActivities(std::vector* failures) +{ + const auto tasks = activitySnapshot(); + + bool all_stopped = true; + for (const auto& task : tasks) { + bool stopped = false; + try { + stopped = task->stopActivity(); + } catch (const std::exception& error) { + if (failures) { + failures->push_back( + task->id() + ": stop threw: " + error.what()); + } + } catch (...) { + if (failures) { + failures->push_back( + task->id() + ": stop threw an unknown exception"); + } + } + if (!stopped) { + all_stopped = false; + if (failures && (failures->empty() || + failures->back().compare(0, task->id().size(), task->id()) != 0)) { + failures->push_back( + task->id() + + ": operational stop was not confirmed"); + } + } + } + return all_stopped; +} + +std::vector> TaskManager::activitySnapshot() const +{ + std::vector> tasks; + std::lock_guard lock(tasks_mutex_); + tasks.reserve(tasks_.size()); + for (const auto& [id, task] : tasks_) { + (void)id; + if (task) { + tasks.push_back(task); + } + } + return tasks; +} + bool TaskManager::startRunTask(const double control_period_s) { + auto& admission_gate = service::globalStopAllAdmissionGate(); + std::uint64_t admission_generation = 0U; + { + auto admission = admission_gate.lockAdmission(); + if (!admission.accepting()) { + CMVR_LOG(WARNING) << "[TaskManager] task startup is paused by " + "System StopAll"; + return false; + } + admission_generation = admission.generation(); + } + std::lock_guard lifecycle_lock(lifecycle_mutex_); + const auto admission_current = [&] { + auto admission = admission_gate.lockAdmission(); + return admission.accepting() && + admission.generation() == admission_generation; + }; if (!initialized_) { CMVR_LOG(ERROR) << "[TaskManager] refusing to start because " "initialization did not complete"; @@ -160,7 +261,7 @@ bool TaskManager::startRunTask(const double control_period_s) return false; } if (running_.load()) { - return true; + return admission_current(); } std::vector> tasks; @@ -176,6 +277,17 @@ bool TaskManager::startRunTask(const double control_period_s) std::vector> started_tasks; for (const auto& task : tasks) { + if (!admission_current()) { + CMVR_LOG(WARNING) << "[TaskManager] task startup was interrupted " + "by System StopAll"; + for (auto it = started_tasks.rbegin(); + it != started_tasks.rend(); ++it) { + stopTaskNoThrow(*it); + } + running_.store(false); + return false; + } + bool started = false; try { started = task->start(); @@ -197,11 +309,31 @@ bool TaskManager::startRunTask(const double control_period_s) return false; } started_tasks.push_back(task); + if (!admission_current()) { + CMVR_LOG(WARNING) << "[TaskManager] task startup crossed a System " + "StopAll boundary: " << task->id(); + for (auto it = started_tasks.rbegin(); + it != started_tasks.rend(); ++it) { + stopTaskNoThrow(*it); + } + running_.store(false); + return false; + } } - running_.store(true); + bool admission_changed = false; try { - run_thread_ = std::thread(&TaskManager::runTaskLoop, this, control_period_s); + // Publish the scheduler under a short admission guard. No task/device + // call or rollback is made while the global StopAll mutex is held. + auto admission = admission_gate.lockAdmission(); + if (!admission.accepting() || + admission.generation() != admission_generation) { + admission_changed = true; + } else { + running_.store(true); + run_thread_ = std::thread( + &TaskManager::runTaskLoop, this, control_period_s); + } } catch (const std::exception& e) { CMVR_LOG(ERROR) << "[TaskManager] failed to start run thread: " << e.what(); running_.store(false); @@ -220,6 +352,16 @@ bool TaskManager::startRunTask(const double control_period_s) } return false; } + if (admission_changed) { + CMVR_LOG(WARNING) << "[TaskManager] scheduler startup was " + "interrupted by System StopAll"; + for (auto it = started_tasks.rbegin(); + it != started_tasks.rend(); ++it) { + stopTaskNoThrow(*it); + } + running_.store(false); + return false; + } return true; } @@ -244,6 +386,9 @@ void TaskManager::stopRunTask() } } } + std::sort(tasks.begin(), tasks.end(), [](const auto& lhs, const auto& rhs) { + return lhs->shutdownPhase() < rhs->shutdownPhase(); + }); for (const auto& task : tasks) { stopTaskNoThrow(task); } diff --git a/cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp b/cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp index 5efeeb12..1402bc29 100644 --- a/cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp +++ b/cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp @@ -1,10 +1,18 @@ #include "manager/task_manager/include/task_manager.h" +#include +#include +#include #include +#include #include +#include +#include +#include #include +#include "service/stop_all/include/stop_all_admission_gate.h" #include "task/task_factory.h" namespace { @@ -13,14 +21,22 @@ struct TaskBehavior { bool init_result{true}; bool start_result{true}; bool throw_on_start{false}; + bool stop_activity_result{true}; + bool block_start{false}; + bool query_snapshot_on_stop{false}; }; TaskBehavior task_behavior; +std::vector stop_order; class LifecycleTask final : public cmvr::task::Task { public: - explicit LifecycleTask(std::string id) - : id_(std::move(id)) + explicit LifecycleTask( + std::string id, + const cmvr::task::TaskShutdownPhase shutdown_phase = + cmvr::task::TaskShutdownPhase::DEPENDENT_ACTIVITY) + : id_(std::move(id)), + shutdown_phase_(shutdown_phase) { } @@ -29,6 +45,10 @@ public: { return cmvr::task::TaskRunMode::BLOCKING_SERVICE; } + cmvr::task::TaskShutdownPhase shutdownPhase() const override + { + return shutdown_phase_; + } bool init() override { @@ -42,6 +62,14 @@ public: bool start() override { ++start_calls; + { + std::unique_lock lock(start_mutex); + start_entered = true; + start_condition.notify_all(); + start_condition.wait(lock, [] { + return !task_behavior.block_start; + }); + } if (task_behavior.throw_on_start) { throw std::runtime_error("start failure"); } @@ -56,9 +84,19 @@ public: void stop() override { ++stop_calls; + if (task_behavior.query_snapshot_on_stop) { + (void)cmvr::task::TaskManager::activitySnapshotIfInitialized(); + } + stop_order.push_back(id_); state_ = cmvr::task::TaskState::STOPPED; } + bool stopActivity() override + { + ++stop_activity_calls; + return task_behavior.stop_activity_result; + } + cmvr::task::TaskState state() const override { return state_; } bool isBusy() const override { @@ -84,14 +122,20 @@ public: int init_calls{0}; int start_calls{0}; int stop_calls{0}; + int stop_activity_calls{0}; + std::mutex start_mutex; + std::condition_variable start_condition; + bool start_entered{false}; private: std::string id_; + cmvr::task::TaskShutdownPhase shutdown_phase_; cmvr::task::TaskState state_{ cmvr::task::TaskState::UNINITIALIZED}; }; std::shared_ptr created_task; +std::shared_ptr created_ingress_task; cmvr::config::TaskManagerConfig enabledTaskConfig() { @@ -112,8 +156,11 @@ protected: void SetUp() override { cmvr::task::TaskManager::destroyInstance(); + cmvr::service::globalStopAllAdmissionGate().clearForTesting(); task_behavior = {}; created_task.reset(); + created_ingress_task.reset(); + stop_order.clear(); cmvr::task::TaskFactory::registerCreator( cmvr::config::TaskConfigEntry::TASK_TYPE_UME_TELEOP, [](const cmvr::config::TaskConfigEntry& entry) { @@ -125,8 +172,18 @@ protected: void TearDown() override { + if (created_task) { + { + std::lock_guard lock(created_task->start_mutex); + task_behavior.block_start = false; + } + created_task->start_condition.notify_all(); + } cmvr::task::TaskManager::destroyInstance(); + cmvr::service::globalStopAllAdmissionGate().clearForTesting(); created_task.reset(); + created_ingress_task.reset(); + stop_order.clear(); } }; @@ -191,4 +248,181 @@ TEST_F(TaskManagerLifecycleTest, SuccessfulStartAndStopAreReported) EXPECT_EQ(created_task->stop_calls, 1); } +TEST_F(TaskManagerLifecycleTest, + CommandIngressStopsBeforeDependentTaskActivity) +{ + cmvr::task::TaskFactory::registerCreator( + cmvr::config::TaskConfigEntry::TASK_TYPE_GRPC_SERVER, + [](const cmvr::config::TaskConfigEntry& entry) { + created_ingress_task = std::make_shared( + entry.id(), + cmvr::task::TaskShutdownPhase::COMMAND_INGRESS); + return created_ingress_task; + }); + + auto config = enabledTaskConfig(); + auto* ingress = config.add_tasks(); + ingress->set_id("control_ingress"); + ingress->set_type( + cmvr::config::TaskConfigEntry::TASK_TYPE_GRPC_SERVER); + ingress->set_enable(true); + ingress->set_run_mode( + cmvr::config::TaskConfigEntry::TASK_RUN_MODE_BLOCKING_SERVICE); + + auto& manager = cmvr::task::TaskManager::getInstance(config); + ASSERT_TRUE(manager.initialized()); + ASSERT_TRUE(manager.startRunTask()); + manager.stopRunTask(); + + ASSERT_EQ(stop_order.size(), 2U); + EXPECT_EQ(stop_order[0], "control_ingress"); + EXPECT_EQ(stop_order[1], "lifecycle_task"); +} + +TEST_F(TaskManagerLifecycleTest, + DestroyDoesNotHoldSingletonLockWhileStoppingTasks) +{ + task_behavior.query_snapshot_on_stop = true; + auto& manager = + cmvr::task::TaskManager::getInstance(enabledTaskConfig()); + ASSERT_TRUE(manager.initialized()); + ASSERT_TRUE(manager.startRunTask()); + + auto destroy = std::async(std::launch::async, [] { + cmvr::task::TaskManager::destroyInstance(); + }); + ASSERT_EQ( + destroy.wait_for(std::chrono::seconds(1)), + std::future_status::ready); + destroy.get(); + + ASSERT_NE(created_task, nullptr); + EXPECT_EQ(created_task->stop_calls, 1); +} + +TEST_F(TaskManagerLifecycleTest, + StopAllActivitiesDoesNotStopSchedulerOrTaskLifecycle) +{ + auto& manager = + cmvr::task::TaskManager::getInstance(enabledTaskConfig()); + + ASSERT_TRUE(manager.initialized()); + ASSERT_TRUE(manager.startRunTask()); + ASSERT_NE(created_task, nullptr); + EXPECT_TRUE(manager.stopAllActivities()); + EXPECT_TRUE(manager.running()); + EXPECT_EQ(created_task->stop_activity_calls, 1); + EXPECT_EQ(created_task->stop_calls, 0); + + manager.stopRunTask(); + EXPECT_EQ(created_task->stop_calls, 1); +} + +TEST_F(TaskManagerLifecycleTest, + StopAllActivitiesReportsUnconfirmedOperationalStop) +{ + task_behavior.stop_activity_result = false; + auto& manager = + cmvr::task::TaskManager::getInstance(enabledTaskConfig()); + + std::vector failures; + EXPECT_FALSE(manager.stopAllActivities(&failures)); + ASSERT_EQ(failures.size(), 1U); + EXPECT_NE(failures.front().find("lifecycle_task"), std::string::npos); + EXPECT_EQ(created_task->stop_calls, 0); +} + +TEST_F(TaskManagerLifecycleTest, + StopAllActivitiesIsSuccessfulWhenManagerIsNotInitialized) +{ + cmvr::task::TaskManager::destroyInstance(); + std::vector failures; + EXPECT_TRUE( + cmvr::task::TaskManager::stopAllActivitiesIfInitialized(&failures)); + EXPECT_TRUE(failures.empty()); +} + +TEST_F(TaskManagerLifecycleTest, StartIsRejectedWhileStopAllAdmissionIsClosed) +{ + auto& manager = + cmvr::task::TaskManager::getInstance(enabledTaskConfig()); + auto& gate = cmvr::service::globalStopAllAdmissionGate(); + const auto ticket = gate.beginStopAll(); + + EXPECT_FALSE(manager.startRunTask()); + EXPECT_FALSE(manager.running()); + ASSERT_NE(created_task, nullptr); + EXPECT_EQ(created_task->start_calls, 0); + + EXPECT_TRUE(gate.finishStopAll(ticket, true)); + EXPECT_TRUE(manager.startRunTask()); + EXPECT_TRUE(manager.running()); +} + +TEST_F(TaskManagerLifecycleTest, + StartCrossingStopAllGenerationRollsBackStartedTasks) +{ + task_behavior.block_start = true; + auto& manager = + cmvr::task::TaskManager::getInstance(enabledTaskConfig()); + ASSERT_NE(created_task, nullptr); + + std::atomic start_result{true}; + std::thread starter([&] { + start_result.store(manager.startRunTask()); + }); + bool start_entered = false; + { + std::unique_lock lock(created_task->start_mutex); + start_entered = created_task->start_condition.wait_for( + lock, std::chrono::seconds(2), [&] { + return created_task->start_entered; + }); + } + if (!start_entered) { + { + std::lock_guard lock(created_task->start_mutex); + task_behavior.block_start = false; + } + created_task->start_condition.notify_all(); + starter.join(); + FAIL() << "task start did not reach the generation-race barrier"; + } + + auto& gate = cmvr::service::globalStopAllAdmissionGate(); + const auto ticket = gate.beginStopAll(); + { + std::lock_guard lock(created_task->start_mutex); + task_behavior.block_start = false; + } + created_task->start_condition.notify_all(); + starter.join(); + + EXPECT_FALSE(start_result.load()); + EXPECT_FALSE(manager.running()); + EXPECT_EQ(created_task->start_calls, 1); + EXPECT_EQ(created_task->stop_calls, 1); + + EXPECT_TRUE(gate.finishStopAll(ticket, true)); + EXPECT_TRUE(manager.startRunTask()); +} + +TEST_F(TaskManagerLifecycleTest, + FailedStopAllKeepsTaskStartupRejectedUntilSuccessfulRound) +{ + auto& manager = + cmvr::task::TaskManager::getInstance(enabledTaskConfig()); + auto& gate = cmvr::service::globalStopAllAdmissionGate(); + auto ticket = gate.beginStopAll(); + EXPECT_FALSE(gate.finishStopAll(ticket, false)); + + EXPECT_FALSE(manager.startRunTask()); + ASSERT_NE(created_task, nullptr); + EXPECT_EQ(created_task->start_calls, 0); + + ticket = gate.beginStopAll(); + EXPECT_TRUE(gate.finishStopAll(ticket, true)); + EXPECT_TRUE(manager.startRunTask()); +} + } // namespace diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index 920a0433..ef742c48 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -1,6 +1,10 @@ add_library(service + stop_all/src/stop_operation_dispatcher.cpp action/src/action_queue_executor.cpp + grpc/src/camera_ptz_activity_registry.cpp + grpc/src/media_activity_coordinator.cpp + grpc/src/motor_activity_coordinator.cpp grpc/src/grpc_camera_service.cpp grpc/src/grpc_system_service.cpp grpc/src/grpc_speaker_service.cpp @@ -20,6 +24,8 @@ target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(service PRIVATE cmvr_es::proto + cmvr_es::stop_all_admission_gate + cmvr_es::camera_operational_activity_registry osqp cmvr_es::control_authority cmvr_es::device_manager @@ -35,6 +41,136 @@ add_library(cmvr_es::service ALIAS service) install(TARGETS service LIBRARY DESTINATION lib) if(BUILD_TESTING) + add_executable(stop_all_admission_gate_test + stop_all/tests/stop_all_admission_gate_test.cpp + stop_all/src/stop_all_admission_gate.cpp + ) + target_include_directories(stop_all_admission_gate_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(stop_all_admission_gate_test PRIVATE + gtest + gtest_main + pthread + ) + add_test( + NAME stop_all_admission_gate_test + COMMAND stop_all_admission_gate_test + ) + set_tests_properties(stop_all_admission_gate_test PROPERTIES TIMEOUT 10) + + add_executable(stop_operation_dispatcher_test + stop_all/tests/stop_operation_dispatcher_test.cpp + stop_all/src/stop_operation_dispatcher.cpp + ) + target_include_directories(stop_operation_dispatcher_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(stop_operation_dispatcher_test PRIVATE + gtest + gtest_main + pthread + ) + add_test( + NAME stop_operation_dispatcher_test + COMMAND stop_operation_dispatcher_test + ) + set_tests_properties(stop_operation_dispatcher_test PROPERTIES TIMEOUT 10) + + add_executable(camera_operational_activity_registry_test + grpc/tests/camera_operational_activity_registry_test.cpp + ) + target_include_directories(camera_operational_activity_registry_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(camera_operational_activity_registry_test PRIVATE + cmvr_es::proto + cmvr_es::camera_operational_activity_registry + gtest + gtest_main + pthread + ) + add_test( + NAME camera_operational_activity_registry_test + COMMAND camera_operational_activity_registry_test + ) + set_tests_properties(camera_operational_activity_registry_test PROPERTIES + TIMEOUT 10) + + add_executable(camera_ptz_activity_registry_test + grpc/tests/camera_ptz_activity_registry_test.cpp + grpc/src/camera_ptz_activity_registry.cpp + stop_all/src/stop_all_admission_gate.cpp + ) + target_include_directories(camera_ptz_activity_registry_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(camera_ptz_activity_registry_test PRIVATE + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME camera_ptz_activity_registry_test + COMMAND camera_ptz_activity_registry_test + ) + set_tests_properties(camera_ptz_activity_registry_test PROPERTIES TIMEOUT 10) + + add_executable(media_activity_coordinator_test + grpc/tests/media_activity_coordinator_test.cpp + grpc/src/media_activity_coordinator.cpp + stop_all/src/stop_all_admission_gate.cpp + ) + target_include_directories(media_activity_coordinator_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(media_activity_coordinator_test PRIVATE + cmvr_es::logging + pthread + ) + add_test( + NAME media_activity_coordinator_test + COMMAND media_activity_coordinator_test + ) + set(_grpc_media_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _grpc_media_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(media_activity_coordinator_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_grpc_media_test_environment}" + ) + + add_executable(motor_activity_coordinator_test + grpc/tests/motor_activity_coordinator_test.cpp + grpc/src/motor_activity_coordinator.cpp + ) + target_include_directories(motor_activity_coordinator_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(motor_activity_coordinator_test PRIVATE + cmvr_es::logging + pthread + ) + add_test( + NAME motor_activity_coordinator_test + COMMAND motor_activity_coordinator_test + ) + set_tests_properties(motor_activity_coordinator_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_grpc_media_test_environment}" + ) + add_executable(grpc_camera_stream_policy_test grpc/tests/grpc_camera_stream_policy_test.cpp ) @@ -222,6 +358,56 @@ if(BUILD_TESTING) ENVIRONMENT "${_grpc_agv_test_environment}" ) + add_executable(grpc_head_service_test + grpc/tests/grpc_head_service_test.cpp + ) + target_include_directories(grpc_head_service_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager + ) + target_link_libraries(grpc_head_service_test + PRIVATE + service + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_head_service_test + COMMAND grpc_head_service_test + ) + set_tests_properties(grpc_head_service_test PROPERTIES + TIMEOUT 15 + ENVIRONMENT "${_grpc_system_test_environment}" + ) + + add_executable(grpc_dexhand_service_test + grpc/tests/grpc_dexhand_service_test.cpp + ) + target_include_directories(grpc_dexhand_service_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager + ) + target_link_libraries(grpc_dexhand_service_test + PRIVATE + service + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_dexhand_service_test + COMMAND grpc_dexhand_service_test + ) + set_tests_properties(grpc_dexhand_service_test PROPERTIES + TIMEOUT 15 + ENVIRONMENT "${_grpc_system_test_environment}" + ) + endif() # -------------------------------------------------------- diff --git a/cmvr-es/service/action/include/action_queue_executor.h b/cmvr-es/service/action/include/action_queue_executor.h index 3dbcac59..13344465 100644 --- a/cmvr-es/service/action/include/action_queue_executor.h +++ b/cmvr-es/service/action/include/action_queue_executor.h @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -30,6 +31,20 @@ public: CanceledAfterAdmission, }; + // Identifies one participant in a StopAll round. Multiple concurrent + // StopAll callers join the same round; ActionQueue admission resumes only + // after every ticket in that round has been completed successfully. + struct StopAllTicket { + std::uint64_t generation{0}; + std::uint64_t ticket_id{0}; + bool active_action_stop_confirmed{false}; + + bool valid() const noexcept + { + return generation != 0U && ticket_id != 0U; + } + }; + explicit ActionQueueExecutor( device::DeviceManager& device_manager, std::size_t max_accepted_action_ids = @@ -46,11 +61,31 @@ public: api::ActionQueueCommand_Feedback& feedback, const std::function& waiter_canceled = {}); - // StopAll uses this fail-closed transition. It rejects future submissions, - // cancels pending actions, and requests a typed stop for the active action. - // Returns true when every active Action device reported a confirmed stop. - // False means at least one resource remains fail-closed quarantined. - bool cancelAllAndDisable(); + // Starts (or joins) a temporary StopAll round. New action IDs are rejected + // and queued/active actions are canceled. By default the executor also + // requests a typed stop for the active action. SystemService delegates that + // stop to its whole-machine sweep so one slow Action backend cannot delay + // stop requests for every other device. + // Existing action IDs remain queryable for idempotent reconciliation. An + // invalid ticket means permanent shutdown has already started. + StopAllTicket beginStopAll(bool delegate_active_stop = false); + + // Completes a StopAll participant after the caller has stopped and + // confirmed all other devices. The queue resumes only when every ticket in + // the current round reports success and the worker is idle. True means this + // ticket was completed successfully; another concurrent ticket may still + // keep the queue paused. False is fail-closed: the ticket was stale, + // shutdown won the race, a stop was not confirmed, or the executor was not + // idle when the last ticket completed. + bool finishStopAll( + const StopAllTicket& ticket, + bool all_devices_stop_confirmed); + + // Permanently rejects new actions, cancels queued/active work, requests a + // typed stop, and tells the worker to exit after canceled work is drained. + // This transition is irreversible for this executor instance. Returns true + // when the active Action devices reported a confirmed stop. + bool disableForShutdown(); bool waitForIdle(std::chrono::milliseconds timeout); const std::string& instanceId() const noexcept; diff --git a/cmvr-es/service/action/src/action_queue_executor.cpp b/cmvr-es/service/action/src/action_queue_executor.cpp index e8641dd4..f21bf5ba 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 "service/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { namespace { @@ -732,6 +733,12 @@ device::AgvActionKind toAgvActionKind( } // namespace struct ActionQueueExecutor::Impl { + enum class RunState { + Accepting, + PausedForStopAll, + ShuttingDown, + }; + struct Record { api::ActionQueueCommand_Request request; RequestFingerprint fingerprint; @@ -840,53 +847,131 @@ struct ActionQueueExecutor::Impl { void shutdown() { - std::shared_ptr active_record; - { - std::lock_guard lock(mutex); - if (joined) { - return; - } - accepting = false; - stopping = true; - for (const auto& record : queue) { - record->cancel_requested.store( - true, std::memory_order_release); - record->condition.notify_all(); - } - active_record = active; - if (active_record) { - active_record->cancel_requested.store( - true, std::memory_order_release); - active_record->condition.notify_all(); - } + if (joined) { + return; } - (void)requestTypedStop(active_record); - queue_condition.notify_all(); + (void)disableForShutdown(); if (worker.joinable()) { worker.join(); } joined = true; } - bool cancelAllAndDisable() + void cancelAllLocked(std::shared_ptr& active_record) + { + for (const auto& record : queue) { + record->cancel_requested.store( + true, std::memory_order_release); + record->condition.notify_all(); + } + active_record = active; + if (active_record) { + active_record->cancel_requested.store( + true, std::memory_order_release); + active_record->condition.notify_all(); + } + } + + ActionQueueExecutor::StopAllTicket beginStopAll( + const bool delegate_active_stop) + { + ActionQueueExecutor::StopAllTicket ticket; + std::shared_ptr active_record; + { + std::lock_guard lock(mutex); + if (run_state == RunState::ShuttingDown) { + return ticket; + } + if (run_state == RunState::Accepting || + outstanding_stop_all_tickets.empty()) { + run_state = RunState::PausedForStopAll; + ++admission_generation; + ++stop_all_generation; + stop_all_failed = false; + } + ticket.generation = stop_all_generation; + ticket.ticket_id = ++next_stop_all_ticket_id; + outstanding_stop_all_tickets.emplace(ticket.ticket_id); + cancelAllLocked(active_record); + } + + if (delegate_active_stop) { + // finishStopAll(all_devices_stop_confirmed=true) is the external + // confirmation for the active Action device in this mode. + ticket.active_action_stop_confirmed = true; + } else { + try { + ticket.active_action_stop_confirmed = + requestTypedStop(active_record); + } catch (const std::exception& error) { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] StopAll typed stop threw, error=" + << error.what(); + } catch (...) { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] StopAll typed stop threw"; + } + } + queue_condition.notify_all(); + return ticket; + } + + bool finishStopAll( + const ActionQueueExecutor::StopAllTicket& ticket, + const bool all_devices_stop_confirmed) + { + std::lock_guard lock(mutex); + if (run_state != RunState::PausedForStopAll || + !ticket.valid() || + ticket.generation != stop_all_generation || + outstanding_stop_all_tickets.erase(ticket.ticket_id) == 0U) { + return false; + } + + const bool caller_confirmed = + ticket.active_action_stop_confirmed && + all_devices_stop_confirmed; + if (!caller_confirmed) { + stop_all_failed = true; + } + if (!outstanding_stop_all_tickets.empty()) { + return caller_confirmed; + } + if (stop_all_failed || !queue.empty() || active) { + stop_all_failed = true; + return false; + } + + run_state = RunState::Accepting; + return true; + } + + bool disableForShutdown() { std::shared_ptr active_record; { std::lock_guard lock(mutex); - accepting = false; - for (const auto& record : queue) { - record->cancel_requested.store( - true, std::memory_order_release); - record->condition.notify_all(); - } - active_record = active; - if (active_record) { - active_record->cancel_requested.store( - true, std::memory_order_release); - active_record->condition.notify_all(); + if (run_state != RunState::ShuttingDown) { + run_state = RunState::ShuttingDown; + ++admission_generation; + ++stop_all_generation; + outstanding_stop_all_tickets.clear(); + stop_all_failed = true; } + cancelAllLocked(active_record); + } + + bool stopped = false; + try { + stopped = requestTypedStop(active_record); + } catch (const std::exception& error) { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] shutdown typed stop threw, error=" + << error.what(); + } catch (...) { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] shutdown typed stop threw"; } - const bool stopped = requestTypedStop(active_record); queue_condition.notify_all(); return stopped; } @@ -914,6 +999,37 @@ struct ActionQueueExecutor::Impl { } } + enum class FailClosedResult { + Quarantined, + AlreadyFenced, + LeaseRetained, + }; + + static FailClosedResult quarantineOrRetainLease( + const std::shared_ptr& record, + const control::ControlLeaseToken& expected_token) noexcept + { + auto& authority = + control::ControlAuthorityManager::instance(); + if (authority.quarantineIfCurrent(expected_token)) { + // LeaseSet must release the displaced handler token when it exits; + // that release is what makes this quarantine recoverable. + return FailClosedResult::Quarantined; + } + try { + if (!authority.validate(expected_token)) { + return FailClosedResult::AlreadyFenced; + } + } catch (...) { + } + + // No independently managed barrier could be confirmed. Retaining the + // exact Action lease is the last fail-closed fallback. + record->retain_control_leases.store( + true, std::memory_order_release); + return FailClosedResult::LeaseRetained; + } + bool requestTypedStop(const std::shared_ptr& record) { if (!record) { @@ -991,9 +1107,7 @@ struct ActionQueueExecutor::Impl { *expected_token, owner, ttl); } catch (const std::exception& error) { all_stopped = false; - (void)authority.quarantineIfCurrent(*expected_token); - record->retain_control_leases.store( - true, std::memory_order_release); + (void)quarantineOrRetainLease(record, *expected_token); CMVR_LOG(ERROR) << "[ActionQueueExecutor] could not establish typed stop " "barrier; control remains quarantined, id=" @@ -1001,9 +1115,7 @@ struct ActionQueueExecutor::Impl { continue; } catch (...) { all_stopped = false; - (void)authority.quarantineIfCurrent(*expected_token); - record->retain_control_leases.store( - true, std::memory_order_release); + (void)quarantineOrRetainLease(record, *expected_token); CMVR_LOG(ERROR) << "[ActionQueueExecutor] could not establish typed stop " "barrier; control remains quarantined, id=" @@ -1011,10 +1123,10 @@ struct ActionQueueExecutor::Impl { continue; } if (!barrier.acquired) { - if (authority.quarantineIfCurrent(*expected_token)) { + const auto fail_closed = + quarantineOrRetainLease(record, *expected_token); + if (fail_closed != FailClosedResult::AlreadyFenced) { all_stopped = false; - record->retain_control_leases.store( - true, std::memory_order_release); CMVR_LOG(ERROR) << "[ActionQueueExecutor] exact typed stop barrier was " "not established while the Action lease remained " @@ -1079,6 +1191,12 @@ struct ActionQueueExecutor::Impl { authority.release(barrier.token); } else { all_stopped = false; + if (!authority.retireSafetyHolder(barrier.token)) { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] failed to retire an " + "unconfirmed typed-stop barrier, id=" + << step.device_id; + } CMVR_LOG(ERROR) << "[ActionQueueExecutor] typed stop was not confirmed; " "control remains quarantined, id=" @@ -1206,10 +1324,30 @@ struct ActionQueueExecutor::Impl { }; bool handled = false; + std::uint64_t observed_admission_generation = 0U; + std::uint64_t observed_system_admission_generation = 0U; const bool initially_canceled = waiterCanceled(waiter_canceled); { + auto system_admission = + globalStopAllAdmissionGate().lockAdmission(); std::lock_guard lock(mutex); handled = lookup_existing_locked(); + if (!handled && !record) { + if (!system_admission.accepting() || + run_state != RunState::Accepting) { + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + run_state == RunState::ShuttingDown + ? "ActionQueue is disabled because the service is shutting down" + : "ActionQueue is temporarily paused by StopAll"); + handled = true; + } else { + observed_admission_generation = admission_generation; + observed_system_admission_generation = + system_admission.generation(); + } + } } if (handled) { return ActionQueueExecutor::WaitResult::Terminal; @@ -1243,15 +1381,25 @@ struct ActionQueueExecutor::Impl { waiterCanceled(waiter_canceled); bool canceled_without_record = false; { + auto system_admission = + globalStopAllAdmissionGate().lockAdmission(); std::lock_guard lock(mutex); handled = lookup_existing_locked(); if (!handled && !record && canceled_before_admission) { canceled_without_record = true; - } else if (!handled && !record && (!accepting || stopping)) { + } else if (!handled && !record && + (!system_admission.accepting() || + system_admission.generation() != + observed_system_admission_generation || + run_state != RunState::Accepting || + admission_generation != + observed_admission_generation)) { fillTerminalFeedback( feedback, request.action_id(), api::ACTION_RESULT_CODE_REJECTED, 0, - "ActionQueue is disabled by a system stop"); + run_state == RunState::ShuttingDown + ? "ActionQueue is disabled because the service is shutting down" + : "ActionQueue admission was interrupted by StopAll; retry after StopAll completes"); handled = true; } else if (!handled && !record && queue.size() + (active ? 1U : 0U) >= @@ -1353,9 +1501,10 @@ struct ActionQueueExecutor::Impl { { std::unique_lock lock(mutex); queue_condition.wait(lock, [this]() { - return stopping || !queue.empty(); + return run_state == RunState::ShuttingDown || + !queue.empty(); }); - if (stopping && queue.empty()) { + if (run_state == RunState::ShuttingDown && queue.empty()) { return; } record = queue.front(); @@ -1646,18 +1795,16 @@ struct ActionQueueExecutor::Impl { barrier = authority.preemptAcquireIfCurrent( *expected_token, owner, ttl); } catch (...) { - (void)authority.quarantineIfCurrent(*expected_token); - record->retain_control_leases.store( - true, std::memory_order_release); + (void)quarantineOrRetainLease(record, *expected_token); throw; } if (!barrier.acquired) { // A direct Stop or another Action stop may already have converted // our lease. Do not join that barrier and, critically, do not // preempt a successor which acquired control after it completed. - if (authority.quarantineIfCurrent(*expected_token)) { - record->retain_control_leases.store( - true, std::memory_order_release); + const auto fail_closed = + quarantineOrRetainLease(record, *expected_token); + if (fail_closed != FailClosedResult::AlreadyFenced) { const std::string detail = "could not establish timed-out RobotArm stop barrier " "while the Action lease remained current: " + @@ -1710,8 +1857,10 @@ struct ActionQueueExecutor::Impl { } else { // Deliberately retain the safety barrier when idle was not // confirmed. Releasing it would allow a new command to overlap an - // unknown physical outcome. Recovery requires an explicit device - // safety procedure or process restart. + // unknown physical outcome. Retiring the token keeps the barrier + // fail-closed while allowing a later confirmed System StopAll + // recovery round to clear it. + (void)authority.retireSafetyHolder(barrier.token); std::lock_guard lock(record->mutex); record->stop_error = "RobotArm stop was not confirmed; control remains quarantined: " + @@ -2156,8 +2305,12 @@ struct ActionQueueExecutor::Impl { std::deque terminal_result_order; std::unordered_set retired_action_ids; std::shared_ptr active; - bool accepting{true}; - bool stopping{false}; + RunState run_state{RunState::Accepting}; + std::uint64_t admission_generation{1U}; + std::uint64_t stop_all_generation{0U}; + std::uint64_t next_stop_all_ticket_id{0U}; + std::unordered_set outstanding_stop_all_tickets; + bool stop_all_failed{false}; bool joined{false}; std::atomic sequence{0}; std::atomic concurrent_submitters{0}; @@ -2185,9 +2338,23 @@ ActionQueueExecutor::WaitResult ActionQueueExecutor::submitAndWait( return result; } -bool ActionQueueExecutor::cancelAllAndDisable() +ActionQueueExecutor::StopAllTicket ActionQueueExecutor::beginStopAll( + const bool delegate_active_stop) { - return impl_->cancelAllAndDisable(); + return impl_->beginStopAll(delegate_active_stop); +} + +bool ActionQueueExecutor::finishStopAll( + const StopAllTicket& ticket, + const bool all_devices_stop_confirmed) +{ + return impl_->finishStopAll( + ticket, all_devices_stop_confirmed); +} + +bool ActionQueueExecutor::disableForShutdown() +{ + return impl_->disableForShutdown(); } bool ActionQueueExecutor::waitForIdle( diff --git a/cmvr-es/service/grpc/include/camera_operational_activity_registry.h b/cmvr-es/service/grpc/include/camera_operational_activity_registry.h new file mode 100644 index 00000000..43762e47 --- /dev/null +++ b/cmvr-es/service/grpc/include/camera_operational_activity_registry.h @@ -0,0 +1,108 @@ +#ifndef CMVR_ES_CAMERA_OPERATIONAL_ACTIVITY_REGISTRY_H +#define CMVR_ES_CAMERA_OPERATIONAL_ACTIVITY_REGISTRY_H + +#include +#include +#include +#include +#include +#include +#include + +#include "devices/camera/abstract_camera.h" + +namespace cmvr::service { + +// Tracks successful CameraService::StartCamera calls. StopAll uses this +// registry to stop the corresponding operational pipelines without invoking +// AbstractDevice::stop(). +class CameraOperationalActivityRegistry final { +public: + enum class DispatchResult { + Success, + RejectedByStopAll, + DeviceFailure, + }; + + struct ActivityToken { + std::string device_id; + std::uint64_t activity_generation{0U}; + bool owns_start{false}; + + bool valid() const noexcept + { + return !device_id.empty() && activity_generation != 0U; + } + }; + + DispatchResult start( + const std::string& device_id, + const std::shared_ptr& camera, + ActivityToken* token = nullptr); + + // 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. + bool stopIfCurrent(const ActivityToken& token); + + // Explicit StopCamera retains its legacy lifecycle behavior, but is + // serialized here so it cannot race an operational StopAll stop. + DispatchResult stopLifecycle( + const std::string& device_id, + const std::shared_ptr& camera); + + // Reconciles a lifecycle stop performed outside CameraService. + void markCameraStopped(const std::string& device_id); + + // StopAll must close the process-wide admission gate first. Successful + // entries are removed. Failed entries remain quarantined for a later + // StopAll retry. + bool stopAllActivities(std::vector* failures = nullptr); + + // StopAll must close the process-wide admission gate first. Stops only the + // operational pipeline tracked for device_id and never invokes the camera + // lifecycle stop(). + bool stopActivitiesForDevice( + const std::string& device_id, + std::vector* failures = nullptr); + + // If no StartCamera activity is tracked, StopAll can still quiesce the + // camera currently present in DeviceManager's inventory. A tracked camera + // takes precedence over the fallback. Repeated calls in one StopAll round + // do not stop the same instance twice. + bool stopActivitiesForDevice( + const std::string& device_id, + const std::shared_ptr& fallback_camera, + std::vector* failures = nullptr); + + std::size_t activeCameraCount() const; + // Does not wait for per-device driver I/O. This conservative snapshot + // includes devices with an admitted or historical dispatch state, allowing + // StopAll to cover activity absent from the DeviceManager snapshot. + std::vector trackedDeviceIds() const; + std::vector activeDeviceIds() const; + void clearForTesting(); + +private: + struct DeviceState { + mutable std::mutex mutex; + std::shared_ptr active_camera; + std::weak_ptr last_stopped_camera; + std::uint64_t last_stopped_generation{0U}; + std::uint64_t activity_generation{0U}; + std::atomic active{false}; + }; + + std::shared_ptr stateForDevice( + const std::string& device_id, + bool create); + + mutable std::mutex states_mutex_; + std::unordered_map> states_; +}; + +CameraOperationalActivityRegistry& +globalCameraOperationalActivityRegistry(); + +} // namespace cmvr::service + +#endif // CMVR_ES_CAMERA_OPERATIONAL_ACTIVITY_REGISTRY_H diff --git a/cmvr-es/service/grpc/include/camera_ptz_activity_registry.h b/cmvr-es/service/grpc/include/camera_ptz_activity_registry.h new file mode 100644 index 00000000..9bf69711 --- /dev/null +++ b/cmvr-es/service/grpc/include/camera_ptz_activity_registry.h @@ -0,0 +1,91 @@ +#ifndef CMVR_ES_CAMERA_PTZ_ACTIVITY_REGISTRY_H +#define CMVR_ES_CAMERA_PTZ_ACTIVITY_REGISTRY_H + +#include +#include +#include +#include +#include +#include + +#include "devices/camera/abstract_camera.h" + +namespace cmvr::service { + +// Tracks PTZ commands whose START has not yet been paired with a successful +// STOP. The registry is an operational control boundary; it never invokes a +// camera lifecycle method. +class CameraPtzActivityRegistry final { +public: + enum class DispatchResult { + Success, + RejectedByStopAll, + DeviceFailure, + }; + + DispatchResult control( + const std::string& device_id, + const std::shared_ptr& camera, + device::PtzCommand command, + bool stop, + int speed); + + // Reconciles externally stopped camera PTZ state with this registry. A + // camera backend can call this if it stops PTZ outside CameraService. + void markCameraStopped(const std::string& device_id); + + // StopAll must close the process-wide admission gate before calling this. + // A true result means every tracked START received a successful matching + // STOP. Failed entries are retained so a later StopAll can retry them. + bool stopAllActivities(std::vector* failures = nullptr); + + // StopAll must close the process-wide admission gate first. Stops only PTZ + // commands tracked for device_id. Calls for different physical cameras may + // execute concurrently; calls for one camera remain ordered with control(). + bool stopActivitiesForDevice( + const std::string& device_id, + std::vector* failures = nullptr); + + std::size_t activeCommandCount() const; + // Does not wait for per-device driver I/O. This conservative snapshot + // includes devices with an admitted or historical dispatch state, allowing + // StopAll to cover activity absent from the DeviceManager snapshot. + std::vector trackedDeviceIds() const; + std::vector activeDeviceIds() const; + void clearForTesting(); + +private: + struct PtzCommandHash { + std::size_t operator()(device::PtzCommand command) const noexcept + { + return static_cast(command); + } + }; + + struct ActiveCommand { + std::shared_ptr camera; + int speed{0}; + }; + + using CameraCommands = std::unordered_map< + device::PtzCommand, ActiveCommand, PtzCommandHash>; + + struct DeviceState { + mutable std::mutex mutex; + CameraCommands commands; + std::atomic active_command_count{0U}; + }; + + std::shared_ptr stateForDevice( + const std::string& device_id, + bool create); + + mutable std::mutex states_mutex_; + std::unordered_map> states_; +}; + +CameraPtzActivityRegistry& globalCameraPtzActivityRegistry(); + +} // namespace cmvr::service + +#endif // CMVR_ES_CAMERA_PTZ_ACTIVITY_REGISTRY_H diff --git a/cmvr-es/service/grpc/include/grpc_motor_service.h b/cmvr-es/service/grpc/include/grpc_motor_service.h index 25137859..67d68910 100644 --- a/cmvr-es/service/grpc/include/grpc_motor_service.h +++ b/cmvr-es/service/grpc/include/grpc_motor_service.h @@ -10,6 +10,7 @@ #include "cmvr/api/motor_service.grpc.pb.h" #include "devices/motor/abstract_motor.h" +#include "service/grpc/include/motor_activity_coordinator.h" namespace cmvr::device { class DeviceManager; @@ -89,9 +90,15 @@ private: std::shared_ptr control; }; + enum class ResolveAccess { + Control, + Observe, + }; + struct MotorControlEntry { std::weak_ptr owner; std::shared_ptr state; + MotorActivityCoordinator::Registration stop_all_registration; }; class ControlLease { @@ -111,9 +118,12 @@ private: }; grpc::Status resolveMotor(const api::MotorTarget& target, - ResolvedMotor& resolved) const; + ResolvedMotor& resolved, + ResolveAccess access = ResolveAccess::Control) const; std::shared_ptr stateFor( const std::shared_ptr& motor) const; + std::shared_ptr existingStateFor( + const std::shared_ptr& motor) const; std::unique_ptr acquireControl( const ResolvedMotor& resolved, api::MotorControlType control, diff --git a/cmvr-es/service/grpc/include/grpc_system_service.h b/cmvr-es/service/grpc/include/grpc_system_service.h index f579aba6..e7f714fe 100644 --- a/cmvr-es/service/grpc/include/grpc_system_service.h +++ b/cmvr-es/service/grpc/include/grpc_system_service.h @@ -14,6 +14,7 @@ namespace cmvr::service { class ActionQueueExecutor; + class StopOperationDispatcher; class gRPCSystemServiceImpl: public api::SystemService::Service { public: @@ -31,6 +32,10 @@ namespace cmvr::service grpc::Status ExecuteActionQueue(grpc::ServerContext* context, const cmvr::api::ActionQueueCommand_Request* request, cmvr::api::ActionQueueCommand_Feedback* response) override; private: device::DeviceManager& dmgr_; + // Outlives ActionQueueExecutor and every StopAll RPC stack. A stop + // backend which ignores the shared deadline can therefore finish in + // its owned worker without accessing destroyed RPC-local state. + std::unique_ptr stop_dispatcher_; std::unique_ptr action_queue_; }; } diff --git a/cmvr-es/service/grpc/include/media_activity_coordinator.h b/cmvr-es/service/grpc/include/media_activity_coordinator.h new file mode 100644 index 00000000..aa0860d5 --- /dev/null +++ b/cmvr-es/service/grpc/include/media_activity_coordinator.h @@ -0,0 +1,119 @@ +#ifndef CMVR_ES_MEDIA_ACTIVITY_COORDINATOR_H +#define CMVR_ES_MEDIA_ACTIVITY_COORDINATOR_H + +#include +#include +#include +#include +#include +#include + +#include "service/stop_all/include/deferred_stop_operation.h" + +namespace cmvr::service { + +// Coordinates in-process media RPC activity with SystemService::StopAll. +// StopAll invalidates the current generation and waits for the affected RPCs +// to release their own device leases; it does not stop device lifecycles. +class MediaActivityCoordinator final { +private: + struct Impl; + struct SessionState; + +public: + using CancelCallback = std::function; + + struct StopAllTicket { + std::uint64_t generation{0}; + std::uint64_t ticket_id{0}; + + bool valid() const noexcept + { + return generation != 0U && ticket_id != 0U; + } + }; + + class Session final { + public: + Session() = default; + ~Session(); + + Session(Session&& other) noexcept; + Session& operator=(Session&& other) noexcept; + + Session(const Session&) = delete; + Session& operator=(const Session&) = delete; + + explicit operator bool() const noexcept; + bool cancelled() const noexcept; + + // Linearizes a short device operation against beginStopAll(). If this + // returns false, StopAll won the race and the operation was not run. + bool runIfCurrent(const std::function& operation) const; + + // Claims an optional process-wide resource for this session. This is + // used by speaker input because one device cannot safely have two RPCs + // feeding and independently stopping the same streaming pipeline. + bool claimExclusiveResource(const std::string& resource_key); + + void reset() noexcept; + + private: + friend class MediaActivityCoordinator; + Session( + std::shared_ptr impl, + std::shared_ptr state); + + std::shared_ptr impl_; + std::shared_ptr state_; + }; + + MediaActivityCoordinator(); + ~MediaActivityCoordinator() = default; + + MediaActivityCoordinator(const MediaActivityCoordinator&) = delete; + MediaActivityCoordinator& operator=(const MediaActivityCoordinator&) = delete; + + // Returns an invalid session while a StopAll round is in progress. + Session beginSession(CancelCallback cancel = {}); + + // Pauses new sessions and invalidates all sessions from the previous + // generation. Cancellation callbacks are normally invoked before this + // returns. SystemService defers them until whole-machine motion stop + // requests have been issued, so a callback cannot delay physical stops. + // Concurrent callers join the same StopAll round. + StopAllTicket beginStopAll(bool defer_cancellation = false); + + // Collects one independently executable, at-most-once cancellation per + // invalidated session. Each operation captures SessionState ownership and + // can safely outlive this coordinator object without capturing `this`. + bool collectCancellationOperations( + const StopAllTicket& ticket, + std::vector& operations, + std::string* error = nullptr) const; + + // Legacy synchronous wrapper which serially executes the operations above. + // Returns false for a stale ticket or when any callback throws. + bool requestCancellation(const StopAllTicket& ticket); + + // Waits until every session invalidated by this ticket has run its cleanup + // and unregistered. A timeout leaves admission paused (fail closed). + bool waitForStopped( + const StopAllTicket& ticket, + std::chrono::milliseconds timeout); + + // Completes one StopAll participant. Admission resumes only after every + // participant succeeds and all invalidated sessions have exited. + bool finishStopAll( + const StopAllTicket& ticket, + bool all_media_stopped); + +private: + std::shared_ptr impl_; +}; + +MediaActivityCoordinator& globalMediaActivityCoordinator(); + +} // namespace cmvr::service + +#endif // CMVR_ES_MEDIA_ACTIVITY_COORDINATOR_H diff --git a/cmvr-es/service/grpc/include/motor_activity_coordinator.h b/cmvr-es/service/grpc/include/motor_activity_coordinator.h new file mode 100644 index 00000000..171bdbda --- /dev/null +++ b/cmvr-es/service/grpc/include/motor_activity_coordinator.h @@ -0,0 +1,149 @@ +#ifndef CMVR_ES_MOTOR_ACTIVITY_COORDINATOR_H +#define CMVR_ES_MOTOR_ACTIVITY_COORDINATOR_H + +#include +#include +#include +#include +#include +#include +#include + +#include "service/stop_all/include/deferred_stop_operation.h" + +namespace cmvr::service { + +// Coordinates MotorService command dispatch with SystemService::StopAll. +// Registered controls expose only operational cancellation and quick-stop; +// this coordinator never invokes a MotorManager or device lifecycle method. +class MotorActivityCoordinator final { +private: + struct Impl; + +public: + using CancelCallback = std::function; + using QuickStopCallback = std::function; + using IdleCallback = std::function; + + struct StopAllTicket { + std::uint64_t generation{0}; + std::uint64_t ticket_id{0}; + + bool valid() const noexcept + { + return generation != 0U && ticket_id != 0U; + } + }; + + class Registration final { + public: + Registration() = default; + ~Registration(); + + Registration(Registration&& other) noexcept; + Registration& operator=(Registration&& other) noexcept; + + Registration(const Registration&) = delete; + Registration& operator=(const Registration&) = delete; + + explicit operator bool() const noexcept; + void reset() noexcept; + + private: + friend class MotorActivityCoordinator; + Registration(std::shared_ptr impl, std::uint64_t id) noexcept; + + std::shared_ptr impl_; + std::uint64_t id_{0}; + }; + + class AdmissionGuard final { + public: + AdmissionGuard(AdmissionGuard&&) noexcept = default; + AdmissionGuard& operator=(AdmissionGuard&&) noexcept = default; + + AdmissionGuard(const AdmissionGuard&) = delete; + AdmissionGuard& operator=(const AdmissionGuard&) = delete; + + bool accepting() const noexcept { return accepting_; } + + private: + friend class MotorActivityCoordinator; + AdmissionGuard( + std::unique_lock&& lock, + bool accepting) noexcept; + + std::unique_lock lock_; + bool accepting_{false}; + }; + + MotorActivityCoordinator(); + ~MotorActivityCoordinator() = default; + + MotorActivityCoordinator(const MotorActivityCoordinator&) = delete; + MotorActivityCoordinator& operator=(const MotorActivityCoordinator&) = delete; + + Registration registerControl( + CancelCallback cancel, + QuickStopCallback quick_stop, + IdleCallback idle, + std::string description = {}); + + // Hold this guard until the MotorControlState has been marked busy. This + // makes final command admission atomic with beginStopAll(). + AdmissionGuard lockAdmission(); + + // Invalidates admission for every command from the preceding generation. + // With defer_callbacks=false, cancellation callbacks retain their legacy + // synchronous behavior. With true, callers must collect and execute every + // target operation; each operation orders cancellation before quick-stop. + StopAllTicket beginStopAll(bool defer_callbacks = false); + + // Collects one independently executable operation per registration that + // belonged to this StopAll round. Operations capture shared state rather + // than this coordinator and are idempotent, including their failure result. + bool collectStopOperations( + const StopAllTicket& ticket, + std::vector& operations, + std::string* error = nullptr) const; + + // Legacy synchronous wrapper which serially executes the operations above. + bool requestStop( + const StopAllTicket& ticket, + std::string* error = nullptr); + + // Waits for in-flight RPC/stream ownership captured by the round to be + // released after requestStop(). + bool waitForStopped( + const StopAllTicket& ticket, + std::chrono::milliseconds timeout, + std::string* error = nullptr); + + // Convenience operation for callers that do not need split-phase stop. + bool stopAndWait( + const StopAllTicket& ticket, + std::chrono::milliseconds timeout, + std::string* error = nullptr); + + // Admission resumes only when all registered controls confirmed their stop + // and every invalidated RPC/stream has exited. Failure remains fail-closed. + bool finishStopAll( + const StopAllTicket& ticket, + bool all_motors_stopped); + + // Wakes StopAll after a MotorControlState releases or changes ownership. + void notifyStateChanged() noexcept; + + // Test/process teardown hook. Runtime recovery must use another successful + // StopAll round instead of bypassing fail-closed state. + void clearForTesting() noexcept; + +private: + std::shared_ptr impl_; +}; + +MotorActivityCoordinator& globalMotorActivityCoordinator(); + +} // namespace cmvr::service + +#endif // CMVR_ES_MOTOR_ACTIVITY_COORDINATOR_H diff --git a/cmvr-es/service/grpc/src/camera_operational_activity_registry.cpp b/cmvr-es/service/grpc/src/camera_operational_activity_registry.cpp new file mode 100644 index 00000000..48b36c4d --- /dev/null +++ b/cmvr-es/service/grpc/src/camera_operational_activity_registry.cpp @@ -0,0 +1,328 @@ +#include "service/grpc/include/camera_operational_activity_registry.h" + +#include +#include + +#include "service/stop_all/include/stop_all_admission_gate.h" + +namespace cmvr::service { +namespace { + +template +CameraOperationalActivityRegistry::DispatchResult dispatchIfAdmitted( + std::mutex& device_mutex, + Operation&& operation) +{ + std::uint64_t admitted_generation = 0U; + { + auto admission = globalStopAllAdmissionGate().lockAdmission(); + if (!admission.accepting()) { + return CameraOperationalActivityRegistry::DispatchResult:: + RejectedByStopAll; + } + admitted_generation = admission.generation(); + } + + std::lock_guard dispatch_lock(device_mutex); + { + auto admission = globalStopAllAdmissionGate().lockAdmission(); + if (!admission.accepting() || + admission.generation() != admitted_generation) { + return CameraOperationalActivityRegistry::DispatchResult:: + RejectedByStopAll; + } + } + + return operation(); +} + +} // namespace + +std::shared_ptr +CameraOperationalActivityRegistry::stateForDevice( + const std::string& device_id, + const bool create) +{ + std::lock_guard lock(states_mutex_); + const auto existing = states_.find(device_id); + if (existing != states_.end()) { + return existing->second; + } + if (!create) { + return {}; + } + + auto state = std::make_shared(); + states_.emplace(device_id, state); + return state; +} + +CameraOperationalActivityRegistry::DispatchResult +CameraOperationalActivityRegistry::start( + const std::string& device_id, + const std::shared_ptr& camera, + ActivityToken* token) +{ + if (token) { + *token = {}; + } + if (device_id.empty() || !camera) { + return DispatchResult::DeviceFailure; + } + + const auto state = stateForDevice(device_id, true); + return dispatchIfAdmitted(state->mutex, [&] { + const auto previous_camera = state->active_camera; + const bool was_active = state->active.load(std::memory_order_acquire); + if (was_active && previous_camera != camera) { + // A device id has one operational owner at a time. Replacing an + // active instance would make a token from either instance unable + // to roll back without risking the other camera. + return DispatchResult::DeviceFailure; + } + if (!previous_camera) { + // Allocate tracking before device I/O so a successful start always + // has a StopAll-visible owner. + state->active_camera = camera; + } + + bool started = false; + try { + started = camera->startOperationalActivity(); + } catch (...) { + state->active_camera = previous_camera; + state->active.store(was_active, std::memory_order_release); + throw; + } + if (!started) { + state->active_camera = previous_camera; + state->active.store(was_active, std::memory_order_release); + return DispatchResult::DeviceFailure; + } + + state->active_camera = camera; + state->active.store(true, std::memory_order_release); + ++state->activity_generation; + if (state->activity_generation == 0U) { + ++state->activity_generation; + } + if (token) { + token->device_id = device_id; + token->activity_generation = state->activity_generation; + token->owns_start = !was_active; + } + return DispatchResult::Success; + }); +} + +bool CameraOperationalActivityRegistry::stopIfCurrent( + const ActivityToken& token) +{ + if (!token.valid() || !token.owns_start) { + return true; + } + + const auto state = stateForDevice(token.device_id, false); + if (!state) { + return true; + } + + std::lock_guard dispatch_lock(state->mutex); + if (!state->active.load(std::memory_order_acquire) || + state->activity_generation != token.activity_generation || + !state->active_camera) { + return true; + } + + bool stopped = false; + try { + stopped = state->active_camera->stopOperationalActivity(); + } catch (...) { + stopped = false; + } + if (!stopped) { + return false; + } + + state->last_stopped_camera = state->active_camera; + state->active_camera.reset(); + state->active.store(false, std::memory_order_release); + return true; +} + +CameraOperationalActivityRegistry::DispatchResult +CameraOperationalActivityRegistry::stopLifecycle( + const std::string& device_id, + const std::shared_ptr& camera) +{ + if (device_id.empty() || !camera) { + return DispatchResult::DeviceFailure; + } + + const auto state = stateForDevice(device_id, true); + return dispatchIfAdmitted(state->mutex, [&] { + if (!camera->stop()) { + return DispatchResult::DeviceFailure; + } + state->active_camera.reset(); + state->active.store(false, std::memory_order_release); + return DispatchResult::Success; + }); +} + +void CameraOperationalActivityRegistry::markCameraStopped( + const std::string& device_id) +{ + const auto state = stateForDevice(device_id, false); + if (!state) { + return; + } + std::lock_guard lock(state->mutex); + state->active_camera.reset(); + state->active.store(false, std::memory_order_release); +} + +bool CameraOperationalActivityRegistry::stopActivitiesForDevice( + const std::string& device_id, + std::vector* failures) +{ + return stopActivitiesForDevice(device_id, {}, failures); +} + +bool CameraOperationalActivityRegistry::stopActivitiesForDevice( + const std::string& device_id, + const std::shared_ptr& fallback_camera, + std::vector* failures) +{ + const auto state = stateForDevice( + device_id, static_cast(fallback_camera)); + if (!state) { + return true; + } + + std::lock_guard dispatch_lock(state->mutex); + const auto active_camera = state->active_camera; + const auto camera = active_camera ? active_camera : fallback_camera; + if (!camera) { + return true; + } + + std::uint64_t stop_generation = 0U; + { + const auto admission = globalStopAllAdmissionGate().lockAdmission(); + stop_generation = admission.generation(); + } + if (!active_camera && + state->last_stopped_generation == stop_generation && + state->last_stopped_camera.lock() == camera) { + return true; + } + + bool stopped = false; + std::string detail; + try { + stopped = camera->stopOperationalActivity(); + if (!stopped) { + detail = "operational camera stop was not confirmed"; + } + } catch (const std::exception& error) { + detail = std::string("operational camera stop threw: ") + error.what(); + } catch (...) { + detail = "operational camera stop threw an unknown exception"; + } + + if (stopped) { + if (active_camera) { + state->active_camera.reset(); + } + state->last_stopped_camera = camera; + state->last_stopped_generation = stop_generation; + state->active.store(false, std::memory_order_release); + return true; + } + if (failures) { + failures->push_back(device_id + ": " + detail); + } + return false; +} + +bool CameraOperationalActivityRegistry::stopAllActivities( + std::vector* failures) +{ + std::vector device_ids; + { + std::lock_guard lock(states_mutex_); + device_ids.reserve(states_.size()); + for (const auto& [device_id, state] : states_) { + (void)state; + device_ids.push_back(device_id); + } + } + + bool all_stopped = true; + for (const auto& device_id : device_ids) { + if (!stopActivitiesForDevice(device_id, failures)) { + all_stopped = false; + } + } + return all_stopped; +} + +std::size_t CameraOperationalActivityRegistry::activeCameraCount() const +{ + return activeDeviceIds().size(); +} + +std::vector +CameraOperationalActivityRegistry::trackedDeviceIds() const +{ + std::vector device_ids; + { + std::lock_guard lock(states_mutex_); + device_ids.reserve(states_.size()); + for (const auto& [device_id, state] : states_) { + (void)state; + device_ids.push_back(device_id); + } + } + std::sort(device_ids.begin(), device_ids.end()); + return device_ids; +} + +std::vector +CameraOperationalActivityRegistry::activeDeviceIds() const +{ + std::vector>> states; + { + std::lock_guard lock(states_mutex_); + states.reserve(states_.size()); + for (const auto& entry : states_) { + states.push_back(entry); + } + } + + std::vector device_ids; + device_ids.reserve(states.size()); + for (const auto& [device_id, state] : states) { + if (state->active.load(std::memory_order_acquire)) { + device_ids.push_back(device_id); + } + } + std::sort(device_ids.begin(), device_ids.end()); + return device_ids; +} + +void CameraOperationalActivityRegistry::clearForTesting() +{ + std::lock_guard lock(states_mutex_); + states_.clear(); +} + +CameraOperationalActivityRegistry& +globalCameraOperationalActivityRegistry() +{ + static CameraOperationalActivityRegistry registry; + return registry; +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/src/camera_ptz_activity_registry.cpp b/cmvr-es/service/grpc/src/camera_ptz_activity_registry.cpp new file mode 100644 index 00000000..475b58e8 --- /dev/null +++ b/cmvr-es/service/grpc/src/camera_ptz_activity_registry.cpp @@ -0,0 +1,224 @@ +#include "service/grpc/include/camera_ptz_activity_registry.h" + +#include +#include +#include + +#include "service/stop_all/include/stop_all_admission_gate.h" + +namespace cmvr::service { + +std::shared_ptr +CameraPtzActivityRegistry::stateForDevice( + const std::string& device_id, + const bool create) +{ + std::lock_guard lock(states_mutex_); + const auto existing = states_.find(device_id); + if (existing != states_.end()) { + return existing->second; + } + if (!create) { + return {}; + } + + auto state = std::make_shared(); + states_.emplace(device_id, state); + return state; +} + +CameraPtzActivityRegistry::DispatchResult +CameraPtzActivityRegistry::control( + const std::string& device_id, + const std::shared_ptr& camera, + const device::PtzCommand command, + const bool stop, + const int speed) +{ + 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. + std::uint64_t admitted_generation = 0U; + { + auto admission = globalStopAllAdmissionGate().lockAdmission(); + if (!admission.accepting()) { + return DispatchResult::RejectedByStopAll; + } + admitted_generation = admission.generation(); + } + + const auto state = stateForDevice(device_id, true); + std::lock_guard dispatch_lock(state->mutex); + { + auto admission = globalStopAllAdmissionGate().lockAdmission(); + if (!admission.accepting() || + admission.generation() != admitted_generation) { + return DispatchResult::RejectedByStopAll; + } + } + + if (!camera->controlPtz(command, stop, speed)) { + return DispatchResult::DeviceFailure; + } + + if (!stop) { + state->commands[command] = {camera, speed}; + } else { + state->commands.erase(command); + } + state->active_command_count.store( + state->commands.size(), std::memory_order_release); + return DispatchResult::Success; +} + +void CameraPtzActivityRegistry::markCameraStopped( + const std::string& device_id) +{ + const auto state = stateForDevice(device_id, false); + if (!state) { + return; + } + std::lock_guard lock(state->mutex); + state->commands.clear(); + state->active_command_count.store(0U, std::memory_order_release); +} + +bool CameraPtzActivityRegistry::stopActivitiesForDevice( + const std::string& device_id, + std::vector* failures) +{ + const auto state = stateForDevice(device_id, false); + if (!state) { + return true; + } + + std::lock_guard dispatch_lock(state->mutex); + bool all_stopped = true; + for (auto command_it = state->commands.begin(); + command_it != state->commands.end();) { + bool stopped = false; + std::string detail; + try { + const auto& activity = command_it->second; + stopped = activity.camera && activity.camera->controlPtz( + command_it->first, true, activity.speed); + if (!stopped) { + detail = "PTZ stop was rejected by the camera"; + } + } catch (const std::exception& error) { + detail = std::string("PTZ stop threw: ") + error.what(); + } catch (...) { + detail = "PTZ stop threw an unknown exception"; + } + + if (stopped) { + command_it = state->commands.erase(command_it); + continue; + } + + all_stopped = false; + if (failures) { + failures->push_back(device_id + ": " + detail); + } + ++command_it; + } + state->active_command_count.store( + state->commands.size(), std::memory_order_release); + return all_stopped; +} + +bool CameraPtzActivityRegistry::stopAllActivities( + std::vector* failures) +{ + std::vector device_ids; + { + std::lock_guard lock(states_mutex_); + device_ids.reserve(states_.size()); + for (const auto& [device_id, state] : states_) { + (void)state; + device_ids.push_back(device_id); + } + } + + bool all_stopped = true; + for (const auto& device_id : device_ids) { + if (!stopActivitiesForDevice(device_id, failures)) { + all_stopped = false; + } + } + return all_stopped; +} + +std::size_t CameraPtzActivityRegistry::activeCommandCount() const +{ + std::vector> states; + { + std::lock_guard lock(states_mutex_); + states.reserve(states_.size()); + for (const auto& [device_id, state] : states_) { + (void)device_id; + states.push_back(state); + } + } + + std::size_t count = 0U; + for (const auto& state : states) { + count += state->active_command_count.load(std::memory_order_acquire); + } + return count; +} + +std::vector CameraPtzActivityRegistry::trackedDeviceIds() const +{ + std::vector device_ids; + { + std::lock_guard lock(states_mutex_); + device_ids.reserve(states_.size()); + for (const auto& [device_id, state] : states_) { + (void)state; + device_ids.push_back(device_id); + } + } + std::sort(device_ids.begin(), device_ids.end()); + return device_ids; +} + +std::vector CameraPtzActivityRegistry::activeDeviceIds() const +{ + std::vector>> states; + { + std::lock_guard lock(states_mutex_); + states.reserve(states_.size()); + for (const auto& entry : states_) { + states.push_back(entry); + } + } + + std::vector device_ids; + device_ids.reserve(states.size()); + for (const auto& [device_id, state] : states) { + if (state->active_command_count.load(std::memory_order_acquire) != 0U) { + device_ids.push_back(device_id); + } + } + std::sort(device_ids.begin(), device_ids.end()); + return device_ids; +} + +void CameraPtzActivityRegistry::clearForTesting() +{ + std::lock_guard lock(states_mutex_); + states_.clear(); +} + +CameraPtzActivityRegistry& globalCameraPtzActivityRegistry() +{ + static CameraPtzActivityRegistry registry; + return registry; +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/src/grpc_agv_service.cpp index b905593f..e1192ace 100644 --- a/cmvr-es/service/grpc/src/grpc_agv_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_agv_service.cpp @@ -12,6 +12,7 @@ #include "common/base/logging/logger.h" #include "manager/control_authority/include/control_authority_manager.h" +#include "service/stop_all/include/stop_all_admission_gate.h" using google::protobuf::util::TimeUtil; @@ -108,6 +109,24 @@ grpc::Status setControlLeaseConflict( response->mutable_header(), device_id, detail); } +grpc::Status setStopAllRejected( + api::CommandHeader_Feedback* response, + const std::string& device_id) +{ + const std::string message = + "AGV control is temporarily paused by StopAll: " + device_id; + fillFeedback(response, false, message); + return grpc::Status(grpc::StatusCode::UNAVAILABLE, message); +} + +template +grpc::Status setStopAllRejected( + Response* response, + const std::string& device_id) +{ + return setStopAllRejected(response->mutable_header(), device_id); +} + class ScopedUnaryAgvControlLease final { public: ScopedUnaryAgvControlLease( @@ -129,27 +148,93 @@ public: const auto ttl = std::chrono::duration_cast< control::ControlAuthorityManager::Duration>( std::chrono::hours(24)); - auto acquired = preemptive - ? manager_.preemptAcquire(device_id, owner, ttl) - : manager_.tryAcquire(device_id, owner, ttl); + control::ControlAcquireResult acquired; + if (preemptive) { + acquired = manager_.preemptAcquire(device_id, owner, ttl); + } else { + auto admission = + globalStopAllAdmissionGate().lockAdmission(); + if (!admission.accepting()) { + rejected_by_stop_all_ = true; + detail_ = "System StopAll admission is closed"; + return; + } + admission_generation_ = admission.generation(); + acquired = manager_.tryAcquire(device_id, owner, ttl); + } acquired_ = acquired.acquired; token_ = std::move(acquired.token); detail_ = std::move(acquired.detail); + release_on_destroy_ = !preemptive; } ~ScopedUnaryAgvControlLease() { if (release_on_destroy_) { manager_.release(token_); + } else if (acquired_) { + (void)manager_.retireSafetyHolder(token_); } } bool acquired() const noexcept { return acquired_; } + bool rejectedByStopAll() const noexcept + { + return rejected_by_stop_all_; + } const std::string& detail() const noexcept { return detail_; } - // Unknown physical outcomes stay fail-closed until an explicit device - // safety procedure or process restart clears the retained holder. - void quarantine() noexcept { release_on_destroy_ = false; } + bool waitForPreemptedRelease( + const control::ControlAuthorityManager::Duration timeout) + { + return manager_.waitForPreemptedRelease(token_, timeout); + } + + bool admissionCurrent() const + { + auto admission = globalStopAllAdmissionGate().lockAdmission(); + return admission.accepting() && + admission.generation() == admission_generation_; + } + + bool current() const + { + return manager_.validate(token_) && admissionCurrent(); + } + + std::function cancellationRequested( + grpc::ServerContext* context) const + { + const auto token = token_; + const auto admission_generation = admission_generation_; + return [context, token, admission_generation]() { + try { + if ((context && context->IsCancelled()) || + !control::ControlAuthorityManager::instance() + .validate(token)) { + return true; + } + auto admission = + globalStopAllAdmissionGate().lockAdmission(); + return !admission.accepting() || + admission.generation() != admission_generation; + } catch (...) { + return true; + } + }; + } + + control::ControlDispatchGuard tryBeginDispatch() + { + auto admission = globalStopAllAdmissionGate().lockAdmission(); + if (!admission.accepting() || + admission.generation() != admission_generation_) { + return {}; + } + return manager_.tryBeginDispatch(token_); + } + + void confirmSafeToRelease() noexcept { release_on_destroy_ = true; } private: control::ControlAuthorityManager& manager_; @@ -157,8 +242,38 @@ private: std::string detail_; bool acquired_{false}; bool release_on_destroy_{true}; + bool rejected_by_stop_all_{false}; + std::uint64_t admission_generation_{0U}; }; +template +grpc::Status setControlAdmissionFailure( + Response* response, + const std::string& device_id, + const ScopedUnaryAgvControlLease& lease) +{ + return lease.rejectedByStopAll() + ? setStopAllRejected(response, device_id) + : setControlLeaseConflict(response, device_id, lease.detail()); +} + +template +grpc::Status setControlDispatchFailure( + Response* response, + const std::string& device_id, + const ScopedUnaryAgvControlLease& lease, + const char* operation) +{ + if (!lease.admissionCurrent()) { + return setStopAllRejected(response, device_id); + } + return setControlLeaseConflict( + response, + device_id, + std::string("control lease was preempted before ") + operation + + " dispatch"); +} + template grpc::Status executeConfirmedAgvStop( Response* response, @@ -168,23 +283,41 @@ grpc::Status executeConfirmedAgvStop( Operation&& operation) { try { - const auto command_result = operation(); + const auto initial_stop = operation(); + if (!initial_stop.ok()) { + return setResponseResult(response, initial_stop); + } + + constexpr auto handler_release_timeout = std::chrono::seconds(15); + if (!control_barrier.waitForPreemptedRelease( + std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>( + handler_release_timeout))) { + return setResponseResult( + response, + device::AgvResult::failure( + device::AgvErrorCode::Timeout, + std::string(operation_name) + + " timed out waiting for the preempted control handler to exit")); + } + + const auto final_stop = operation(); + if (!final_stop.ok()) { + return setResponseResult(response, final_stop); + } + const auto stopped = agv->confirmMotionStopped(); if (!stopped.ok()) { - control_barrier.quarantine(); std::string message = std::string(operation_name) + " did not reach a confirmed stopped state: " + stopped.message; - if (!command_result.ok()) { - message += "; command_result=" + command_result.message; - } return setResponseResult( response, device::AgvResult::failure(stopped.code, message)); } - return setResponseResult(response, command_result); + control_barrier.confirmSafeToRelease(); + return setResponseResult(response, final_stop); } catch (...) { - control_barrier.quarantine(); throw; } } @@ -209,7 +342,7 @@ device::AgvAdapterParams toAdapterParams(const msgs::AgvAdapterParams& src) device::AgvMotionOptions toMotionOptions( const msgs::AgvMotionOptions& src, - grpc::ServerContext* context = nullptr) + std::function cancellation_requested = {}) { device::AgvMotionOptions dst; dst.max_speed = src.max_speed(); @@ -222,11 +355,7 @@ device::AgvMotionOptions toMotionOptions( dst.asynchronous = src.asynchronous(); dst.wait_timeout_ms = src.wait_timeout_ms(); dst.poll_interval_ms = src.poll_interval_ms(); - if (context) { - dst.cancellation_requested = [context]() { - return context->IsCancelled(); - }; - } + dst.cancellation_requested = std::move(cancellation_requested); return dst; } @@ -556,8 +685,13 @@ grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext*, ScopedUnaryAgvControlLease control_lease( device_id, "clearFault"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { + return setControlDispatchFailure( + response, device_id, control_lease, "clearFault"); } return setResponseResult(response, agv->clearFault()); } catch (const std::exception& e) { @@ -582,12 +716,18 @@ grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext* context, ScopedUnaryAgvControlLease control_lease( device_id, "navigateToPose"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + if (!control_lease.current()) { + return setControlDispatchFailure( + response, device_id, control_lease, "navigateToPose"); } return setResponseResult(response, agv->navigateToPose( toPose2d(request->pose()), - toMotionOptions(request->options(), context), + toMotionOptions( + request->options(), + control_lease.cancellationRequested(context)), toAdapterParams(request->adapter_params()))); } catch (const std::exception& e) { fillFeedback(response->mutable_header(), false, e.what()); @@ -611,12 +751,19 @@ grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext* context, ScopedUnaryAgvControlLease control_lease( device_id, "navigateToStation"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + if (!control_lease.current()) { + return setControlDispatchFailure( + response, device_id, control_lease, + "navigateToStation"); } return setResponseResult(response, agv->navigateToStation( request->station_id(), - toMotionOptions(request->options(), context), + toMotionOptions( + request->options(), + control_lease.cancellationRequested(context)), toAdapterParams(request->adapter_params()))); } catch (const std::exception& e) { fillFeedback(response->mutable_header(), false, e.what()); @@ -640,8 +787,12 @@ grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext* context, ScopedUnaryAgvControlLease control_lease( device_id, "followPath"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + if (!control_lease.current()) { + return setControlDispatchFailure( + response, device_id, control_lease, "followPath"); } std::vector path; path.reserve(static_cast(request->path_size())); @@ -652,7 +803,9 @@ grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext* context, response, agv->followPath( path, - toMotionOptions(request->options(), 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()); @@ -678,8 +831,13 @@ grpc::Status gRPCAgvServiceImpl::translate( ScopedUnaryAgvControlLease control_lease( device_id, "translate"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { + return setControlDispatchFailure( + response, device_id, control_lease, "translate"); } return setResponseResult( response, @@ -707,8 +865,14 @@ grpc::Status gRPCAgvServiceImpl::pauseNavigation(grpc::ServerContext*, ScopedUnaryAgvControlLease control_lease( device_id, "pauseNavigation"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { + return setControlDispatchFailure( + response, device_id, control_lease, + "pauseNavigation"); } return setResponseResult(response, agv->pauseNavigation()); } catch (const std::exception& e) { @@ -730,8 +894,14 @@ grpc::Status gRPCAgvServiceImpl::resumeNavigation(grpc::ServerContext*, ScopedUnaryAgvControlLease control_lease( device_id, "resumeNavigation"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { + return setControlDispatchFailure( + response, device_id, control_lease, + "resumeNavigation"); } return setResponseResult(response, agv->resumeNavigation()); } catch (const std::exception& e) { @@ -778,8 +948,13 @@ grpc::Status gRPCAgvServiceImpl::setVelocity(grpc::ServerContext*, ScopedUnaryAgvControlLease control_lease( device_id, "setVelocity"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { + return setControlDispatchFailure( + response, device_id, control_lease, "setVelocity"); } return setResponseResult(response, agv->setVelocity(toVelocity(request->velocity()))); } catch (const std::exception& e) { @@ -874,8 +1049,13 @@ grpc::Status gRPCAgvServiceImpl::switchMap(grpc::ServerContext*, ScopedUnaryAgvControlLease control_lease( device_id, "switchMap"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { + return setControlDispatchFailure( + response, device_id, control_lease, "switchMap"); } return setResponseResult(response, agv->switchMap(request->map_name())); } catch (const std::exception& e) { @@ -897,8 +1077,13 @@ grpc::Status gRPCAgvServiceImpl::uploadMap(grpc::ServerContext*, ScopedUnaryAgvControlLease control_lease( device_id, "uploadMap"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { + return setControlDispatchFailure( + response, device_id, control_lease, "uploadMap"); } return setResponseResult(response, agv->uploadMap(request->map_name(), request->content())); } catch (const std::exception& e) { @@ -942,8 +1127,13 @@ grpc::Status gRPCAgvServiceImpl::startMapping(grpc::ServerContext*, ScopedUnaryAgvControlLease control_lease( device_id, "startMapping"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { + return setControlDispatchFailure( + response, device_id, control_lease, "startMapping"); } device::AgvMappingOptions options; options.dimension = toMapDimension(request->dimension()); @@ -1043,8 +1233,13 @@ grpc::Status gRPCAgvServiceImpl::stopMapping(grpc::ServerContext*, ScopedUnaryAgvControlLease control_lease( device_id, "stopMapping"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { + return setControlDispatchFailure( + response, device_id, control_lease, "stopMapping"); } return setResponseResult(response, agv->stopMapping()); } catch (const std::exception& e) { diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index ed635ce3..89a84745 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -8,6 +8,7 @@ #include "common/base/logging/logger.h" #include "manager/control_authority/include/control_authority_manager.h" +#include "service/stop_all/include/stop_all_admission_gate.h" using google::protobuf::util::TimeUtil; @@ -67,7 +68,9 @@ device::JointVelocityCommand toJointVelocityCommand(const api::JointVelocityComm return dst; } -device::MotionOptions toMotionOptions(const api::MotionOptions& src) +device::MotionOptions toMotionOptions( + const api::MotionOptions& src, + std::function cancellation_requested = {}) { device::MotionOptions dst; dst.velocity = src.velocity(); @@ -77,6 +80,7 @@ device::MotionOptions toMotionOptions(const api::MotionOptions& src) dst.joint_velocity_limits.assign(src.joint_velocity_limits().begin(), src.joint_velocity_limits().end()); dst.asynchronous = src.asynchronous(); + dst.cancellation_requested = std::move(cancellation_requested); return dst; } @@ -124,6 +128,24 @@ grpc::Status setDeviceNotFound(Response* response, const std::string& device_id) return grpc::Status(grpc::StatusCode::NOT_FOUND, message); } +grpc::Status setStopAllRejected( + api::CommandHeader_Feedback* response, + const std::string& device_id) +{ + const std::string message = + "RobotArm control is temporarily paused by StopAll: " + device_id; + fillFeedback(response, false, message); + return grpc::Status(grpc::StatusCode::UNAVAILABLE, message); +} + +template +grpc::Status setStopAllRejected( + Response* response, + const std::string& device_id) +{ + return setStopAllRejected(response->mutable_header(), device_id); +} + grpc::Status setControlLeaseConflict( api::CommandHeader_Feedback* response, const std::string& device_id, @@ -169,9 +191,20 @@ public: const auto ttl = std::chrono::duration_cast< control::ControlAuthorityManager::Duration>( std::chrono::hours(24)); - auto acquired = preemptive - ? manager_.preemptAcquire(device_id, owner, ttl) - : manager_.tryAcquire(device_id, owner, ttl); + control::ControlAcquireResult acquired; + if (preemptive) { + acquired = manager_.preemptAcquire(device_id, owner, ttl); + } else { + auto admission = + globalStopAllAdmissionGate().lockAdmission(); + if (!admission.accepting()) { + rejected_by_stop_all_ = true; + detail_ = "System StopAll admission is closed"; + return; + } + admission_generation_ = admission.generation(); + acquired = manager_.tryAcquire(device_id, owner, ttl); + } acquired_ = acquired.acquired; token_ = std::move(acquired.token); detail_ = std::move(acquired.detail); @@ -182,21 +215,136 @@ public: { if (release_on_destroy_) { manager_.release(token_); + } else if (acquired_) { + (void)manager_.retireSafetyHolder(token_); } } bool acquired() const noexcept { return acquired_; } + bool rejectedByStopAll() const noexcept + { + return rejected_by_stop_all_; + } const std::string& detail() const noexcept { return detail_; } void confirmSafeToRelease() noexcept { release_on_destroy_ = true; } + bool waitForPreemptedRelease( + const control::ControlAuthorityManager::Duration timeout) + { + return manager_.waitForPreemptedRelease(token_, timeout); + } + + bool admissionCurrent() const + { + auto admission = globalStopAllAdmissionGate().lockAdmission(); + return admission.accepting() && + admission.generation() == admission_generation_; + } + + bool current() const + { + return manager_.validate(token_) && admissionCurrent(); + } + + std::function cancellationRequested( + grpc::ServerContext* context) const + { + const auto token = token_; + const auto admission_generation = admission_generation_; + return [context, token, admission_generation]() { + try { + if ((context && context->IsCancelled()) || + !control::ControlAuthorityManager::instance() + .validate(token)) { + return true; + } + auto admission = + globalStopAllAdmissionGate().lockAdmission(); + return !admission.accepting() || + admission.generation() != admission_generation; + } catch (...) { + return true; + } + }; + } + + control::ControlDispatchGuard tryBeginDispatch() + { + auto admission = globalStopAllAdmissionGate().lockAdmission(); + if (!admission.accepting() || + admission.generation() != admission_generation_) { + return {}; + } + return manager_.tryBeginDispatch(token_); + } + private: control::ControlAuthorityManager& manager_; control::ControlLeaseToken token_; std::string detail_; bool acquired_{false}; bool release_on_destroy_{true}; + bool rejected_by_stop_all_{false}; + std::uint64_t admission_generation_{0U}; }; +template +device::Result executeConfirmedArmStop( + ScopedUnaryControlLease& control_barrier, + const char* operation_name, + Operation&& operation) +{ + const auto initial_stop = operation(); + if (!initial_stop.ok()) { + return initial_stop; + } + + constexpr auto handler_release_timeout = std::chrono::seconds(15); + if (!control_barrier.waitForPreemptedRelease( + std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>( + handler_release_timeout))) { + return device::Result::failure( + device::ArmErrorCode::Timeout, + std::string(operation_name) + + " timed out waiting for the preempted control handler to exit"); + } + + const auto final_stop = operation(); + if (final_stop.ok()) { + control_barrier.confirmSafeToRelease(); + } + return final_stop; +} + +template +grpc::Status setControlAdmissionFailure( + Response* response, + const std::string& device_id, + const ScopedUnaryControlLease& lease) +{ + return lease.rejectedByStopAll() + ? setStopAllRejected(response, device_id) + : setControlLeaseConflict(response, device_id, lease.detail()); +} + +template +grpc::Status setControlDispatchFailure( + Response* response, + const std::string& device_id, + const ScopedUnaryControlLease& lease, + const char* operation) +{ + if (!lease.admissionCurrent()) { + return setStopAllRejected(response, device_id); + } + return setControlLeaseConflict( + response, + device_id, + std::string("control lease was preempted before ") + operation + + " dispatch"); +} + } // namespace gRPCArmServiceImpl::gRPCArmServiceImpl() @@ -220,10 +368,10 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*, return setControlLeaseConflict( response, device_id, control_barrier.detail()); } - const auto result = arm->torqueOff(); - if (result.ok()) { - control_barrier.confirmSafeToRelease(); - } + const auto result = executeConfirmedArmStop( + control_barrier, + "torqueOff", + [&arm]() { return arm->torqueOff(); }); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { logRpcSuccess("torqueOff", device_id); @@ -248,8 +396,13 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "torqueOn"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { + return setControlDispatchFailure( + response, device_id, control_lease, "torqueOn"); } const auto result = arm->torqueOn(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); @@ -263,7 +416,7 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*, } } -grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*, +grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext* context, const api::MoveJ_Request* request, api::MoveJ_Response* response) { @@ -276,11 +429,18 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "moveJ"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); } - const auto result = arm->moveJ(toJointPositionCommand(request->target()), - toMotionOptions(request->options())); + if (!control_lease.current()) { + return setControlDispatchFailure( + response, device_id, control_lease, "moveJ"); + } + auto options = toMotionOptions( + request->options(), + control_lease.cancellationRequested(context)); + const auto result = arm->moveJ( + toJointPositionCommand(request->target()), options); if (result.ok()) { CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveJ): success, id=" << device_id << ", positions=" << request->target().position_size(); @@ -292,7 +452,7 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*, } } -grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*, +grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext* context, const api::MoveL_Request* request, api::MoveL_Response* response) { @@ -305,12 +465,20 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "moveL"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); } - const auto result = arm->moveL(toCartesianPose(request->target()), - toMotionOptions(request->options()), - toFrameType(request->frame())); + if (!control_lease.current()) { + return setControlDispatchFailure( + response, device_id, control_lease, "moveL"); + } + auto options = toMotionOptions( + request->options(), + control_lease.cancellationRequested(context)); + const auto result = arm->moveL( + toCartesianPose(request->target()), + options, + toFrameType(request->frame())); if (result.ok()) { CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveL): success, id=" << device_id << ", frame=" << request->frame(); @@ -335,8 +503,12 @@ grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "speedJ"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + if (!control_lease.current()) { + return setControlDispatchFailure( + response, device_id, control_lease, "speedJ"); } const auto result = arm->speedJ(toJointVelocityCommand(request->velocity()), request->acceleration(), @@ -367,8 +539,12 @@ grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "speedL"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + if (!control_lease.current()) { + return setControlDispatchFailure( + response, device_id, control_lease, "speedL"); } const auto result = arm->speedL(toCartesianVelocity(request->velocity()), request->acceleration(), @@ -400,8 +576,13 @@ grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "servoJ"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { + return setControlDispatchFailure( + response, device_id, control_lease, "servoJ"); } const auto result = arm->servoJ(toJointPositionCommand(request->target())); if (result.ok()) { @@ -431,10 +612,10 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*, return setControlLeaseConflict( response, device_id, control_barrier.detail()); } - const auto result = arm->stopMotion(); - if (result.ok()) { - control_barrier.confirmSafeToRelease(); - } + const auto result = executeConfirmedArmStop( + control_barrier, + "stopMotion", + [&arm]() { return arm->stopMotion(); }); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { logRpcSuccess("stopMotion", device_id); @@ -514,8 +695,12 @@ grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "calibrateZeroQ"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + if (!control_lease.current()) { + return setControlDispatchFailure( + response, device_id, control_lease, "calibrateZeroQ"); } const auto result = arm->calibrateZeroQ(request->joint_name()); if (result.ok()) { @@ -561,6 +746,18 @@ grpc::Status gRPCArmServiceImpl::ExecuteJsonCommand( return grpc::Status::OK; } + ScopedUnaryControlLease control_lease( + device_id, "ExecuteJsonCommand"); + if (!control_lease.acquired()) { + return setControlAdmissionFailure( + response, device_id, control_lease); + } + if (!control_lease.current()) { + return setControlDispatchFailure( + response, device_id, control_lease, + "ExecuteJsonCommand"); + } + std::string response_json; const bool success = arm->executeJsonCommand( request->request_json(), response_json); @@ -592,8 +789,13 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context, ScopedUnaryControlLease control_lease( device_id, "clearFault"); if (!control_lease.acquired()) { - return setControlLeaseConflict( - response, device_id, control_lease.detail()); + return setControlAdmissionFailure( + response, device_id, control_lease); + } + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { + return setControlDispatchFailure( + response, device_id, control_lease, "clearFault"); } const auto result = arm->clearFault(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); 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 21e301d1..c90c75b3 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_teleop_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_teleop_service.cpp @@ -14,6 +14,8 @@ #include #include +#include "service/stop_all/include/stop_all_admission_gate.h" + namespace cmvr::service { namespace { @@ -497,10 +499,22 @@ grpc::Status ArmTeleopServiceImpl::Teleoperate( } const std::string session_id = nextSessionId(); - const auto acquired = authority_->tryAcquire( - backend_manifest.robot_id(), - session_id, - std::chrono::milliseconds(negotiated.lease_ms)); + control::ControlAcquireResult acquired; + { + auto admission = + globalStopAllAdmissionGate().lockAdmission(); + if (!admission.accepting()) { + const grpc::Status status( + grpc::StatusCode::UNAVAILABLE, + "arm teleoperation is temporarily paused by StopAll"); + writeBareRejection(stream, status); + return status; + } + acquired = authority_->tryAcquire( + backend_manifest.robot_id(), + session_id, + std::chrono::milliseconds(negotiated.lease_ms)); + } if (!acquired.acquired) { const grpc::Status status( grpc::StatusCode::RESOURCE_EXHAUSTED, @@ -524,7 +538,7 @@ grpc::Status ArmTeleopServiceImpl::Teleoperate( return status; } - bool backend_open_attempted = true; + bool backend_open_attempted = false; bool backend_stopped = false; const auto safeStop = [&](const arm_teleop::StopReason reason, @@ -552,7 +566,20 @@ grpc::Status ArmTeleopServiceImpl::Teleoperate( "arm teleoperation handler terminated unexpectedly"); }); - const auto backend_open = backend_->open(first_frame.open()); + ArmTeleopBackendResult backend_open; + { + auto dispatch = + authority_->tryBeginDispatch(control_lease); + if (!dispatch.acquired()) { + const grpc::Status status( + grpc::StatusCode::ABORTED, + "arm teleoperation control authority was revoked before backend open"); + writeBareRejection(stream, status); + return status; + } + backend_open_attempted = true; + backend_open = backend_->open(first_frame.open()); + } if (!backend_open.success) { const auto stopped = safeStop( arm_teleop::STOP_REASON_PROTOCOL_ERROR, @@ -793,6 +820,17 @@ grpc::Status ArmTeleopServiceImpl::Teleoperate( status.error_message() + "; " + stopped.detail); } + if (!authority_->validate(control_lease)) { + const std::string detail = + "arm teleoperation control authority was revoked"; + return finish( + arm_teleop::SESSION_PHASE_LEASE_LOST, + arm_teleop::STOP_REASON_LEASE_REVOKED, + detail, + grpc::Status( + grpc::StatusCode::ABORTED, detail), + true); + } std::optional pending; bool ended = false; @@ -1064,8 +1102,24 @@ grpc::Status ArmTeleopServiceImpl::Teleoperate( session.lease_deadline = pending->arrived + std::chrono::milliseconds(session.lease_ms); - const auto applied = - backend_->applySetpoint(setpoint, command_deadline); + ArmTeleopBackendResult applied; + { + auto dispatch = + authority_->tryBeginDispatch(control_lease); + if (!dispatch.acquired()) { + const std::string detail = + "arm teleoperation control authority was revoked"; + return finish( + arm_teleop::SESSION_PHASE_LEASE_LOST, + arm_teleop::STOP_REASON_LEASE_REVOKED, + detail, + grpc::Status( + grpc::StatusCode::ABORTED, detail), + true); + } + applied = backend_->applySetpoint( + setpoint, command_deadline); + } if (!applied.success) { ++session.rejected_setpoints; return finish( diff --git a/cmvr-es/service/grpc/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/src/grpc_camera_service.cpp index 5ca371ef..f0a4dc64 100644 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_camera_service.cpp @@ -1,5 +1,8 @@ #include "common/base/logging/logger.h" #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/media_activity_coordinator.h" // // Created by xtkuang on 2025/6/1. // @@ -72,9 +75,13 @@ bool toPtzCommand(cmvr::api::ControlPtzCommand_Command command, PtzCommand& out) // the one startStreaming() reference acquired by this call. class CameraStreamingLease final { public: - explicit CameraStreamingLease(std::shared_ptr camera) + CameraStreamingLease( + std::shared_ptr camera, + const MediaActivityCoordinator::Session& session) : camera_(std::move(camera)) { - active_ = camera_ && camera_->startStreaming(); + (void)session.runIfCurrent([this] { + active_ = camera_ && camera_->startStreaming(); + }); } ~CameraStreamingLease() { @@ -100,6 +107,25 @@ private: std::shared_ptr camera_; bool active_{false}; }; + +grpc::Status mediaStoppedStatus() +{ + return grpc::Status( + grpc::StatusCode::CANCELLED, + "Media activity stopped by StopAll"); +} + +template +grpc::Status rejectStreamDuringStopAll(StreamT* stream) +{ + FeedbackT response; + response.mutable_header()->set_success(false); + response.mutable_header()->set_error_message( + "Media activities are temporarily paused by StopAll"); + setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); + stream->Write(response); + return grpc::Status::OK; +} } gRPCCameraServiceImpl::gRPCCameraServiceImpl( @@ -143,6 +169,11 @@ 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 { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StartCamera): id=" << dev_id; @@ -150,7 +181,24 @@ grpc::Status gRPCCameraServiceImpl::StartCamera(grpc::ServerContext* context, if (!dev) { return failResponse(response, "Camera device not found: " + dev_id); } - if (!dev->start()) { + CameraOperationalActivityRegistry::DispatchResult dispatch = + CameraOperationalActivityRegistry::DispatchResult::DeviceFailure; + const bool start_allowed = media_session.runIfCurrent([&] { + dispatch = globalCameraOperationalActivityRegistry().start( + dev_id, dev); + }); + if (!start_allowed) { + return failResponse( + response, "Camera start was canceled by StopAll"); + } + if (dispatch == + CameraOperationalActivityRegistry::DispatchResult:: + RejectedByStopAll) { + return failResponse( + response, "Camera start was canceled by StopAll"); + } + if (dispatch == + CameraOperationalActivityRegistry::DispatchResult::DeviceFailure) { return failResponse(response, "Failed to start camera: " + dev_id); } response->mutable_header()->set_success(true); @@ -175,9 +223,20 @@ grpc::Status gRPCCameraServiceImpl::StopCamera(grpc::ServerContext* context, if (!dev) { return failResponse(response, "Camera device not found: " + dev_id); } - if (!dev->stop()) { + const auto dispatch = + globalCameraOperationalActivityRegistry().stopLifecycle( + dev_id, dev); + if (dispatch == + CameraOperationalActivityRegistry::DispatchResult:: + RejectedByStopAll) { + return failResponse( + response, "Camera control is temporarily paused by StopAll"); + } + if (dispatch == + CameraOperationalActivityRegistry::DispatchResult::DeviceFailure) { return failResponse(response, "Failed to stop camera: " + dev_id); } + globalCameraPtzActivityRegistry().markCameraStopped(dev_id); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); return grpc::Status::OK; @@ -193,6 +252,16 @@ grpc::Status gRPCCameraServiceImpl::StopCamera(grpc::ServerContext* context, grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context, const api::GetRGBImageCommand_Request* request, api::GetRGBImageCommand_Feedback* response) { + auto media_session = globalMediaActivityCoordinator().beginSession( + [context] { + if (context) { + context->TryCancel(); + } + }); + if (!media_session) { + return failResponse( + response, "Camera capture is temporarily paused by StopAll"); + } try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImage): id=" << dev_id; @@ -202,7 +271,11 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context, return failResponse(response, "Camera device not found: " + dev_id); } Rs2Intrinsics intrinsics = {0}; - dev->getRGBImage(image,intrinsics); + if (!media_session.runIfCurrent( + [&] { dev->getRGBImage(image, intrinsics); })) { + return failResponse( + response, "Camera capture was canceled by StopAll"); + } response->mutable_header()->set_success(true); response->mutable_intrinsics()->set_fx(intrinsics.fx); @@ -246,6 +319,16 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context, grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context, const api::GetDepthImageCommand_Request* request, api::GetDepthImageCommand_Feedback* response) { + auto media_session = globalMediaActivityCoordinator().beginSession( + [context] { + if (context) { + context->TryCancel(); + } + }); + if (!media_session) { + return failResponse( + response, "Camera capture is temporarily paused by StopAll"); + } try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImage): id=" << dev_id; @@ -255,7 +338,11 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context, return failResponse(response, "Camera device not found: " + dev_id); } Rs2Intrinsics intrinsics = {0}; - dev->getDepthImage(image,intrinsics); + if (!media_session.runIfCurrent( + [&] { dev->getDepthImage(image, intrinsics); })) { + return failResponse( + response, "Camera capture was canceled by StopAll"); + } response->mutable_header()->set_success(true); response->mutable_intrinsics()->set_fx(intrinsics.fx); @@ -302,6 +389,16 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context, grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, const api::GetRGBDImagesCommand_Request* request, api::GetRGBDImagesCommand_Feedback* response) { + auto media_session = globalMediaActivityCoordinator().beginSession( + [context] { + if (context) { + context->TryCancel(); + } + }); + if (!media_session) { + return failResponse( + response, "Camera capture is temporarily paused by StopAll"); + } try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImages): id=" << dev_id; @@ -311,7 +408,14 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, return failResponse(response, "Camera device not found: " + dev_id); } Rs2Intrinsics intrinsics = {0}; - dev->getRGBDImages(color_image,depth_image, intrinsics); + if (!media_session.runIfCurrent( + [&] { + dev->getRGBDImages( + color_image, depth_image, intrinsics); + })) { + return failResponse( + response, "Camera capture was canceled by StopAll"); + } response->mutable_header()->set_success(true); response->mutable_intrinsics()->set_fx(intrinsics.fx); @@ -371,6 +475,11 @@ 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 { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StartRecording): id=" << dev_id; @@ -378,7 +487,12 @@ grpc::Status gRPCCameraServiceImpl::StartRecording(grpc::ServerContext* context, if (!dev) { return failResponse(response, "Camera device not found: " + dev_id); } - dev->startRecording(request->video_path()); + if (!media_session.runIfCurrent([&] { + dev->startRecording(request->video_path()); + })) { + return failResponse( + response, "Camera recording start was canceled by StopAll"); + } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); return grpc::Status::OK; @@ -438,7 +552,17 @@ grpc::Status gRPCCameraServiceImpl::ControlPtz(grpc::ServerContext* context, return failResponse(response, "Invalid PTZ action"); } const bool stop = request->action() == api::ControlPtzCommand_Action_STOP; - if (!dev->controlPtz(command, stop, static_cast(request->speed()))) { + const auto dispatch = globalCameraPtzActivityRegistry().control( + dev_id, + dev, + command, + stop, + static_cast(request->speed())); + if (dispatch == CameraPtzActivityRegistry::DispatchResult::RejectedByStopAll) { + return failResponse( + response, "PTZ control is temporarily paused by StopAll"); + } + if (dispatch == CameraPtzActivityRegistry::DispatchResult::DeviceFailure) { CameraState state{}; dev->getState(state); const std::string error_message = @@ -461,11 +585,19 @@ grpc::Status gRPCCameraServiceImpl::ControlPtz(grpc::ServerContext* context, grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* context , grpc::ServerReaderWriter* stream){ + auto media_session = globalMediaActivityCoordinator().beginSession( + [context] { context->TryCancel(); }); + if (!media_session) { + return rejectStreamDuringStopAll< + api::GetDepthImageStreamCommand_Feedback>(stream); + } try { //读取首次传递的数据，获取设备id api::GetDepthImageStreamCommand_Request request; if (!stream->Read(&request)) { - return grpc::Status::OK; + return media_session.cancelled() + ? mediaStoppedStatus() + : grpc::Status::OK; } string dev_id = request.header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImageStream): start,id=" << dev_id; @@ -478,7 +610,7 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con stream->Write(response); return grpc::Status::OK; } - CameraStreamingLease stream_lease(dev); + CameraStreamingLease stream_lease(dev, media_session); if (!stream_lease) { api::GetDepthImageStreamCommand_Feedback response; response.mutable_header()->set_success(false); @@ -492,7 +624,7 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con size_t index = 0; while (true) { - if (context->IsCancelled()) + if (media_session.cancelled() || context->IsCancelled()) { CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled,id=" << dev_id; break; @@ -501,6 +633,7 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con api::GetDepthImageStreamCommand_Feedback response; cmvr::device::StreamFrameData frame_data; if (dev->waitEncodedFrame(frame_data, index, std::chrono::milliseconds(100)) && + !media_session.cancelled() && !frame_data.depthFrame.empty()) { response.mutable_header()->set_success(true); setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); @@ -527,9 +660,14 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con } } CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImageStream): end,id=" << dev_id; - return grpc::Status::OK; + return media_session.cancelled() + ? mediaStoppedStatus() + : grpc::Status::OK; } catch (const exception &e) { + if (media_session.cancelled()) { + return mediaStoppedStatus(); + } api::GetDepthImageStreamCommand_Feedback response; response.mutable_header()->set_success(false); response.mutable_header()->set_error_message(e.what()); @@ -540,11 +678,19 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con } grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* context , grpc::ServerReaderWriter* stream){ + auto media_session = globalMediaActivityCoordinator().beginSession( + [context] { context->TryCancel(); }); + if (!media_session) { + return rejectStreamDuringStopAll< + api::GetRGBDImagesStreamCommand_Feedback>(stream); + } try { //读取首次传递的数据，获取设备id api::GetRGBDImagesStreamCommand_Request request; if (!stream->Read(&request)) { - return grpc::Status::OK; + return media_session.cancelled() + ? mediaStoppedStatus() + : grpc::Status::OK; } string dev_id = request.header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): start,id=" << dev_id; @@ -557,7 +703,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con stream->Write(response); return grpc::Status::OK; } - CameraStreamingLease stream_lease(dev); + CameraStreamingLease stream_lease(dev, media_session); if (!stream_lease) { api::GetRGBDImagesStreamCommand_Feedback response; response.mutable_header()->set_success(false); @@ -571,7 +717,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con size_t index = 0; while (true) { - if (context->IsCancelled()) + if (media_session.cancelled() || context->IsCancelled()) { CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBDImagesStream) context is cancelled,id=" << dev_id; break; @@ -580,6 +726,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con api::GetRGBDImagesStreamCommand_Feedback response; cmvr::device::StreamFrameData frame_data; if (dev->waitEncodedFrame(frame_data, index, std::chrono::milliseconds(100)) && + !media_session.cancelled() && !frame_data.rgbFrame.empty() && !frame_data.depthFrame.empty()) { response.mutable_header()->set_success(true); @@ -613,9 +760,14 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con } } CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): end,id=" << dev_id; - return grpc::Status::OK; + return media_session.cancelled() + ? mediaStoppedStatus() + : grpc::Status::OK; } catch (const exception &e) { + if (media_session.cancelled()) { + return mediaStoppedStatus(); + } api::GetRGBDImagesStreamCommand_Feedback response; response.mutable_header()->set_success(false); response.mutable_header()->set_error_message(e.what()); @@ -625,11 +777,19 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con } } grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream){ + auto media_session = globalMediaActivityCoordinator().beginSession( + [context] { context->TryCancel(); }); + if (!media_session) { + return rejectStreamDuringStopAll< + api::GetRGBImageStreamCommand_Feedback>(stream); + } try { //读取首次传递的数据，获取设备id api::GetRGBImageStreamCommand_Request request; if (!stream->Read(&request)) { - return grpc::Status::OK; + return media_session.cancelled() + ? mediaStoppedStatus() + : grpc::Status::OK; } string dev_id = request.header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id @@ -646,10 +806,17 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte } auto& media_hub = cmvr::media::globalMediaSourceHub(); const std::string track_id = cmvr::media::cameraColorTrackId(dev_id); - if (!cmvr::media::ensureCameraMediaSource(media_hub, dev)) { + bool source_ready = false; + const bool source_setup_allowed = media_session.runIfCurrent([&] { + source_ready = cmvr::media::ensureCameraMediaSource(media_hub, dev); + }); + if (!source_setup_allowed || !source_ready) { api::GetRGBImageStreamCommand_Feedback response; response.mutable_header()->set_success(false); - response.mutable_header()->set_error_message("Failed to register camera media source: " + dev_id); + response.mutable_header()->set_error_message( + media_session.cancelled() + ? "Camera stream start was canceled by StopAll" + : "Failed to register camera media source: " + dev_id); setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); stream->Write(response); return grpc::Status::OK; @@ -657,7 +824,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte auto subscription = media_hub.subscribe( track_id, cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED, - [context] { return context->IsCancelled(); }); + [context, &media_session] { + return context->IsCancelled() || media_session.cancelled(); + }); if (!subscription) { api::GetRGBImageStreamCommand_Feedback response; response.mutable_header()->set_success(false); @@ -713,13 +882,16 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte }; while (true) { - if (context->IsCancelled()) + if (media_session.cancelled() || context->IsCancelled()) { CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled,id=" << dev_id; break; } const auto read = subscription.waitRead(std::chrono::milliseconds(100)); + if (media_session.cancelled()) { + break; + } if (!read || !read->value || read->value->empty()) { if (!subscription.valid()) { break; @@ -807,7 +979,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte static_cast(std::numeric_limits::max())))); const auto write_started = std::chrono::steady_clock::now(); - if (!stream->Write(response)) { + if (media_session.cancelled() || !stream->Write(response)) { CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id; break; } @@ -830,9 +1002,14 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte } } CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end,id=" << dev_id; - return grpc::Status::OK; + return media_session.cancelled() + ? mediaStoppedStatus() + : grpc::Status::OK; } catch (const exception &e) { + if (media_session.cancelled()) { + return mediaStoppedStatus(); + } api::GetRGBImageStreamCommand_Feedback response; response.mutable_header()->set_success(false); response.mutable_header()->set_error_message(e.what()); diff --git a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp b/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp index 84bf5003..6149aa6f 100644 --- a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp @@ -5,13 +5,19 @@ #include "../include/grpc_dexhand_service.h" +#include #include #include +#include #include #include +#include #include #include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h" +#include "manager/control_authority/include/control_authority_manager.h" +#include "service/grpc/include/media_activity_coordinator.h" +#include "service/stop_all/include/stop_all_admission_gate.h" using namespace std; using namespace cmvr::service; @@ -121,6 +127,124 @@ grpc::Status failResponse(ResponseT* response, const std::string& message) { return grpc::Status::OK; } +class ScopedDexHandControlLease final { +public: + ScopedDexHandControlLease(const std::string& device_id, + const char* operation) + : manager_(cmvr::control::ControlAuthorityManager::instance()) { + static std::atomic sequence{0U}; + const std::string owner = + std::string("grpc-dexhand-unary:") + operation + ":" + + std::to_string( + sequence.fetch_add(1U, std::memory_order_relaxed) + 1U); + const auto ttl = std::chrono::duration_cast< + cmvr::control::ControlAuthorityManager::Duration>( + std::chrono::hours(24)); + + auto admission = globalStopAllAdmissionGate().lockAdmission(); + if (!admission.accepting()) { + rejected_by_stop_all_ = true; + detail_ = "System StopAll admission is closed"; + return; + } + admission_generation_ = admission.generation(); + + auto acquired = manager_.tryAcquire(device_id, owner, ttl); + acquired_ = acquired.acquired; + token_ = std::move(acquired.token); + detail_ = std::move(acquired.detail); + } + + ~ScopedDexHandControlLease() { + manager_.release(token_); + } + + bool acquired() const noexcept { return acquired_; } + bool rejectedByStopAll() const noexcept { + return rejected_by_stop_all_; + } + const std::string& detail() const noexcept { return detail_; } + + bool admissionCurrent() const { + auto admission = globalStopAllAdmissionGate().lockAdmission(); + return admission.accepting() && + admission.generation() == admission_generation_; + } + + cmvr::control::ControlDispatchGuard tryBeginDispatch() { + auto admission = globalStopAllAdmissionGate().lockAdmission(); + if (!admission.accepting() || + admission.generation() != admission_generation_) { + return {}; + } + return manager_.tryBeginDispatch(token_); + } + +private: + cmvr::control::ControlAuthorityManager& manager_; + cmvr::control::ControlLeaseToken token_; + std::string detail_; + bool acquired_{false}; + bool rejected_by_stop_all_{false}; + std::uint64_t admission_generation_{0U}; +}; + +template +grpc::Status failControlAdmission( + ResponseT* response, + const std::string& device_id, + const ScopedDexHandControlLease& lease) { + if (lease.rejectedByStopAll()) { + return failResponse( + response, + "DexHand control is temporarily paused by StopAll: " + device_id); + } + return failResponse( + response, + "DexHand control is leased by another active operation: " + + device_id + + (lease.detail().empty() ? "" : " (" + lease.detail() + ")")); +} + +template +grpc::Status failControlDispatch( + ResponseT* response, + const std::string& device_id, + const ScopedDexHandControlLease& lease) { + if (!lease.admissionCurrent()) { + return failResponse( + response, + "DexHand control was preempted by StopAll: " + device_id); + } + return failResponse( + response, + "DexHand control lease was preempted before device dispatch: " + + device_id); +} + +template +bool dispatchDexHandCommand( + ResponseT* response, + const std::string& device_id, + const std::shared_ptr& dev, + ScopedDexHandControlLease& lease, + Operation&& operation) { + auto dispatch = lease.tryBeginDispatch(); + if (!dispatch.acquired()) { + (void)failControlDispatch(response, device_id, lease); + return false; + } + if (!dev->resumeOperationalActivity()) { + (void)failResponse( + response, + "DexHand operational activity could not be resumed: " + + device_id); + return false; + } + std::forward(operation)(); + return true; +} + std::vector readCurrentAngles(const std::shared_ptr& dev) { DexHandState state{}; dev->getState(state); @@ -234,6 +358,10 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context if (!dev) { return failResponse(response, "DexHand device not found: " + dev_id); } + ScopedDexHandControlLease control_lease(dev_id, "SetDexHandPos"); + if (!control_lease.acquired()) { + return failControlAdmission(response, dev_id, control_lease); + } if (respondUnsupportedForRh56(dev, "SetDexHandPos", "Use SetDexHandAngle for RH56 joint commands.", @@ -246,7 +374,14 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context if (!applyFreedomValues(request->values(), DEXHAND_MAX_POSITION, finger_joint_targets, &error_message)) { return failResponse(response, error_message); } - dev->setPositions(finger_joint_targets); + if (!dispatchDexHandCommand( + response, + dev_id, + dev, + control_lease, + [&] { dev->setPositions(finger_joint_targets); })) { + return grpc::Status::OK; + } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandPos): success, id=" << dev_id @@ -271,6 +406,10 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* contex if (!dev) { return failResponse(response, "DexHand device not found: " + dev_id); } + ScopedDexHandControlLease control_lease(dev_id, "SetDexHandAngle"); + if (!control_lease.acquired()) { + return failControlAdmission(response, dev_id, control_lease); + } if (const auto rh56 = std::dynamic_pointer_cast(dev)) { std::vector finger_joint_targets = readCurrentAngles(dev); @@ -278,14 +417,28 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* contex if (!applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets, &error_message)) { return failResponse(response, error_message); } - rh56->setAngles(finger_joint_targets); + if (!dispatchDexHandCommand( + response, + dev_id, + dev, + control_lease, + [&] { rh56->setAngles(finger_joint_targets); })) { + return grpc::Status::OK; + } } else { std::vector finger_joint_targets(static_cast(kDexHandDofCount), -1); std::string error_message; if (!applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets, &error_message)) { return failResponse(response, error_message); } - dev->setAngles(finger_joint_targets); + if (!dispatchDexHandCommand( + response, + dev_id, + dev, + control_lease, + [&] { dev->setAngles(finger_joint_targets); })) { + return grpc::Status::OK; + } } response->mutable_header()->set_success(true); @@ -312,6 +465,10 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* contex if (!dev) { return failResponse(response, "DexHand device not found: " + dev_id); } + ScopedDexHandControlLease control_lease(dev_id, "SetDexHandForce"); + if (!control_lease.acquired()) { + return failControlAdmission(response, dev_id, control_lease); + } if (respondUnsupportedForRh56(dev, "SetDexHandForce", "RH56DFTPDexhand currently exposes angle and tactile APIs only.", @@ -324,7 +481,14 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* contex if (!applyFreedomValues(request->values(), DEXHAND_MAX_FORCE, finger_joint_targets, &error_message)) { return failResponse(response, error_message); } - dev->setForce(finger_joint_targets); + if (!dispatchDexHandCommand( + response, + dev_id, + dev, + control_lease, + [&] { dev->setForce(finger_joint_targets); })) { + return grpc::Status::OK; + } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandForce): success, id=" << dev_id @@ -349,6 +513,10 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* contex if (!dev) { return failResponse(response, "DexHand device not found: " + dev_id); } + ScopedDexHandControlLease control_lease(dev_id, "SetDexHandSpeed"); + if (!control_lease.acquired()) { + return failControlAdmission(response, dev_id, control_lease); + } if (respondUnsupportedForRh56(dev, "SetDexHandSpeed", "RH56DFTPDexhand currently exposes angle and tactile APIs only.", @@ -361,7 +529,14 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* contex if (!applyFreedomValues(request->values(), DEXHAND_MAX_SPEED, finger_joint_targets, &error_message)) { return failResponse(response, error_message); } - dev->setVelocities(finger_joint_targets); + if (!dispatchDexHandCommand( + response, + dev_id, + dev, + control_lease, + [&] { dev->setVelocities(finger_joint_targets); })) { + return grpc::Status::OK; + } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandSpeed): success, id=" << dev_id @@ -386,6 +561,11 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPresetAct(grpc::ServerContext* co if (!dev) { return failResponse(response, "DexHand device not found: " + dev_id); } + ScopedDexHandControlLease control_lease( + dev_id, "SetDexHandPresetAct"); + if (!control_lease.acquired()) { + return failControlAdmission(response, dev_id, control_lease); + } if (respondUnsupportedForRh56(dev, "SetDexHandPresetAct", "RH56DFTPDexhand currently exposes angle and tactile APIs only.", @@ -394,7 +574,14 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPresetAct(grpc::ServerContext* co } auto presetActId = request->presetactid(); - dev->setPresetAct(presetActId); + if (!dispatchDexHandCommand( + response, + dev_id, + dev, + control_lease, + [&] { dev->setPresetAct(presetActId); })) { + return grpc::Status::OK; + } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandPresetAct): success, id=" << dev_id @@ -413,6 +600,13 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context , const cmvr::api::GetSensorDataCommand_Request* request , cmvr::api::GetSensorDataCommand_Feedback* response) { + auto media_session = globalMediaActivityCoordinator().beginSession(); + if (!media_session) { + return failResponse( + response, + "DexHand sensor activity is temporarily paused by StopAll"); + } + try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorData): id=" << dev_id; @@ -420,8 +614,28 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context if (!dev) { return failResponse(response, "DexHand device not found: " + dev_id); } - maybeConfigureRh56FullTactilePolling(dev); - appendSensorData(dev->getSensorData(), response); + + bool resumed = false; + std::vector sensor_data; + if (!media_session.runIfCurrent([&] { + resumed = dev->resumeOperationalActivity(); + if (!resumed) { + return; + } + maybeConfigureRh56FullTactilePolling(dev); + sensor_data = dev->getSensorData(); + })) { + return failResponse( + response, + "DexHand sensor activity was preempted by StopAll: " + + dev_id); + } + if (!resumed) { + return failResponse( + response, + "DexHand sensor activity could not be resumed: " + dev_id); + } + appendSensorData(sensor_data, response); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorData): success, id=" << dev_id @@ -439,6 +653,18 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* context , grpc::ServerReaderWriter* stream) { + auto media_session = globalMediaActivityCoordinator().beginSession( + [context] { + if (context) { + context->TryCancel(); + } + }); + if (!media_session) { + return grpc::Status( + grpc::StatusCode::UNAVAILABLE, + "DexHand sensor stream is temporarily paused by StopAll"); + } + try { api::GetSensorDataStreamCommand_Request request; if (!stream->Read(&request)) { @@ -456,16 +682,46 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co stream->Write(response); return grpc::Status::OK; } - maybeConfigureRh56FullTactilePolling(dev); + + bool resumed = false; + if (!media_session.runIfCurrent([&] { + resumed = dev->resumeOperationalActivity(); + if (resumed) { + maybeConfigureRh56FullTactilePolling(dev); + } + })) { + return grpc::Status( + grpc::StatusCode::CANCELLED, + "DexHand sensor stream was preempted by StopAll"); + } + if (!resumed) { + api::GetSensorDataStreamCommand_Feedback response; + response.mutable_header()->set_success(false); + response.mutable_header()->set_error_message( + "DexHand sensor activity could not be resumed: " + dev_id); + setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); + stream->Write(response); + return grpc::Status::OK; + } CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): streaming success, id=" << dev_id; - while (!context->IsCancelled()) + while (!media_session.cancelled() && + !(context && context->IsCancelled())) { api::GetSensorDataStreamCommand_Feedback response; - appendSensorData(dev->getSensorData(), &response); + std::vector sensor_data; + if (!media_session.runIfCurrent( + [&] { sensor_data = dev->getSensorData(); })) { + break; + } + appendSensorData(sensor_data, &response); response.mutable_header()->set_success(true); setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); + if (media_session.cancelled() || + (context && context->IsCancelled())) { + break; + } if (!stream->Write(response)) { CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (stream->Write) failed,id=" << dev_id; break; diff --git a/cmvr-es/service/grpc/src/grpc_head_service.cpp b/cmvr-es/service/grpc/src/grpc_head_service.cpp index 6ad3aff8..9ba03019 100644 --- a/cmvr-es/service/grpc/src/grpc_head_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_head_service.cpp @@ -5,9 +5,12 @@ #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/media_activity_coordinator.h" +#include "service/stop_all/include/stop_all_admission_gate.h" #include #include #include +#include using namespace std; using namespace cmvr::service; @@ -28,6 +31,25 @@ void logSuccess(const char* rpc_name, const std::string& device_id) { CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (" << rpc_name << "): success, id=" << device_id; } + +std::optional admitHeadCommand( + const std::shared_ptr& robot) +{ + auto admission = globalStopAllAdmissionGate().lockAdmission(); + if (!admission.accepting() || !robot) { + return std::nullopt; + } + return robot->beginOperationalActivity(); +} + +template +grpc::Status failStoppedCommand(ResponseT* response) +{ + return failResponse( + response, + "Biohead command was rejected because StopAll is in progress or " + "the command was preempted"); +} } gRPCMBioHeadServiceImpl::gRPCMBioHeadServiceImpl() @@ -47,7 +69,12 @@ grpc::Status gRPCMBioHeadServiceImpl::SetExpression( return failResponse(response, "Biohead device not found: " + dev_id); } - FacialExpressionState& expression_state = robot->expression_state_; + 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(); @@ -70,7 +97,10 @@ grpc::Status gRPCMBioHeadServiceImpl::SetExpression( expression_state.upper_lip_y = request->expression().mouth().upper_lip_y(); expression_state.lower_lip_y = request->expression().mouth().lower_lip_y(); - robot->setExpressionPose(expression_state); + if (!robot->setExpressionPoseIfCurrent( + *activity, expression_state)) { + return failStoppedCommand(response); + } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -94,12 +124,25 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression( std::string dev_id; std::shared_ptr robot; bool first_message = true; + AbstractBiohead::OperationalToken activity{0U}; + auto media_session = globalMediaActivityCoordinator().beginSession( + [context] { + if (context) { + context->TryCancel(); + } + }); + if (!media_session) { + return grpc::Status( + grpc::StatusCode::UNAVAILABLE, + "Biohead stream rejected because StopAll is in progress"); + } try { StreamFacialExpression_Request request_msg; constexpr float control_frequency = 10; const auto time_interval = std::chrono::milliseconds(static_cast(1000 / control_frequency)); - auto last_control_time = std::chrono::steady_clock::now(); + auto last_control_time = + std::chrono::steady_clock::now() - time_interval; CMVR_LOG(INFO) << "StreamExpression started."; @@ -125,15 +168,26 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression( return grpc::Status::OK; } - // ✅ 重置紧急停止标志 - robot->emergency_stop_requested = false; + const auto admitted = admitHeadCommand(robot); + if (!admitted) { + feedback_msg.mutable_header()->set_success(false); + feedback_msg.mutable_header()->set_error_message( + "Biohead stream rejected because StopAll is in " + "progress"); + setCurrentTimestamp( + feedback_msg.mutable_header()->mutable_timestamp()); + stream->Write(feedback_msg); + return grpc::Status::OK; + } + activity = *admitted; first_message = false; CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (StreamExpression): streaming success, id=" << dev_id; } // ✅ 如果紧急停止触发，直接退出 - if (robot->emergency_stop_requested) { + if (media_session.cancelled() || + (context && context->IsCancelled())) { CMVR_LOG(WARNING) << "[Stream] Emergency stop requested. Terminating stream for device: " << dev_id; break; } @@ -181,7 +235,18 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression( expression_state.jaw_x = request_msg.expr().jaw().x(); expression_state.jaw_y = request_msg.expr().jaw().y(); - robot->streamFacialPose(expression_state, 0, 0); + bool dispatched = false; + const bool current_session = media_session.runIfCurrent([&] { + dispatched = robot->streamFacialPoseIfCurrent( + activity, expression_state, 0, 0); + }); + if (!current_session || !dispatched) { + CMVR_LOG(WARNING) + << "[gRPCMBioHeadServiceImpl] StreamExpression was " + "preempted, id=" + << dev_id; + break; + } last_control_time = current_time; feedback_msg.mutable_header()->set_success(true); @@ -245,13 +310,12 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop( return failResponse(response, "Biohead device not found: " + dev_id); } - robot->eStop(); // 停止执行 - robot->emergency_stop_requested = true; // ✅ 设置中断标志 - - - - - + 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); @@ -279,7 +343,10 @@ grpc::Status gRPCMBioHeadServiceImpl::SpeakStart(grpc::ServerContext* context, c return failResponse(response, "Biohead device not found: " + dev_id); } - robot->speakstart(); // kaish开始 + const auto activity = admitHeadCommand(robot); + if (!activity || !robot->speakStartIfCurrent(*activity)) { + return failStoppedCommand(response); + } response->mutable_header()->set_success(true); @@ -333,7 +400,10 @@ grpc::Status gRPCMBioHeadServiceImpl::Happy(grpc::ServerContext* context, const return failResponse(response, "Biohead device not found: " + dev_id); } - robot->expressionHappy(); // 停止执行 + 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()); @@ -357,7 +427,10 @@ grpc::Status gRPCMBioHeadServiceImpl::Surprise(grpc::ServerContext* context, con return failResponse(response, "Biohead device not found: " + dev_id); } - robot->expressionSurprised(); // + 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()); @@ -382,7 +455,10 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionTired(grpc::ServerContext* conte return failResponse(response, "Biohead device not found: " + dev_id); } - robot->expressionTired(); // 停止执行 + 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()); @@ -408,7 +484,10 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionAngry(grpc::ServerContext* conte return failResponse(response, "Biohead device not found: " + dev_id); } - robot->expressionAngry(); // 停止执行 + 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()); @@ -434,7 +513,10 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionSadness(grpc::ServerContext* con return failResponse(response, "Biohead device not found: " + dev_id); } - robot->expressionSadness(); // 停止执行 + 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()); @@ -459,7 +541,10 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionYawn(grpc::ServerContext* contex return failResponse(response, "Biohead device not found: " + dev_id); } - robot->expressionYawn(); // 停止执行 + 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()); diff --git a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp index 5edc18b0..b04997f6 100644 --- a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp @@ -13,6 +13,7 @@ #include "common/base/logging/logger.h" #include "manager/task_manager/include/task_manager.h" +#include "service/stop_all/include/stop_all_admission_gate.h" #include "task/touch_screen_task/include/touch_screen_task.h" @@ -51,7 +52,26 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr: return grpc::Status(grpc::StatusCode::NOT_FOUND, error); } - if (!touch_task->touch(request->u(), request->v())) { + auto& admission_gate = globalStopAllAdmissionGate(); + std::uint64_t admission_generation = 0U; + { + auto admission = admission_gate.lockAdmission(); + if (!admission.accepting()) { + const std::string error = + "TouchScreenTask is temporarily paused by StopAll"; + fillTouchResponse(response, false, error); + return grpc::Status(grpc::StatusCode::UNAVAILABLE, error); + } + admission_generation = admission.generation(); + } + + if (!touch_task->touchIfCurrent( + request->u(), request->v(), + [&admission_gate, admission_generation] { + auto admission = admission_gate.lockAdmission(); + return admission.accepting() && + admission.generation() == admission_generation; + })) { const std::string error = buildTouchFailureMessage(*touch_task, "TouchScreenTask touch request rejected"); fillTouchResponse(response, false, error); diff --git a/cmvr-es/service/grpc/src/grpc_microphone_service.cpp b/cmvr-es/service/grpc/src/grpc_microphone_service.cpp index 63f07cb2..c44a0f57 100644 --- a/cmvr-es/service/grpc/src/grpc_microphone_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_microphone_service.cpp @@ -1,5 +1,6 @@ #include "common/base/logging/logger.h" #include "manager/media_source_hub/include/device_media_source_adapter.h" +#include "service/grpc/include/media_activity_coordinator.h" #include #include #include @@ -24,6 +25,13 @@ grpc::Status failResponse(ResponseT* response, const std::string& message) { return grpc::Status::OK; } +grpc::Status mediaStoppedStatus() +{ + return grpc::Status( + grpc::StatusCode::CANCELLED, + "Media activity stopped by StopAll"); +} + } gRPCMicroPhoneServiceImpl::gRPCMicroPhoneServiceImpl(): dmgr_(DeviceManager::getInstance()) {} @@ -63,6 +71,11 @@ 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 { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StartRecord): id=" << dev_id; @@ -70,10 +83,20 @@ grpc::Status gRPCMicroPhoneServiceImpl::StartRecord(grpc::ServerContext* context if (!dev) { return failResponse(response, "Microphone device not found: " + dev_id); } - if (!dev->start()) { + bool started = false; + const bool start_allowed = media_session.runIfCurrent([&] { + started = dev->start(); + if (started) { + dev->startRecording(request->file_path()); + } + }); + if (!start_allowed) { + return failResponse( + response, "Microphone recording start was canceled by StopAll"); + } + if (!started) { return failResponse(response, "Failed to start microphone: " + dev_id); } - dev->startRecording(request->file_path()); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (StartRecord): success, id=" << dev_id @@ -136,6 +159,11 @@ grpc::Status gRPCMicroPhoneServiceImpl::PauseRecord(grpc::ServerContext* context 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 { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (ResumeRecord): id=" << dev_id; @@ -143,7 +171,10 @@ grpc::Status gRPCMicroPhoneServiceImpl::ResumeRecord(grpc::ServerContext* contex if (!dev) { return failResponse(response, "Microphone device not found: " + dev_id); } - dev->resume(); + if (!media_session.runIfCurrent([&] { dev->resume(); })) { + return failResponse( + response, "Microphone recording resume was canceled by StopAll"); + } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (ResumeRecord): success, id=" << dev_id; @@ -160,6 +191,17 @@ grpc::Status gRPCMicroPhoneServiceImpl::ResumeRecord(grpc::ServerContext* contex grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context, const api::StreamMicAudioCommand_Request* request, grpc::ServerWriter* writer) { + auto media_session = globalMediaActivityCoordinator().beginSession( + [context] { context->TryCancel(); }); + if (!media_session) { + api::StreamMicAudioCommand_Feedback feedback; + feedback.mutable_header()->set_success(false); + feedback.mutable_header()->set_error_message( + "Media activities are temporarily paused by StopAll"); + setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp()); + writer->Write(feedback); + return grpc::Status::OK; + } try { const string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StreamAudio): id=" << dev_id; @@ -175,10 +217,18 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context auto& media_hub = cmvr::media::globalMediaSourceHub(); const std::string track_id = cmvr::media::microphoneTrackId(dev_id); - if (!cmvr::media::ensureMicrophoneMediaSource(media_hub, dev)) { + bool source_ready = false; + const bool source_setup_allowed = media_session.runIfCurrent([&] { + source_ready = + cmvr::media::ensureMicrophoneMediaSource(media_hub, dev); + }); + if (!source_setup_allowed || !source_ready) { api::StreamMicAudioCommand_Feedback feedback; feedback.mutable_header()->set_success(false); - feedback.mutable_header()->set_error_message("Failed to register microphone media source: " + dev_id); + feedback.mutable_header()->set_error_message( + media_session.cancelled() + ? "Microphone stream start was canceled by StopAll" + : "Failed to register microphone media source: " + dev_id); setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp()); writer->Write(feedback); return grpc::Status::OK; @@ -187,7 +237,9 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context auto subscription = media_hub.subscribe( track_id, cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED, - [context] { return context->IsCancelled(); }); + [context, &media_session] { + return context->IsCancelled() || media_session.cancelled(); + }); if (!subscription) { api::StreamMicAudioCommand_Feedback feedback; feedback.mutable_header()->set_success(false); @@ -197,8 +249,11 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context return grpc::Status::OK; } - while (!context->IsCancelled()) { + while (!media_session.cancelled() && !context->IsCancelled()) { const auto read = subscription.waitRead(std::chrono::milliseconds(100)); + if (media_session.cancelled()) { + break; + } if (!read || !read->value || read->value->empty()) { if (!subscription.valid()) { break; @@ -236,7 +291,9 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context feedback.mutable_header()->set_error_message( "Unsupported microphone stream codec: " + dev_id); feedback.clear_audio(); - writer->Write(feedback); + if (!media_session.cancelled()) { + writer->Write(feedback); + } break; } audio->set_pts(frame.pts); @@ -247,12 +304,17 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context sample_count, 0, std::numeric_limits::max()))); - if (!writer->Write(feedback)) { + if (media_session.cancelled() || !writer->Write(feedback)) { break; } } - return grpc::Status::OK; + return media_session.cancelled() + ? mediaStoppedStatus() + : grpc::Status::OK; } catch (const std::exception& error) { + if (media_session.cancelled()) { + return mediaStoppedStatus(); + } api::StreamMicAudioCommand_Feedback feedback; feedback.mutable_header()->set_success(false); feedback.mutable_header()->set_error_message(error.what()); diff --git a/cmvr-es/service/grpc/src/grpc_motor_service.cpp b/cmvr-es/service/grpc/src/grpc_motor_service.cpp index a08e67de..f4a88b0c 100644 --- a/cmvr-es/service/grpc/src/grpc_motor_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_motor_service.cpp @@ -15,6 +15,7 @@ #include "common/base/logging/logger.h" #include "devices/motor/manager/include/motor_manager.h" #include "manager/device_manager/include/device_manager.h" +#include "service/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { @@ -386,7 +387,7 @@ grpc::Status runCyclicLoop( } if (is_preempted(generation)) { const std::string error = - "cyclic stream preempted by emergency stop"; + "cyclic stream preempted by a stop request"; set_last_error(error); const bool stopped = safeStop(); joinReader(true); @@ -529,17 +530,21 @@ gRPCMotorServiceImpl::ControlLease::~ControlLease() if (!state_) { return; } - std::lock_guard lock(state_->mutex); - if (std::uncaught_exceptions() > uncaught_on_entry_) { - // Keep ownership reserved until the public RPC exception barrier has - // completed its best-effort stop. This closes the window where a new - // RPC could acquire the motor between stack unwinding and cleanup. - state_->exception_cleanup_pending = true; - ++state_->cancel_generation; - return; + { + std::lock_guard lock(state_->mutex); + if (std::uncaught_exceptions() > uncaught_on_entry_) { + // Keep ownership reserved until the public RPC exception barrier + // has completed its best-effort stop. This closes the window where + // a new RPC could acquire the motor between stack unwinding and + // cleanup. + state_->exception_cleanup_pending = true; + ++state_->cancel_generation; + } else { + state_->busy = false; + state_->active_control = api::MOTOR_CONTROL_NONE; + } } - state_->busy = false; - state_->active_control = api::MOTOR_CONTROL_NONE; + globalMotorActivityCoordinator().notifyStateChanged(); } gRPCMotorServiceImpl::gRPCMotorServiceImpl() @@ -570,15 +575,74 @@ gRPCMotorServiceImpl::stateFor( } auto state = std::make_shared(); + const std::weak_ptr weak_state = state; + const std::weak_ptr weak_motor = motor; + auto registration = globalMotorActivityCoordinator().registerControl( + [weak_state]() { + if (const auto state = weak_state.lock()) { + std::lock_guard state_lock(state->mutex); + ++state->cancel_generation; + state->last_error = "motor command preempted by System StopAll"; + } + }, + [weak_state, weak_motor]() { + const auto state = weak_state.lock(); + const auto motor = weak_motor.lock(); + if (!state || !motor) { + return true; + } + + bool stopped = false; + try { + // This confirmed stop is ordered after any device write that + // passed its generation check before StopAll invalidated it. + std::lock_guard command_lock(state->command_mutex); + stopped = motor->quickStop(); + } catch (...) { + stopped = false; + } + if (!stopped) { + std::lock_guard state_lock(state->mutex); + state->last_error = + "System StopAll could not confirm motor quick-stop"; + } + return stopped; + }, + [weak_state]() { + const auto state = weak_state.lock(); + if (!state) { + return true; + } + std::lock_guard state_lock(state->mutex); + return !state->busy && !state->exception_cleanup_pending; + }, + "motor " + std::to_string(motor->id()) + " (" + + motor->jointName() + ")"); states_.emplace( - motor.get(), MotorControlEntry{std::weak_ptr(motor), - state}); + motor.get(), + MotorControlEntry{ + std::weak_ptr(motor), state, + std::move(registration)}); return state; } +std::shared_ptr +gRPCMotorServiceImpl::existingStateFor( + const std::shared_ptr& motor) const +{ + std::lock_guard lock(states_mutex_); + const auto existing = states_.find(motor.get()); + if (existing == states_.end()) { + return nullptr; + } + const auto owner = existing->second.owner.lock(); + return owner && owner == motor ? existing->second.state : nullptr; +} + grpc::Status gRPCMotorServiceImpl::resolveMotor( const api::MotorTarget& target, - ResolvedMotor& resolved) const + ResolvedMotor& resolved, + const ResolveAccess access) const { const std::string& manager_id = target.header().device_id(); if (manager_id.empty()) { @@ -617,7 +681,19 @@ grpc::Status gRPCMotorServiceImpl::resolveMotor( return grpc::Status(grpc::StatusCode::NOT_FOUND, "motor not found in MotorManager: " + manager_id); } - resolved.control = stateFor(resolved.motor); + const auto arm_owner = manager->armOwnerForJoint( + resolved.motor->jointName()); + if (access == ResolveAccess::Control && !arm_owner.empty()) { + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, + "motor joint is controlled by RobotArm '" + arm_owner + + "'; use ArmService for control: " + + resolved.motor->jointName()); + } + resolved.control = + access == ResolveAccess::Observe && !arm_owner.empty() + ? existingStateFor(resolved.motor) + : stateFor(resolved.motor); return grpc::Status::OK; } @@ -628,6 +704,15 @@ gRPCMotorServiceImpl::acquireControl( grpc::Status& failure, const bool allow_emergency_stopped) const { + auto system_admission = globalStopAllAdmissionGate().lockAdmission(); + auto motor_admission = globalMotorActivityCoordinator().lockAdmission(); + if (!system_admission.accepting() || !motor_admission.accepting()) { + failure = grpc::Status( + grpc::StatusCode::ABORTED, + "motor control admission is paused by System StopAll"); + return nullptr; + } + std::lock_guard lock(resolved.control->mutex); if (resolved.control->busy) { failure = grpc::Status(grpc::StatusCode::RESOURCE_EXHAUSTED, @@ -717,6 +802,7 @@ void gRPCMotorServiceImpl::bestEffortQuickStop( resolved.control->busy = false; resolved.control->active_control = api::MOTOR_CONTROL_NONE; } + globalMotorActivityCoordinator().notifyStateChanged(); } catch (...) { } } @@ -729,7 +815,7 @@ void gRPCMotorServiceImpl::fillMotorStatus( bool emergency_stopped = false; api::MotorControlType active_control = api::MOTOR_CONTROL_NONE; std::string last_error; - { + if (resolved.control) { std::lock_guard lock(resolved.control->mutex); busy = resolved.control->busy; emergency_stopped = resolved.control->emergency_stopped; @@ -921,7 +1007,7 @@ grpc::Status gRPCMotorServiceImpl::setZeroImpl( } if (preempted) { const std::string error = - "zero calibration preempted by emergency stop"; + "zero calibration preempted by a stop request"; lease.reset(); fillFeedback(response->mutable_header(), false, error); fillMotorStatus(resolved, response->mutable_status()); @@ -1038,7 +1124,7 @@ grpc::Status gRPCMotorServiceImpl::runProfilePosition( } if (preempted) { const std::string error = - "profile position preempted by emergency stop"; + "profile position preempted by a stop request"; lease.reset(); fillFeedback(response->mutable_header(), false, error); fillMotorStatus(resolved, response->mutable_status()); @@ -1085,7 +1171,7 @@ grpc::Status gRPCMotorServiceImpl::runProfilePosition( } if (preempted_during_dispatch) { const std::string error = - "profile position rejected while being preempted by emergency stop"; + "profile position rejected while being preempted by a stop request"; setLastError(resolved.control, error); lease.reset(); fillFeedback(response->mutable_header(), false, error); @@ -1157,7 +1243,8 @@ grpc::Status gRPCMotorServiceImpl::waitForPosition( preempted = resolved.control->cancel_generation != generation; } if (preempted) { - const std::string error = "profile position preempted by emergency stop"; + const std::string error = + "profile position preempted by a stop request"; setLastError(resolved.control, error); fillFeedback(response->mutable_header(), false, error); response->set_elapsed_ms(elapsedMs(started)); @@ -1320,7 +1407,8 @@ grpc::Status gRPCMotorServiceImpl::waitForVelocity( preempted = resolved.control->cancel_generation != generation; } if (preempted) { - const std::string error = "profile velocity preempted by emergency stop"; + const std::string error = + "profile velocity preempted by a stop request"; setLastError(resolved.control, error); fillFeedback(response->mutable_header(), false, error); response->set_elapsed_ms(elapsedMs(started)); @@ -1483,7 +1571,7 @@ grpc::Status gRPCMotorServiceImpl::profileVelocityImpl( } if (preempted) { const std::string error = - "profile velocity preempted by emergency stop"; + "profile velocity preempted by a stop request"; lease.reset(); fillFeedback(response->mutable_header(), false, error); fillMotorStatus(resolved, response->mutable_status()); @@ -1531,7 +1619,7 @@ grpc::Status gRPCMotorServiceImpl::profileVelocityImpl( } if (preempted_during_dispatch) { const std::string error = - "profile velocity rejected while being preempted by emergency stop"; + "profile velocity rejected while being preempted by a stop request"; setLastError(resolved.control, error); lease.reset(); fillFeedback(response->mutable_header(), false, error); @@ -1620,7 +1708,7 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicPositionImpl( if (preempted) { return grpc::Status( grpc::StatusCode::ABORTED, - "cyclic position open preempted by emergency stop"); + "cyclic position open preempted by a stop request"); } resolved.motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); std::lock_guard state_lock(resolved.control->mutex); @@ -1645,7 +1733,7 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicPositionImpl( if (resolved.control->cancel_generation != generation) { return grpc::Status( grpc::StatusCode::ABORTED, - "cyclic position setpoint preempted by emergency stop"); + "cyclic position setpoint preempted by a stop request"); } } if (!isFinite(setpoint.target_position_rad()) || @@ -1743,7 +1831,7 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicVelocityImpl( if (preempted) { return grpc::Status( grpc::StatusCode::ABORTED, - "cyclic velocity open preempted by emergency stop"); + "cyclic velocity open preempted by a stop request"); } resolved.motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); std::lock_guard state_lock(resolved.control->mutex); @@ -1768,7 +1856,7 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicVelocityImpl( if (resolved.control->cancel_generation != generation) { return grpc::Status( grpc::StatusCode::ABORTED, - "cyclic velocity setpoint preempted by emergency stop"); + "cyclic velocity setpoint preempted by a stop request"); } } if (!isFinite(setpoint.target_velocity_rad_s())) { @@ -1884,7 +1972,8 @@ grpc::Status gRPCMotorServiceImpl::getStatusImpl( api::GetMotorStatusResponse* response) { ResolvedMotor resolved; - auto status = resolveMotor(request->target(), resolved); + auto status = resolveMotor( + request->target(), resolved, ResolveAccess::Observe); if (!status.ok()) { fillFeedback(response->mutable_header(), false, status.error_message()); return status; @@ -1949,7 +2038,7 @@ grpc::Status gRPCMotorServiceImpl::setEnabledImpl( } if (preempted) { const std::string error = - "enable/disable preempted by emergency stop"; + "enable/disable preempted by a stop request"; lease.reset(); fillFeedback(response->mutable_header(), false, error); fillMotorStatus(resolved, response->mutable_status()); @@ -2012,7 +2101,7 @@ grpc::Status gRPCMotorServiceImpl::setEnabledImpl( } if (preempted_after_dispatch) { const std::string error = - "enable/disable preempted by emergency stop during dispatch"; + "enable/disable preempted by a stop request during dispatch"; setLastError(resolved.control, error); lease.reset(); fillFeedback(response->mutable_header(), false, error); diff --git a/cmvr-es/service/grpc/src/grpc_speaker_service.cpp b/cmvr-es/service/grpc/src/grpc_speaker_service.cpp index 756fab83..4eaf03dd 100644 --- a/cmvr-es/service/grpc/src/grpc_speaker_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_speaker_service.cpp @@ -1,4 +1,5 @@ #include "common/base/logging/logger.h" +#include "service/grpc/include/media_activity_coordinator.h" #include // // Created by xtkuang on 2025/6/10. @@ -46,6 +47,52 @@ AudioStreamFrameData fromProtoAudioData(const cmvr::api::AudioData& audio) { frame.nb_samples = audio.nb_samples(); return frame; } + +std::string speakerResourceKey(const std::string& device_id) +{ + return "speaker:" + device_id; +} + +grpc::Status mediaStoppedStatus() +{ + return grpc::Status( + grpc::StatusCode::CANCELLED, + "Media activity stopped by StopAll"); +} + +class SpeakerStreamingLease final { +public: + explicit SpeakerStreamingLease(std::shared_ptr speaker) + : speaker_(std::move(speaker)) {} + + ~SpeakerStreamingLease() + { + if (!armed_ || !speaker_) { + return; + } + try { + speaker_->stopStreaming(); + } catch (const std::exception& error) { + CMVR_LOG(ERROR) + << "[gRPCSpeakerServiceImpl] failed to release speaker " + "stream session: " + << error.what(); + } catch (...) { + CMVR_LOG(ERROR) + << "[gRPCSpeakerServiceImpl] failed to release speaker " + "stream session"; + } + } + + SpeakerStreamingLease(const SpeakerStreamingLease&) = delete; + SpeakerStreamingLease& operator=(const SpeakerStreamingLease&) = delete; + + void arm() noexcept { armed_ = true; } + +private: + std::shared_ptr speaker_; + bool armed_{false}; +}; } gRPCSpeakerServiceImpl::gRPCSpeakerServiceImpl(): dmgr_(DeviceManager::getInstance()) {} @@ -86,6 +133,11 @@ 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 { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (PlayAudio): id=" << dev_id; @@ -93,8 +145,16 @@ grpc::Status gRPCSpeakerServiceImpl::PlayAudio(grpc::ServerContext* context, if (!dev) { return failResponse(response, "Speaker device not found: " + dev_id); } - //dev->start(); - dev->play(request->audio_path()); + if (!media_session.claimExclusiveResource(speakerResourceKey(dev_id))) { + return failResponse( + response, "Speaker is already controlled by another media session: " + dev_id); + } + if (!media_session.runIfCurrent([&] { + dev->play(request->audio_path()); + })) { + return failResponse( + response, "Speaker playback start was canceled by StopAll"); + } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (PlayAudio): success, id=" << dev_id @@ -112,12 +172,22 @@ grpc::Status gRPCSpeakerServiceImpl::PlayAudio(grpc::ServerContext* context, grpc::Status gRPCSpeakerServiceImpl::StreamAudio(grpc::ServerContext* context, grpc::ServerReader* reader, api::StreamSpeakerAudioCommand_Feedback* response) { + auto media_session = globalMediaActivityCoordinator().beginSession( + [context] { context->TryCancel(); }); + if (!media_session) { + return failResponse( + response, "Media activities are temporarily paused by StopAll"); + } try { api::StreamSpeakerAudioCommand_Request request; std::shared_ptr dev; + std::unique_ptr stream_lease; std::string dev_id; while (reader->Read(&request)) { + if (media_session.cancelled()) { + return mediaStoppedStatus(); + } if (!dev) { dev_id = request.header().device_id(); CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (StreamAudio): id=" << dev_id; @@ -125,25 +195,42 @@ grpc::Status gRPCSpeakerServiceImpl::StreamAudio(grpc::ServerContext* context, if (!dev) { return failResponse(response, "Speaker device not found: " + dev_id); } - if (!dev->start()) { - return failResponse(response, "Failed to start speaker: " + dev_id); + if (!media_session.claimExclusiveResource( + speakerResourceKey(dev_id))) { + return failResponse( + response, + "Speaker is already controlled by another media session: " + dev_id); } + stream_lease = std::make_unique(dev); } - if (!dev->pushAudioFrame(fromProtoAudioData(request.audio()))) { - dev->stopStreaming(); + const auto frame = fromProtoAudioData(request.audio()); + bool pushed = false; + const bool push_allowed = media_session.runIfCurrent([&] { + if (!frame.data.empty()) { + stream_lease->arm(); + } + pushed = dev->pushAudioFrame(frame); + }); + if (!push_allowed || media_session.cancelled()) { + return mediaStoppedStatus(); + } + if (!pushed) { return failResponse(response, "Failed to push speaker audio frame: " + dev_id); } } - if (dev) { - dev->stopStreaming(); + if (media_session.cancelled()) { + return mediaStoppedStatus(); } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); return grpc::Status::OK; } catch (const std::exception& e) { + if (media_session.cancelled()) { + return mediaStoppedStatus(); + } response->mutable_header()->set_success(false); response->mutable_header()->set_error_message(e.what()); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -160,7 +247,7 @@ grpc::Status gRPCSpeakerServiceImpl::StopPlayback(grpc::ServerContext* context, if (!dev) { return failResponse(response, "Speaker device not found: " + dev_id); } - if (!dev->stop()) { + if (!dev->stopPlayback()) { return failResponse(response, "Failed to stop speaker: " + dev_id); } response->mutable_header()->set_success(true); @@ -201,6 +288,11 @@ grpc::Status gRPCSpeakerServiceImpl::PausePlayback(grpc::ServerContext* context, 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 { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (ResumePlayback): id=" << dev_id; @@ -208,7 +300,14 @@ grpc::Status gRPCSpeakerServiceImpl::ResumePlayback(grpc::ServerContext* context if (!dev) { return failResponse(response, "Speaker device not found: " + dev_id); } - dev->resume(); + if (!media_session.claimExclusiveResource(speakerResourceKey(dev_id))) { + return failResponse( + response, "Speaker is already controlled by another media session: " + dev_id); + } + if (!media_session.runIfCurrent([&] { dev->resume(); })) { + return failResponse( + response, "Speaker playback resume was canceled by StopAll"); + } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (ResumePlayback): success, 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 f67a72c9..2d7fb369 100644 --- a/cmvr-es/service/grpc/src/grpc_system_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_system_service.cpp @@ -4,18 +4,38 @@ #include "../include/grpc_system_service.h" +#include #include #include #include +#include +#include #include #include +#include +#include +#include #include #include "common/base/logging/logger.h" #include "devices/agv/abstract_agv.h" #include "devices/arm/robot_arm.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/speaker/abstract_speaker.h" #include "manager/control_authority/include/control_authority_manager.h" +#include "manager/media_source_hub/include/device_media_source_adapter.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" using namespace cmvr::device; using namespace cmvr::service; @@ -31,11 +51,22 @@ public: for (const auto& barrier : barriers_) { if (barrier.release_on_destroy) { authority.release(barrier.token); + } else { + (void)authority.retireSafetyHolder(barrier.token); } } } bool acquire(const std::string& device_id, std::string& detail) + { + cmvr::control::ControlLeaseToken ignored; + return acquire(device_id, detail, ignored); + } + + bool acquire( + const std::string& device_id, + std::string& detail, + cmvr::control::ControlLeaseToken& token) { static std::atomic sequence{0}; const std::string owner = @@ -55,17 +86,44 @@ public: return false; } // StopAll barriers default to fail-closed. If retaining the token in - // this local vector throws, deliberately leave the manager-side safety - // holder installed: releasing it would reopen control after StopAll - // already preempted an in-flight command. - barriers_.push_back({result.token, false}); + // this local vector throws, retire the manager-side holder so a later + // successful StopAll can recover it without reopening control now. + try { + barriers_.push_back({result.token, false}); + } catch (...) { + (void)cmvr::control::ControlAuthorityManager::instance() + .retireSafetyHolder(result.token); + throw; + } + token = result.token; return true; } + bool waitForPreemptedRelease( + const std::string& device_id, + const std::chrono::milliseconds timeout) + { + const auto* barrier = find_(device_id); + return barrier && + cmvr::control::ControlAuthorityManager::instance() + .waitForPreemptedRelease(barrier->token, timeout); + } + + bool recoverRetiredSafetyHolders(const std::string& device_id) + { + const auto* barrier = find_(device_id); + return barrier && + cmvr::control::ControlAuthorityManager::instance() + .recoverRetiredSafetyHolders(barrier->token); + } + void quarantine(const std::string& device_id) { + auto& authority = + cmvr::control::ControlAuthorityManager::instance(); for (auto& barrier : barriers_) { if (barrier.token.resource_id == device_id) { + (void)authority.retireSafetyHolder(barrier.token); barrier.release_on_destroy = false; } } @@ -73,7 +131,10 @@ public: void quarantineAll() { + auto& authority = + cmvr::control::ControlAuthorityManager::instance(); for (auto& barrier : barriers_) { + (void)authority.retireSafetyHolder(barrier.token); barrier.release_on_destroy = false; } } @@ -85,15 +146,234 @@ public: } } + bool empty() const noexcept { return barriers_.empty(); } + + void recoverRetiredSafetyHoldersAll() + { + for (const auto& barrier : barriers_) { + if (!recoverRetiredSafetyHolders( + barrier.token.resource_id)) { + throw std::runtime_error( + "StopAll could not recover an earlier failed safety " + "barrier: " + barrier.token.resource_id); + } + } + } + + void releaseAll() noexcept + { + auto& authority = + cmvr::control::ControlAuthorityManager::instance(); + for (const auto& barrier : barriers_) { + authority.release(barrier.token); + } + barriers_.clear(); + } + private: struct Barrier { cmvr::control::ControlLeaseToken token; bool release_on_destroy{false}; }; + const Barrier* find_(const std::string& device_id) const noexcept + { + for (const auto& barrier : barriers_) { + if (barrier.token.resource_id == device_id) { + return &barrier; + } + } + return nullptr; + } + std::vector barriers_; }; +class ScopedMotorStopAll final { +public: + ScopedMotorStopAll() + : coordinator_(globalMotorActivityCoordinator()), + ticket_(coordinator_.beginStopAll(true)) + { + } + + ~ScopedMotorStopAll() + { + if (ticket_.valid() && !completed_) { + (void)coordinator_.finishStopAll(ticket_, false); + } + } + + bool valid() const noexcept { return ticket_.valid(); } + + bool stopAndWait( + const std::chrono::milliseconds timeout, + std::string* error) + { + return ticket_.valid() && + coordinator_.stopAndWait(ticket_, timeout, error); + } + + bool requestStop(std::string* error) + { + return ticket_.valid() && coordinator_.requestStop(ticket_, error); + } + + bool collectStopOperations( + std::vector& operations, + std::string* error) + { + return ticket_.valid() && + coordinator_.collectStopOperations(ticket_, operations, error); + } + + bool waitForStopped( + const std::chrono::milliseconds timeout, + std::string* error) + { + return ticket_.valid() && + coordinator_.waitForStopped(ticket_, timeout, error); + } + + bool complete(const bool all_motors_stopped) + { + if (!ticket_.valid() || completed_) { + return false; + } + const bool completed = coordinator_.finishStopAll( + ticket_, all_motors_stopped); + completed_ = completed; + return completed; + } + +private: + MotorActivityCoordinator& coordinator_; + MotorActivityCoordinator::StopAllTicket ticket_; + bool completed_{false}; +}; + +class ScopedActionQueueStopAll final { +public: + explicit ScopedActionQueueStopAll( + cmvr::service::ActionQueueExecutor& executor) + : executor_(executor), ticket_(executor_.beginStopAll(true)) + { + } + + ~ScopedActionQueueStopAll() + { + if (ticket_.valid() && !completed_) { + (void)executor_.finishStopAll(ticket_, false); + } + } + + bool valid() const noexcept { return ticket_.valid(); } + + bool complete(const bool all_devices_stop_confirmed) + { + if (!ticket_.valid() || completed_) { + return false; + } + const bool completed = executor_.finishStopAll( + ticket_, all_devices_stop_confirmed); + completed_ = completed; + return completed; + } + +private: + cmvr::service::ActionQueueExecutor& executor_; + cmvr::service::ActionQueueExecutor::StopAllTicket ticket_; + bool completed_{false}; +}; + +class ScopedMediaStopAll final { +public: + ScopedMediaStopAll() + : coordinator_(cmvr::service::globalMediaActivityCoordinator()), + ticket_(coordinator_.beginStopAll(true)) + { + } + + ~ScopedMediaStopAll() + { + if (ticket_.valid() && !completed_) { + (void)coordinator_.finishStopAll(ticket_, false); + } + } + + bool valid() const noexcept { return ticket_.valid(); } + + bool waitForStopped(const std::chrono::milliseconds timeout) + { + return ticket_.valid() && + coordinator_.waitForStopped(ticket_, timeout); + } + + bool requestCancellation() + { + return ticket_.valid() && coordinator_.requestCancellation(ticket_); + } + + bool collectCancellationOperations( + std::vector& operations, + std::string* error) + { + return ticket_.valid() && + coordinator_.collectCancellationOperations( + ticket_, operations, error); + } + + bool complete(const bool all_media_stopped) + { + if (!ticket_.valid() || completed_) { + return false; + } + const bool completed = + coordinator_.finishStopAll(ticket_, all_media_stopped); + completed_ = completed; + return completed; + } + +private: + cmvr::service::MediaActivityCoordinator& coordinator_; + cmvr::service::MediaActivityCoordinator::StopAllTicket ticket_; + bool completed_{false}; +}; + +class ScopedAdmissionStopAll final { +public: + ScopedAdmissionStopAll() + : gate_(globalStopAllAdmissionGate()), + ticket_(gate_.beginStopAll()) + { + } + + ~ScopedAdmissionStopAll() + { + if (ticket_.valid() && !completed_) { + (void)gate_.finishStopAll(ticket_, false); + } + } + + bool valid() const noexcept { return ticket_.valid(); } + + bool complete(const bool all_domains_stop_confirmed) + { + if (!ticket_.valid() || completed_) { + return false; + } + const auto result = gate_.finishStopAllDetailed( + ticket_, all_domains_stop_confirmed); + completed_ = result.ticket_consumed; + return result.admission_reopened; + } + +private: + StopAllAdmissionGate& gate_; + StopAllAdmissionGate::StopAllTicket ticket_; + bool completed_{false}; +}; + std::uint64_t unixTimeMs() noexcept { const auto elapsed = std::chrono::duration_cast( @@ -103,6 +383,467 @@ std::uint64_t unixTimeMs() noexcept : 0U; } +std::chrono::milliseconds remainingStopBudget( + const std::chrono::steady_clock::time_point deadline) noexcept +{ + const auto now = std::chrono::steady_clock::now(); + if (now >= deadline) { + return std::chrono::milliseconds::zero(); + } + return std::chrono::duration_cast( + deadline - now); +} + +std::timed_mutex& processStopAllMutex() +{ + // SystemService is expected to be unique, but keeping the mutex at process + // scope also protects test, reload, and accidental multi-instance paths + // from issuing overlapping whole-device stop rounds. + static std::timed_mutex mutex; + return mutex; +} + +using StopDispatcher = cmvr::service::StopOperationDispatcher; +using StopHandle = StopDispatcher::Handle; +using StopOutcome = StopDispatcher::OperationResult; + +struct StopHandleEntry final { + std::string device_id; + StopHandle handle; + bool control_resource{false}; + bool require_success{true}; +}; + +std::string stopResourceKey(const std::string& device_id) +{ + return "device:" + device_id; +} + +void appendFailure( + std::vector& failures, + const std::string& subject, + const std::string& detail) +{ + failures.push_back( + subject + ": " + + (detail.empty() ? "operational stop was not confirmed" : detail)); +} + +bool waitForStopOperations( + const std::vector& operations, + const StopDispatcher::Deadline deadline, + ScopedControlBarrierSet& control_barriers, + std::vector& failures, + std::unordered_set* completed_devices = nullptr) +{ + bool all_succeeded = true; + for (const auto& operation : operations) { + const auto outcome = operation.handle.waitUntil(deadline); + if (outcome.completed && completed_devices) { + completed_devices->emplace(operation.device_id); + } + if (outcome.completed && outcome.result) { + continue; + } + if (outcome.completed && !operation.require_success) { + CMVR_LOG(WARNING) + << "[gRPCSystemServiceImpl] (StopAll): initial stop was not " + "confirmed and will be retried, id=" + << operation.device_id << ", detail=" << outcome.detail; + continue; + } + + all_succeeded = false; + if (operation.control_resource) { + control_barriers.quarantine(operation.device_id); + } + appendFailure( + failures, + operation.device_id, + outcome.detail.empty() + ? "stop operation did not complete before the deadline" + : outcome.detail); + } + return all_succeeded; +} + +StopOutcome stopArm( + const std::shared_ptr& arm, + const bool final_confirmation) +{ + const auto result = arm->stopMotion(); + if (!result.ok()) { + return { + false, + std::string(final_confirmation ? "final " : "initial ") + + "RobotArm stop was not confirmed" + + (result.message.empty() ? "" : ": " + result.message)}; + } + if (final_confirmation) { + try { + if (arm->busy()) { + return {false, "RobotArm remained busy after final stop"}; + } + } catch (const std::exception& error) { + return { + false, + std::string("RobotArm idle confirmation threw: ") + + error.what()}; + } catch (...) { + return { + false, + "RobotArm idle confirmation threw an unknown exception"}; + } + } + return {true, {}}; +} + +bool agvStopResultAccepted(const cmvr::device::AgvResult& result) noexcept +{ + return result.ok() || + result.code == cmvr::device::AgvErrorCode::UnsupportedCommand; +} + +StopOutcome stopAgv( + const std::shared_ptr& agv, + const bool final_confirmation) +{ + const auto cancel = agv->cancelNavigation(); + const auto velocity = agv->stopVelocityControl(); + const auto mapping = agv->stopMapping(); + const auto stopped = agv->confirmMotionStopped(); + + if (final_confirmation && + (!agvStopResultAccepted(cancel) || + !agvStopResultAccepted(velocity) || + !agvStopResultAccepted(mapping) || !stopped.ok())) { + return {false, "final AGV operational stop was not confirmed"}; + } + if (!final_confirmation && !stopped.ok()) { + return { + false, + "initial AGV stopped state was not confirmed" + + (stopped.message.empty() ? "" : ": " + stopped.message)}; + } + return {true, {}}; +} + +StopOutcome stopDexHand( + const std::shared_ptr& hand, + const bool final_confirmation) +{ + if (!hand->stopOperationalActivity()) { + return { + false, + std::string(final_confirmation ? "final " : "initial ") + + "DexHand operational stop was not confirmed"}; + } + return {true, {}}; +} + +template +StopOutcome invokeStopOperation( + Operation&& operation, + const std::string& description) noexcept +{ + try { + return std::forward(operation)(); + } catch (const std::exception& error) { + return { + false, + description + " threw: " + error.what()}; + } catch (...) { + return { + false, + description + " threw an unknown exception"}; + } +} + +template +StopOutcome stopControlWithFence( + const cmvr::control::ControlLeaseToken& barrier, + const StopDispatcher::Deadline deadline, + InitialStop&& initial_stop, + FinalStop&& final_stop, + const std::string& description) +{ + const auto initial = invokeStopOperation( + std::forward(initial_stop), + "initial " + description + " stop"); + if (!initial.success) { + CMVR_LOG(WARNING) + << "[gRPCSystemServiceImpl] (StopAll): " << initial.detail; + } + + if (!barrier.valid()) { + return { + false, + description + + " safety barrier was unavailable after the initial stop"}; + } + + const bool handler_drained = + cmvr::control::ControlAuthorityManager::instance() + .waitForPreemptedRelease( + barrier, remainingStopBudget(deadline)); + const auto final = invokeStopOperation( + std::forward(final_stop), + "final " + description + " stop"); + if (!handler_drained) { + return { + false, + "timed out waiting for the preempted " + description + + " control handler to exit"}; + } + return final; +} + +StopOutcome stopCameraActivities( + const std::string& device_id, + const std::shared_ptr& camera) +{ + std::vector failures; + bool stopped = cmvr::media::globalMediaSourceHub() + .stopSourcesForDevice(device_id, &failures); + if (!globalCameraPtzActivityRegistry().stopActivitiesForDevice( + device_id, &failures)) { + stopped = false; + } + + try { + cmvr::device::CameraState state{}; + if (camera) { + camera->getState(state); + } + if (camera && state.is_recording) { + camera->stopRecording(); + camera->getState(state); + if (state.is_recording) { + stopped = false; + failures.push_back( + device_id + ": camera recording did not stop"); + } + } + } catch (const std::exception& error) { + stopped = false; + failures.push_back( + device_id + ": camera recording stop threw: " + error.what()); + } catch (...) { + stopped = false; + failures.push_back( + device_id + + ": camera recording stop threw an unknown exception"); + } + + if (!globalCameraOperationalActivityRegistry().stopActivitiesForDevice( + device_id, camera, &failures)) { + stopped = false; + } + return { + stopped, + failures.empty() + ? std::string{} + : "camera activities were not fully stopped: " + + failures.front()}; +} + +StopOutcome stopMicrophone( + const std::string& device_id, + const std::shared_ptr& microphone) +{ + std::vector failures; + bool stopped = cmvr::media::globalMediaSourceHub() + .stopSourcesForDevice(device_id, &failures); + try { + cmvr::device::MicrophoneState state{}; + microphone->getState(state); + if (state.is_recording) { + microphone->stopRecording(); + microphone->getState(state); + if (state.is_recording) { + stopped = false; + failures.push_back( + device_id + ": microphone recording did not stop"); + } + } + } catch (const std::exception& error) { + stopped = false; + failures.push_back( + device_id + ": microphone recording stop threw: " + + error.what()); + } catch (...) { + stopped = false; + failures.push_back( + device_id + + ": microphone recording stop threw an unknown exception"); + } + return { + stopped, + failures.empty() + ? std::string{} + : "microphone activities were not fully stopped: " + + failures.front()}; +} + +StopOutcome stopSpeaker( + const std::shared_ptr& speaker) +{ + return speaker->stopPlayback() + ? StopOutcome{true, {}} + : StopOutcome{false, "speaker playback did not stop"}; +} + +StopOutcome stopTrackedMediaActivities(const std::string& device_id) +{ + std::vector failures; + bool stopped = cmvr::media::globalMediaSourceHub() + .stopSourcesForDevice(device_id, &failures); + if (!globalCameraPtzActivityRegistry().stopActivitiesForDevice( + device_id, &failures)) { + stopped = false; + } + if (!globalCameraOperationalActivityRegistry().stopActivitiesForDevice( + device_id, &failures)) { + stopped = false; + } + return { + stopped, + failures.empty() + ? std::string{} + : "tracked media activities were not fully stopped: " + + failures.front()}; +} + +void mergeStopOutcome( + const StopOutcome& outcome, + bool& all_stopped, + std::vector& failures) +{ + if (outcome.success) { + return; + } + all_stopped = false; + failures.push_back( + outcome.detail.empty() + ? "operational stop was not confirmed" + : outcome.detail); +} + +template +StopOutcome stopOperationalDevice( + const std::shared_ptr& device, + const char* description); + +StopOutcome stopOtherActivities( + const std::string& device_id, + const std::shared_ptr& camera, + const std::shared_ptr& microphone, + const std::shared_ptr& speaker, + const std::shared_ptr& head, + const std::shared_ptr& gripper, + const bool tracked_media) +{ + bool all_stopped = true; + bool media_stopped_by_typed_device = false; + std::vector failures; + + if (camera) { + mergeStopOutcome( + invokeStopOperation( + [&] { return stopCameraActivities(device_id, camera); }, + "camera activity stop"), + all_stopped, failures); + media_stopped_by_typed_device = true; + } + if (microphone) { + mergeStopOutcome( + invokeStopOperation( + [&] { return stopMicrophone(device_id, microphone); }, + "microphone activity stop"), + all_stopped, failures); + media_stopped_by_typed_device = true; + } + if (tracked_media && !media_stopped_by_typed_device) { + mergeStopOutcome( + invokeStopOperation( + [&] { return stopTrackedMediaActivities(device_id); }, + "tracked media activity stop"), + all_stopped, failures); + } + if (speaker) { + mergeStopOutcome( + invokeStopOperation( + [&] { return stopSpeaker(speaker); }, + "speaker activity stop"), + all_stopped, failures); + } + if (head) { + mergeStopOutcome( + invokeStopOperation( + [&] { return stopOperationalDevice(head, "BioHead"); }, + "BioHead activity stop"), + all_stopped, failures); + } + if (gripper) { + mergeStopOutcome( + invokeStopOperation( + [&] { return stopOperationalDevice(gripper, "gripper"); }, + "gripper activity stop"), + all_stopped, failures); + } + + return { + all_stopped, + failures.empty() ? std::string{} : failures.front()}; +} + +StopOutcome stopTaskActivity( + const std::shared_ptr& task) +{ + if (!task) { + return {false, "task activity target was null"}; + } + return task->stopActivity() + ? StopOutcome{true, {}} + : StopOutcome{ + false, + "task " + task->id() + + " operational stop was not confirmed"}; +} + +void submitDeferredOperations( + StopDispatcher& dispatcher, + std::vector operations, + std::vector& handles) +{ + handles.reserve(handles.size() + operations.size()); + for (auto& operation : operations) { + const auto subject = operation.resource_key; + auto callback = std::move(operation.operation); + auto handle = dispatcher.submit( + operation.resource_key, + [callback = std::move(callback)]() mutable { + const auto result = callback(); + return StopOutcome{result.success, result.detail}; + }); + handles.push_back( + {subject, std::move(handle), false, true}); + } +} + +template +StopOutcome stopOperationalDevice( + const std::shared_ptr& device, + const char* description) +{ + return device->stopOperationalActivity() + ? StopOutcome{true, {}} + : StopOutcome{ + false, + std::string(description) + + " operational stop was not confirmed"}; +} + cmvr::api::SystemDeviceType toApiDeviceType( const cmvr::device::DeviceKind kind) noexcept { @@ -187,6 +928,7 @@ cmvr::api::SystemDeviceHealth toApiDeviceHealth( gRPCSystemServiceImpl::gRPCSystemServiceImpl() : dmgr_(DeviceManager::getInstance()), + stop_dispatcher_(std::make_unique()), action_queue_(std::make_unique(dmgr_)) { } @@ -195,7 +937,12 @@ gRPCSystemServiceImpl::~gRPCSystemServiceImpl() = default; void gRPCSystemServiceImpl::prepareForShutdown() { - (void)action_queue_->cancelAllAndDisable(); + // Do not wait for a business StopAll round here. StopAll may be blocked in + // a device backend, while server shutdown must still invalidate queued and + // active ActionQueue work promptly. ActionQueueExecutor owns the state lock + // and generation fence needed to make this transition race-safe; an + // in-flight StopAll ticket then becomes stale and fails closed. + (void)action_queue_->disableForShutdown(); } grpc::Status gRPCSystemServiceImpl::GetSystemInfo(grpc::ServerContext* context, @@ -331,132 +1078,442 @@ 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) { - (void)context; (void)request; + constexpr auto stop_timeout = std::chrono::seconds(15); + const auto stop_deadline = + std::chrono::steady_clock::now() + stop_timeout; + + std::unique_lock stop_all_lock( + processStopAllMutex(), std::defer_lock); + while (!stop_all_lock.try_lock_for(std::min( + std::chrono::milliseconds(50), + remainingStopBudget(stop_deadline)))) { + if (context && context->IsCancelled()) { + return grpc::Status( + grpc::StatusCode::CANCELLED, + "StopAll was cancelled while waiting for another StopAll round"); + } + if (remainingStopBudget(stop_deadline) == + std::chrono::milliseconds::zero()) { + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message( + "StopAll timed out waiting for another StopAll round"); + setCurrentTimestamp( + response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; + } + } + try { - const auto snapshot = dmgr_.snapshot(); - ScopedControlBarrierSet control_barriers; - for (const auto& device : snapshot.devices) { - if (device.kind == cmvr::device::DeviceKind::Arm || - device.kind == cmvr::device::DeviceKind::AGV) { - std::string detail; - if (!control_barriers.acquire(device.id, detail)) { - CMVR_LOG(WARNING) - << "[gRPCSystemServiceImpl] (StopAll): failed to " - "acquire device safety barrier, id=" - << device.id << ", detail=" << detail; - throw std::runtime_error( - "StopAll could not acquire the device safety " - "barrier: " + device.id); - } - } + // Close every admission domain before resolving devices or issuing + // stops. The process remains alive; only new operational work pauses. + ScopedAdmissionStopAll admission_stop; + ScopedMediaStopAll media_stop; + ScopedMotorStopAll motor_stop; + ScopedActionQueueStopAll action_stop(*action_queue_); + if (!admission_stop.valid()) { + throw std::runtime_error( + "StopAll could not establish the system admission barrier"); } - const bool action_stop_confirmed = - action_queue_->cancelAllAndDisable(); - if (!action_stop_confirmed) { - control_barriers.quarantineAll(); + if (!media_stop.valid()) { + throw std::runtime_error( + "StopAll could not establish the media activity barrier"); } - std::vector unconfirmed_devices; - for (const auto& device : snapshot.devices) { - if (device.kind != cmvr::device::DeviceKind::Arm) { - continue; - } - auto arm = dmgr_.getDevice(device.id); - if (!arm) { - continue; - } - try { - const auto stopped = arm->stopMotion(); - if (!stopped.ok()) { - control_barriers.quarantine(device.id); - unconfirmed_devices.push_back( - device.id + ": " + stopped.message); - } - } catch (const std::exception& error) { - control_barriers.quarantine(device.id); - unconfirmed_devices.push_back( - device.id + ": stop threw: " + error.what()); - } catch (...) { - control_barriers.quarantine(device.id); - unconfirmed_devices.push_back( - device.id + ": stop threw an unknown exception"); - } + if (!action_stop.valid()) { + throw std::runtime_error( + "StopAll cannot start because the service is shutting down"); + } + if (!motor_stop.valid()) { + throw std::runtime_error( + "StopAll could not establish the motor activity barrier"); } - // AbstractAGV::stop() is a lifecycle hook and some backends do not - // map it to a motion stop. Use the typed non-E-stop controls here; - // StopAll must not be silently upgraded to emergencyStop semantics. - for (const auto& device : snapshot.devices) { - if (device.kind != cmvr::device::DeviceKind::AGV) { + // This snapshot is intentionally metadata-only. A wedged health query + // must not prevent physical stop requests from being dispatched. + const auto inventory = dmgr_.inventorySnapshot(); + ScopedControlBarrierSet control_barriers; + std::vector unconfirmed_devices; + + struct ControlTarget final { + std::string id; + cmvr::device::DeviceKind kind{ + cmvr::device::DeviceKind::Unknown}; + std::shared_ptr arm; + std::shared_ptr agv; + std::shared_ptr hand; + cmvr::control::ControlLeaseToken barrier; + }; + std::vector control_targets; + control_targets.reserve(inventory.size()); + + struct OtherTarget final { + std::string id; + std::shared_ptr camera; + std::shared_ptr microphone; + std::shared_ptr speaker; + std::shared_ptr head; + std::shared_ptr gripper; + bool tracked_media{false}; + }; + std::unordered_map other_targets; + other_targets.reserve(inventory.size()); + + for (const auto& device : inventory) { + const bool is_control = + device.kind == cmvr::device::DeviceKind::Arm || + device.kind == cmvr::device::DeviceKind::AGV || + device.kind == cmvr::device::DeviceKind::DexHand; + if (!is_control) { + OtherTarget target; + target.id = device.id; + switch (device.kind) { + case cmvr::device::DeviceKind::Camera: + target.camera = std::dynamic_pointer_cast< + cmvr::device::AbstractCamera>(device.device); + break; + case cmvr::device::DeviceKind::Microphone: + target.microphone = std::dynamic_pointer_cast< + cmvr::device::AbstractMicrophone>(device.device); + break; + case cmvr::device::DeviceKind::Speaker: + target.speaker = std::dynamic_pointer_cast< + cmvr::device::AbstractSpeaker>(device.device); + break; + case cmvr::device::DeviceKind::BioHead: + target.head = std::dynamic_pointer_cast< + cmvr::device::AbstractBiohead>(device.device); + break; + case cmvr::device::DeviceKind::Gripper: + target.gripper = std::dynamic_pointer_cast< + cmvr::device::AbstractGripper>(device.device); + break; + default: + continue; + } + if (!target.camera && !target.microphone && + !target.speaker && !target.head && !target.gripper) { + appendFailure( + unconfirmed_devices, device.id, + "inventory type did not resolve to its typed device"); + continue; + } + other_targets.emplace(device.id, std::move(target)); continue; } - auto agv = dmgr_.getDevice( - device.id); - if (!agv) { + + ControlTarget target; + target.id = device.id; + target.kind = device.kind; + if (device.kind == cmvr::device::DeviceKind::Arm) { + target.arm = std::dynamic_pointer_cast< + cmvr::device::RobotArm>(device.device); + } else if (device.kind == cmvr::device::DeviceKind::AGV) { + target.agv = std::dynamic_pointer_cast< + cmvr::device::AbstractAGV>(device.device); + } else { + target.hand = std::dynamic_pointer_cast< + cmvr::device::AbstractDexHand>(device.device); + } + if (!target.arm && !target.agv && !target.hand) { + appendFailure( + unconfirmed_devices, device.id, + "inventory type did not resolve to the typed control device"); continue; } + + std::string detail; + bool acquired = false; try { - const auto cancel_result = agv->cancelNavigation(); - if (!cancel_result.ok() && - cancel_result.code != - cmvr::device::AgvErrorCode::UnsupportedCommand) { - CMVR_LOG(WARNING) - << "[gRPCSystemServiceImpl] (StopAll): AGV navigation " - "cancel failed, id=" - << device.id << ", detail=" << cancel_result.message; - } - const auto velocity_stop = agv->stopVelocityControl(); - if (!velocity_stop.ok() && - velocity_stop.code != - cmvr::device::AgvErrorCode::UnsupportedCommand) { - CMVR_LOG(WARNING) - << "[gRPCSystemServiceImpl] (StopAll): AGV velocity " - "stop failed, id=" - << device.id << ", detail=" - << velocity_stop.message; - } - const auto stopped = agv->confirmMotionStopped(); - if (!stopped.ok()) { - control_barriers.quarantine(device.id); - unconfirmed_devices.push_back( - device.id + ": " + stopped.message); - } + acquired = control_barriers.acquire( + device.id, detail, target.barrier); } catch (const std::exception& error) { - control_barriers.quarantine(device.id); - unconfirmed_devices.push_back( - device.id + ": stop confirmation threw: " + - error.what()); + detail = error.what(); } catch (...) { - control_barriers.quarantine(device.id); - unconfirmed_devices.push_back( - device.id + - ": stop confirmation threw an unknown exception"); + detail = "unknown exception"; } + if (!acquired) { + appendFailure( + unconfirmed_devices, device.id, + "could not establish the device safety barrier" + + (detail.empty() ? "" : ": " + detail)); + continue; + } + control_targets.push_back(std::move(target)); } - if (!action_queue_->waitForIdle(std::chrono::seconds(15))) { - control_barriers.quarantineAll(); - throw std::runtime_error( - "StopAll timed out waiting for ActionQueue to become idle"); + + std::unordered_set tracked_media_ids; + const auto merge_tracked_ids = [&tracked_media_ids]( + const std::vector& ids) { + tracked_media_ids.insert(ids.begin(), ids.end()); + }; + merge_tracked_ids( + cmvr::media::globalMediaSourceHub().trackedSourceIds()); + merge_tracked_ids( + globalCameraPtzActivityRegistry().trackedDeviceIds()); + merge_tracked_ids( + globalCameraOperationalActivityRegistry().trackedDeviceIds()); + for (const auto& id : tracked_media_ids) { + auto [target, inserted] = other_targets.try_emplace(id); + if (inserted) { + target->second.id = id; + } + target->second.tracked_media = true; } - if (!action_stop_confirmed) { - throw std::runtime_error( - "StopAll could not confirm that every active ActionQueue " - "device stopped; affected control resources remain " - "quarantined"); + + // Every independent stop is submitted before waiting for any result. + // A blocked backend therefore cannot delay peer motion or media stops. + std::vector control_stops; + control_stops.reserve(control_targets.size()); + for (const auto& target : control_targets) { + StopHandle handle; + if (target.arm) { + const auto arm = target.arm; + const auto barrier = target.barrier; + handle = stop_dispatcher_->submit( + "control:" + stopResourceKey(target.id), + [arm, barrier, stop_deadline] { + return stopControlWithFence( + barrier, stop_deadline, + [arm] { return stopArm(arm, false); }, + [arm] { return stopArm(arm, true); }, + "RobotArm"); + }); + } else if (target.agv) { + const auto agv = target.agv; + const auto barrier = target.barrier; + handle = stop_dispatcher_->submit( + "control:" + stopResourceKey(target.id), + [agv, barrier, stop_deadline] { + return stopControlWithFence( + barrier, stop_deadline, + [agv] { return stopAgv(agv, false); }, + [agv] { return stopAgv(agv, true); }, + "AGV"); + }); + } else { + const auto hand = target.hand; + const auto barrier = target.barrier; + handle = stop_dispatcher_->submit( + "control:" + stopResourceKey(target.id), + [hand, barrier, stop_deadline] { + return stopControlWithFence( + barrier, stop_deadline, + [hand] { return stopDexHand(hand, false); }, + [hand] { return stopDexHand(hand, true); }, + "DexHand"); + }); + } + control_stops.push_back( + {target.id, std::move(handle), true, true}); + } + + std::vector motor_operations; + std::vector + deferred_motor_operations; + std::string motor_collection_error; + if (!motor_stop.collectStopOperations( + deferred_motor_operations, &motor_collection_error)) { + appendFailure( + unconfirmed_devices, "motors", + motor_collection_error.empty() + ? "could not collect registered motor stop operations" + : motor_collection_error); + } else { + submitDeferredOperations( + *stop_dispatcher_, std::move(deferred_motor_operations), + motor_operations); + } + + std::vector media_cancellations; + std::vector + deferred_media_cancellations; + std::string media_collection_error; + if (!media_stop.collectCancellationOperations( + deferred_media_cancellations, &media_collection_error)) { + appendFailure( + unconfirmed_devices, "media", + media_collection_error.empty() + ? "could not collect active media cancellations" + : media_collection_error); + } else { + submitDeferredOperations( + *stop_dispatcher_, std::move(deferred_media_cancellations), + media_cancellations); + } + + std::vector task_stops; + const auto task_targets = + cmvr::task::TaskManager::activitySnapshotIfInitialized(); + task_stops.reserve(task_targets.size()); + for (const auto& task : task_targets) { + const auto task_id = task ? task->id() : std::string{"unknown"}; + auto handle = stop_dispatcher_->submit( + "task:" + task_id, + [task] { + return invokeStopOperation( + [task] { return stopTaskActivity(task); }, + "task activity stop"); + }); + task_stops.push_back( + {task_id, std::move(handle), false, true}); + } + + std::vector other_stops; + other_stops.reserve(other_targets.size()); + for (const auto& [id, target] : other_targets) { + const auto camera = target.camera; + const auto microphone = target.microphone; + const auto speaker = target.speaker; + const auto head = target.head; + const auto gripper = target.gripper; + const bool tracked_media = target.tracked_media; + auto handle = stop_dispatcher_->submit( + "activity:" + stopResourceKey(id), + [id, camera, microphone, speaker, head, gripper, + tracked_media] { + return stopOtherActivities( + id, camera, microphone, speaker, head, gripper, + tracked_media); + }); + other_stops.push_back( + {id, std::move(handle), false, true}); + } + + std::unordered_set completed_control_stops; + (void)waitForStopOperations( + control_stops, stop_deadline, control_barriers, + unconfirmed_devices, &completed_control_stops); + (void)waitForStopOperations( + motor_operations, stop_deadline, control_barriers, + unconfirmed_devices); + (void)waitForStopOperations( + media_cancellations, stop_deadline, control_barriers, + unconfirmed_devices); + (void)waitForStopOperations( + task_stops, stop_deadline, control_barriers, + unconfirmed_devices); + std::unordered_set completed_other_stops; + (void)waitForStopOperations( + other_stops, stop_deadline, control_barriers, + unconfirmed_devices, &completed_other_stops); + + std::string motor_error; + const bool motors_stopped = motor_stop.waitForStopped( + remainingStopBudget(stop_deadline), &motor_error); + if (!motors_stopped) { + unconfirmed_devices.push_back( + "motors: " + (motor_error.empty() + ? "operational stop was not confirmed" + : motor_error)); + } + + const bool media_stopped = media_stop.waitForStopped( + remainingStopBudget(stop_deadline)); + if (!media_stopped) { + unconfirmed_devices.push_back( + "media: timed out waiting for active RPCs to stop"); + } else { + // The first device stop runs in parallel with media cancellation so + // motion stops are never delayed by a blocked media handler. A + // handler already inside runIfCurrent(), however, can finish a + // start/resume after that first stop. Once every old media session + // has drained, repeat the typed operational stops while all + // admission gates are still closed. Do not retry a device whose + // first stop is still running; StopOperationDispatcher would only + // join that old job and a concurrent driver stop would be unsafe. + std::vector final_media_stops; + final_media_stops.reserve(completed_other_stops.size()); + for (const auto& [id, target] : other_targets) { + const bool needs_final_media_stop = + target.camera || target.microphone || target.speaker || + target.tracked_media; + if (!needs_final_media_stop || + completed_other_stops.count(id) == 0U) { + continue; + } + + const auto camera = target.camera; + const auto microphone = target.microphone; + const auto speaker = target.speaker; + const bool tracked_media = target.tracked_media; + auto handle = stop_dispatcher_->submit( + "activity:" + stopResourceKey(id), + [id, camera, microphone, speaker, tracked_media] { + return stopOtherActivities( + id, camera, microphone, speaker, nullptr, nullptr, + tracked_media); + }); + final_media_stops.push_back( + {id, std::move(handle), false, true}); + } + + // DexHand sensor RPCs also belong to the media coordinator and can + // race their resumeOperationalActivity() with the initial control + // stop. Their typed safety barrier is still held here, so a final + // operational stop cannot admit a new hand command. + for (const auto& target : control_targets) { + if (!target.hand || + completed_control_stops.count(target.id) == 0U) { + continue; + } + const auto hand = target.hand; + auto handle = stop_dispatcher_->submit( + "final-media-control:" + stopResourceKey(target.id), + [hand] { + return invokeStopOperation( + [hand] { return stopDexHand(hand, true); }, + "final DexHand media activity stop"); + }); + final_media_stops.push_back( + {target.id, std::move(handle), true, true}); + } + (void)waitForStopOperations( + final_media_stops, stop_deadline, control_barriers, + unconfirmed_devices); + } + + if (!action_queue_->waitForIdle( + remainingStopBudget(stop_deadline))) { + unconfirmed_devices.push_back( + "ActionQueue: timed out waiting for the execution queue to " + "become idle"); } if (!unconfirmed_devices.empty()) { + control_barriers.quarantineAll(); throw std::runtime_error( "StopAll could not confirm that every device stopped; " - "affected control resources remain quarantined: " + + "control remains paused and affected resources remain " + "quarantined: " + unconfirmed_devices.front()); } - // Do not close device transports while the Action worker may still be - // unwinding a synchronous driver call. - dmgr_.stop(); + control_barriers.recoverRetiredSafetyHoldersAll(); + if (!action_stop.complete(true)) { + throw std::runtime_error( + "StopAll stopped all devices but could not safely resume " + "ActionQueue admission"); + } + if (!media_stop.complete(true)) { + throw std::runtime_error( + "StopAll stopped all media but could not safely resume " + "media activity admission"); + } + if (!motor_stop.complete(true)) { + throw std::runtime_error( + "StopAll stopped all motors but could not safely resume " + "motor command admission"); + } + // Release the current round's typed control barriers while the global + // gate is still closed. Retired barriers from earlier failed rounds + // were recovered above; unrelated active safety holders are preserved. + control_barriers.releaseAll(); + if (!admission_stop.complete(true)) { + throw std::runtime_error( + "StopAll stopped all activities but could not safely resume " + "system admission"); + } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (StopAll): success"; - control_barriers.confirmSafeToReleaseAll(); return grpc::Status::OK; } catch (std::exception& e) { @@ -465,6 +1522,13 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); return grpc::Status::OK; } + catch (...) { + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message( + "StopAll failed with an unknown exception"); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; + } } grpc::Status gRPCSystemServiceImpl::ExecuteActionQueue( diff --git a/cmvr-es/service/grpc/src/media_activity_coordinator.cpp b/cmvr-es/service/grpc/src/media_activity_coordinator.cpp new file mode 100644 index 00000000..1650cad1 --- /dev/null +++ b/cmvr-es/service/grpc/src/media_activity_coordinator.cpp @@ -0,0 +1,437 @@ +#include "service/grpc/include/media_activity_coordinator.h" + +#include "common/base/logging/logger.h" +#include "service/stop_all/include/stop_all_admission_gate.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cmvr::service { + +struct MediaActivityCoordinator::SessionState final { + SessionState( + const std::uint64_t session_id_value, + const std::uint64_t generation_value, + CancelCallback callback) + : session_id(session_id_value), + generation(generation_value), + cancel_callback(std::move(callback)) {} + + void markCancelled() noexcept + { + is_cancelled.store(true, std::memory_order_release); + } + + DeferredStopResult requestCancel() + { + markCancelled(); + + CancelCallback callback; + { + std::unique_lock lock(callback_mutex); + if (callback_in_flight) { + callback_condition.wait( + lock, [this] { return !callback_in_flight; }); + } + if (callback_invoked) { + return cancel_result; + } + if (released || !cancel_callback) { + callback_invoked = true; + cancel_result = {true, {}}; + return cancel_result; + } + callback_invoked = true; + callback_in_flight = true; + callback = cancel_callback; + } + + try { + DeferredStopResult result{true, {}}; + try { + callback(); + } catch (const std::exception& error) { + result = { + false, + std::string("media cancellation callback threw: ") + + error.what()}; + CMVR_LOG(ERROR) + << "[MediaActivityCoordinator] " << result.detail; + } catch (...) { + result = { + false, + "media cancellation callback threw an unknown exception"}; + CMVR_LOG(ERROR) + << "[MediaActivityCoordinator] " << result.detail; + } + + { + std::lock_guard lock(callback_mutex); + cancel_result = result; + callback_in_flight = false; + } + callback_condition.notify_all(); + return result; + } catch (...) { + { + std::lock_guard lock(callback_mutex); + callback_in_flight = false; + } + callback_condition.notify_all(); + throw; + } + } + + void prepareRelease() noexcept + { + std::unique_lock lock(callback_mutex); + released = true; + cancel_callback = {}; + callback_condition.wait(lock, [this] { return !callback_in_flight; }); + } + + const std::uint64_t session_id; + const std::uint64_t generation; + std::atomic is_cancelled{false}; + std::mutex callback_mutex; + std::condition_variable callback_condition; + CancelCallback cancel_callback; + bool callback_invoked{false}; + bool callback_in_flight{false}; + bool released{false}; + DeferredStopResult cancel_result; + std::size_t dispatches_in_flight{0U}; + std::unordered_set exclusive_resources; +}; + +struct MediaActivityCoordinator::Impl final { + bool hasSessionsBefore(const std::uint64_t generation) const + { + for (const auto& [session_id, session] : sessions) { + (void)session_id; + if (session->generation < generation) { + return true; + } + } + return false; + } + + mutable std::mutex mutex; + std::condition_variable condition; + std::unordered_map> sessions; + std::unordered_map exclusive_resources; + std::unordered_set stop_all_tickets; + std::uint64_t generation{1U}; + std::uint64_t next_session_id{0U}; + std::uint64_t next_ticket_id{0U}; + bool accepting{true}; + bool stop_all_failed{false}; +}; + +MediaActivityCoordinator::Session::Session( + std::shared_ptr impl, + std::shared_ptr state) + : impl_(std::move(impl)), state_(std::move(state)) {} + +MediaActivityCoordinator::Session::~Session() +{ + reset(); +} + +MediaActivityCoordinator::Session::Session(Session&& other) noexcept + : impl_(std::move(other.impl_)), state_(std::move(other.state_)) {} + +MediaActivityCoordinator::Session& +MediaActivityCoordinator::Session::operator=(Session&& other) noexcept +{ + if (this != &other) { + reset(); + impl_ = std::move(other.impl_); + state_ = std::move(other.state_); + } + return *this; +} + +MediaActivityCoordinator::Session::operator bool() const noexcept +{ + return impl_ && state_; +} + +bool MediaActivityCoordinator::Session::cancelled() const noexcept +{ + return !state_ || state_->is_cancelled.load(std::memory_order_acquire); +} + +bool MediaActivityCoordinator::Session::runIfCurrent( + const std::function& operation) const +{ + if (!impl_ || !state_ || !operation) { + return false; + } + + { + std::lock_guard lock(impl_->mutex); + const auto active = impl_->sessions.find(state_->session_id); + if (!impl_->accepting || + state_->generation != impl_->generation || + state_->is_cancelled.load(std::memory_order_acquire) || + active == impl_->sessions.end() || + active->second != state_) { + return false; + } + ++state_->dispatches_in_flight; + } + + const auto finish_dispatch = [impl = impl_, state = state_]() noexcept { + try { + { + std::lock_guard lock(impl->mutex); + if (state->dispatches_in_flight > 0U) { + --state->dispatches_in_flight; + } + } + impl->condition.notify_all(); + } catch (...) { + } + }; + + try { + operation(); + } catch (...) { + finish_dispatch(); + throw; + } + finish_dispatch(); + return true; +} + +bool MediaActivityCoordinator::Session::claimExclusiveResource( + const std::string& resource_key) +{ + if (!impl_ || !state_ || resource_key.empty()) { + return false; + } + + std::lock_guard lock(impl_->mutex); + const auto active = impl_->sessions.find(state_->session_id); + if (!impl_->accepting || + state_->generation != impl_->generation || + state_->is_cancelled.load(std::memory_order_acquire) || + active == impl_->sessions.end() || + active->second != state_) { + return false; + } + + const auto owner = impl_->exclusive_resources.find(resource_key); + if (owner != impl_->exclusive_resources.end()) { + return owner->second == state_->session_id; + } + impl_->exclusive_resources.emplace(resource_key, state_->session_id); + state_->exclusive_resources.emplace(resource_key); + return true; +} + +void MediaActivityCoordinator::Session::reset() noexcept +{ + auto impl = std::move(impl_); + auto state = std::move(state_); + if (!impl || !state) { + return; + } + + state->prepareRelease(); + { + std::unique_lock lock(impl->mutex); + state->markCancelled(); + impl->condition.wait(lock, [&state] { + return state->dispatches_in_flight == 0U; + }); + const auto active = impl->sessions.find(state->session_id); + if (active != impl->sessions.end() && active->second == state) { + impl->sessions.erase(active); + } + for (const auto& resource_key : state->exclusive_resources) { + const auto owner = impl->exclusive_resources.find(resource_key); + if (owner != impl->exclusive_resources.end() && + owner->second == state->session_id) { + impl->exclusive_resources.erase(owner); + } + } + } + impl->condition.notify_all(); +} + +MediaActivityCoordinator::MediaActivityCoordinator() + : impl_(std::make_shared()) {} + +MediaActivityCoordinator::Session MediaActivityCoordinator::beginSession( + CancelCallback cancel) +{ + auto system_admission = + globalStopAllAdmissionGate().lockAdmission(); + std::lock_guard lock(impl_->mutex); + if (!system_admission.accepting() || !impl_->accepting) { + return {}; + } + + const auto session_id = ++impl_->next_session_id; + auto state = std::make_shared( + session_id, impl_->generation, std::move(cancel)); + impl_->sessions.emplace(session_id, state); + return Session(impl_, std::move(state)); +} + +MediaActivityCoordinator::StopAllTicket +MediaActivityCoordinator::beginStopAll(const bool defer_cancellation) +{ + StopAllTicket ticket; + std::vector> sessions; + { + std::lock_guard lock(impl_->mutex); + if (impl_->accepting || impl_->stop_all_tickets.empty()) { + impl_->accepting = false; + ++impl_->generation; + impl_->stop_all_failed = false; + } + + ticket.generation = impl_->generation; + ticket.ticket_id = ++impl_->next_ticket_id; + impl_->stop_all_tickets.emplace(ticket.ticket_id); + + sessions.reserve(impl_->sessions.size()); + for (const auto& [session_id, session] : impl_->sessions) { + (void)session_id; + if (session->generation < ticket.generation) { + session->markCancelled(); + sessions.push_back(session); + } + } + } + + if (!defer_cancellation) { + for (const auto& session : sessions) { + (void)session->requestCancel(); + } + } + return ticket; +} + +bool MediaActivityCoordinator::collectCancellationOperations( + const StopAllTicket& ticket, + std::vector& operations, + std::string* error) const +{ + operations.clear(); + if (error) { + error->clear(); + } + if (!ticket.valid()) { + if (error) { + *error = "invalid media StopAll ticket"; + } + return false; + } + + { + std::lock_guard lock(impl_->mutex); + if (impl_->accepting || ticket.generation != impl_->generation || + impl_->stop_all_tickets.count(ticket.ticket_id) == 0U) { + if (error) { + *error = "media StopAll ticket is no longer current"; + } + return false; + } + operations.reserve(impl_->sessions.size()); + for (const auto& [session_id, session] : impl_->sessions) { + if (session->generation < ticket.generation) { + operations.push_back({ + "media-session:" + std::to_string(session_id), + [session] { return session->requestCancel(); }}); + } + } + } + std::sort( + operations.begin(), operations.end(), + [](const auto& lhs, const auto& rhs) { + return lhs.resource_key < rhs.resource_key; + }); + return true; +} + +bool MediaActivityCoordinator::requestCancellation( + const StopAllTicket& ticket) +{ + std::vector operations; + if (!collectCancellationOperations(ticket, operations)) { + return false; + } + + bool all_cancelled = true; + for (const auto& operation : operations) { + try { + if (!operation.operation().success) { + all_cancelled = false; + } + } catch (...) { + all_cancelled = false; + } + } + return all_cancelled; +} + +bool MediaActivityCoordinator::waitForStopped( + const StopAllTicket& ticket, + const std::chrono::milliseconds timeout) +{ + if (!ticket.valid() || timeout < std::chrono::milliseconds::zero()) { + return false; + } + + std::unique_lock lock(impl_->mutex); + if (ticket.generation != impl_->generation || + impl_->stop_all_tickets.count(ticket.ticket_id) == 0U) { + return false; + } + return impl_->condition.wait_for(lock, timeout, [this, &ticket] { + return ticket.generation == impl_->generation && + !impl_->hasSessionsBefore(ticket.generation); + }); +} + +bool MediaActivityCoordinator::finishStopAll( + 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; + } + + const bool caller_succeeded = + all_media_stopped && !impl_->hasSessionsBefore(ticket.generation); + if (!caller_succeeded) { + impl_->stop_all_failed = true; + } + if (impl_->stop_all_tickets.empty() && !impl_->stop_all_failed && + !impl_->hasSessionsBefore(ticket.generation)) { + impl_->accepting = true; + } + return caller_succeeded; +} + +MediaActivityCoordinator& globalMediaActivityCoordinator() +{ + static MediaActivityCoordinator coordinator; + return coordinator; +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/src/motor_activity_coordinator.cpp b/cmvr-es/service/grpc/src/motor_activity_coordinator.cpp new file mode 100644 index 00000000..9f46a48e --- /dev/null +++ b/cmvr-es/service/grpc/src/motor_activity_coordinator.cpp @@ -0,0 +1,488 @@ +#include "service/grpc/include/motor_activity_coordinator.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "common/base/logging/logger.h" + +namespace cmvr::service { + +namespace { + +struct ControlCallbacks final { + MotorActivityCoordinator::CancelCallback cancel; + MotorActivityCoordinator::QuickStopCallback quick_stop; + MotorActivityCoordinator::IdleCallback idle; + std::string description; +}; + +struct MotorStopOperationState final { + explicit MotorStopOperationState(ControlCallbacks callbacks_value) + : callbacks(std::move(callbacks_value)) + { + } + + DeferredStopResult cancel() + { + { + std::unique_lock lock(mutex); + condition.wait(lock, [this] { return !cancel_running; }); + if (cancel_completed) { + return cancel_result; + } + cancel_running = true; + } + + try { + DeferredStopResult result{true, {}}; + try { + callbacks.cancel(); + } catch (const std::exception& error) { + result = { + false, + std::string("motor cancellation callback threw: ") + + error.what()}; + } catch (...) { + result = { + false, + "motor cancellation callback threw an unknown exception"}; + } + + { + std::lock_guard lock(mutex); + cancel_result = result; + cancel_completed = true; + cancel_running = false; + } + condition.notify_all(); + return result; + } catch (...) { + { + std::lock_guard lock(mutex); + cancel_running = false; + } + condition.notify_all(); + throw; + } + } + + DeferredStopResult run() + { + { + std::unique_lock lock(mutex); + condition.wait(lock, [this] { return !stop_running; }); + if (stop_completed) { + return stop_result; + } + stop_running = true; + } + + try { + DeferredStopResult result = cancel(); + bool quick_stopped = false; + std::string quick_stop_error; + try { + quick_stopped = callbacks.quick_stop(); + if (!quick_stopped) { + quick_stop_error = callbacks.description.empty() + ? "a registered motor did not confirm quick-stop" + : callbacks.description + + ": quick-stop was not confirmed"; + } + } catch (const std::exception& error) { + quick_stop_error = + std::string("motor quick-stop callback threw: ") + + error.what(); + } catch (...) { + quick_stop_error = + "motor quick-stop callback threw an unknown exception"; + } + if (!quick_stopped) { + if (result.success) { + result = {false, std::move(quick_stop_error)}; + } else if (!quick_stop_error.empty()) { + result.detail += "; " + quick_stop_error; + } + } + + { + std::lock_guard lock(mutex); + stop_result = result; + stop_completed = true; + stop_running = false; + } + condition.notify_all(); + return result; + } catch (...) { + { + std::lock_guard lock(mutex); + stop_running = false; + } + condition.notify_all(); + throw; + } + } + + ControlCallbacks callbacks; + std::mutex mutex; + std::condition_variable condition; + bool cancel_running{false}; + bool cancel_completed{false}; + bool stop_running{false}; + bool stop_completed{false}; + DeferredStopResult cancel_result; + DeferredStopResult stop_result; +}; + +} // namespace + +struct MotorActivityCoordinator::Impl final { + bool targetsIdle() const + { + for (const auto& [id, stop_state] : round_targets) { + (void)stop_state; + const auto entry = controls.find(id); + if (entry == controls.end()) { + continue; + } + try { + if (!entry->second.idle || !entry->second.idle()) { + return false; + } + } catch (...) { + return false; + } + } + return true; + } + + std::mutex mutex; + std::condition_variable condition; + std::unordered_map controls; + std::unordered_map< + std::uint64_t, std::shared_ptr> round_targets; + std::unordered_set stop_all_tickets; + std::uint64_t generation{1U}; + std::uint64_t next_control_id{0U}; + std::uint64_t next_ticket_id{0U}; + bool accepting{true}; + bool stop_all_failed{false}; +}; + +MotorActivityCoordinator::Registration::Registration( + std::shared_ptr impl, + const std::uint64_t id) noexcept + : impl_(std::move(impl)), id_(id) +{ +} + +MotorActivityCoordinator::Registration::~Registration() +{ + reset(); +} + +MotorActivityCoordinator::Registration::Registration( + Registration&& other) noexcept + : impl_(std::move(other.impl_)), id_(std::exchange(other.id_, 0U)) +{ +} + +MotorActivityCoordinator::Registration& +MotorActivityCoordinator::Registration::operator=(Registration&& other) noexcept +{ + if (this != &other) { + reset(); + impl_ = std::move(other.impl_); + id_ = std::exchange(other.id_, 0U); + } + return *this; +} + +MotorActivityCoordinator::Registration::operator bool() const noexcept +{ + return impl_ && id_ != 0U; +} + +void MotorActivityCoordinator::Registration::reset() noexcept +{ + auto impl = std::move(impl_); + const auto id = std::exchange(id_, 0U); + if (!impl || id == 0U) { + return; + } + try { + { + std::lock_guard lock(impl->mutex); + impl->controls.erase(id); + } + impl->condition.notify_all(); + } catch (...) { + } +} + +MotorActivityCoordinator::AdmissionGuard::AdmissionGuard( + std::unique_lock&& lock, + const bool accepting) noexcept + : lock_(std::move(lock)), accepting_(accepting) +{ +} + +MotorActivityCoordinator::MotorActivityCoordinator() + : impl_(std::make_shared()) +{ +} + +MotorActivityCoordinator::Registration +MotorActivityCoordinator::registerControl( + CancelCallback cancel, + QuickStopCallback quick_stop, + IdleCallback idle, + std::string description) +{ + if (!cancel || !quick_stop || !idle) { + return {}; + } + + std::lock_guard lock(impl_->mutex); + const auto id = ++impl_->next_control_id; + impl_->controls.emplace( + id, + ControlCallbacks{ + std::move(cancel), std::move(quick_stop), std::move(idle), + std::move(description)}); + return Registration(impl_, id); +} + +MotorActivityCoordinator::AdmissionGuard +MotorActivityCoordinator::lockAdmission() +{ + std::unique_lock lock(impl_->mutex); + return AdmissionGuard(std::move(lock), impl_->accepting); +} + +MotorActivityCoordinator::StopAllTicket +MotorActivityCoordinator::beginStopAll(const bool defer_callbacks) +{ + StopAllTicket ticket; + std::vector> targets; + { + std::lock_guard lock(impl_->mutex); + if (impl_->accepting || impl_->stop_all_tickets.empty()) { + impl_->accepting = false; + impl_->stop_all_failed = false; + ++impl_->generation; + impl_->round_targets.clear(); + targets.reserve(impl_->controls.size()); + for (const auto& [id, callbacks] : impl_->controls) { + auto state = + std::make_shared(callbacks); + impl_->round_targets.emplace(id, state); + targets.push_back(std::move(state)); + } + } + + ticket.generation = impl_->generation; + ticket.ticket_id = ++impl_->next_ticket_id; + impl_->stop_all_tickets.emplace(ticket.ticket_id); + if (targets.empty()) { + targets.reserve(impl_->round_targets.size()); + for (const auto& [id, state] : impl_->round_targets) { + (void)id; + targets.push_back(state); + } + } + } + + if (!defer_callbacks) { + for (const auto& target : targets) { + const auto result = target->cancel(); + if (!result.success) { + CMVR_LOG(ERROR) + << "[MotorActivityCoordinator] " << result.detail; + } + } + } + impl_->condition.notify_all(); + return ticket; +} + +bool MotorActivityCoordinator::collectStopOperations( + const StopAllTicket& ticket, + std::vector& operations, + std::string* error) const +{ + operations.clear(); + if (error) { + error->clear(); + } + if (!ticket.valid()) { + if (error) { + *error = "invalid motor StopAll ticket"; + } + return false; + } + + { + std::lock_guard lock(impl_->mutex); + if (impl_->accepting || ticket.generation != impl_->generation || + impl_->stop_all_tickets.count(ticket.ticket_id) == 0U) { + if (error) { + *error = "motor StopAll ticket is no longer current"; + } + return false; + } + operations.reserve(impl_->round_targets.size()); + for (const auto& [id, state] : impl_->round_targets) { + operations.push_back({ + "motor-control:" + std::to_string(id), + [state] { return state->run(); }}); + } + } + std::sort( + operations.begin(), operations.end(), + [](const auto& lhs, const auto& rhs) { + return lhs.resource_key < rhs.resource_key; + }); + return true; +} + +bool MotorActivityCoordinator::requestStop( + const StopAllTicket& ticket, + std::string* error) +{ + std::vector operations; + if (!collectStopOperations(ticket, operations, error)) { + return false; + } + + bool all_stopped = true; + for (const auto& operation : operations) { + DeferredStopResult result; + try { + result = operation.operation(); + } catch (const std::exception& exception) { + result = { + false, + std::string("motor stop operation threw: ") + + exception.what()}; + } catch (...) { + result = { + false, "motor stop operation threw an unknown exception"}; + } + if (!result.success) { + all_stopped = false; + if (error && error->empty()) { + *error = result.detail.empty() + ? "a registered motor did not confirm quick-stop" + : result.detail; + } + CMVR_LOG(ERROR) + << "[MotorActivityCoordinator] " + << (result.detail.empty() + ? "motor stop operation was not confirmed" + : result.detail); + } + } + return all_stopped; +} + +bool MotorActivityCoordinator::waitForStopped( + const StopAllTicket& ticket, + const std::chrono::milliseconds timeout, + std::string* error) +{ + if (!ticket.valid() || timeout < std::chrono::milliseconds::zero()) { + if (error && error->empty()) { + *error = "invalid motor StopAll ticket or timeout"; + } + return false; + } + + const auto deadline = std::chrono::steady_clock::now() + timeout; + std::unique_lock lock(impl_->mutex); + const bool idle = impl_->condition.wait_until(lock, deadline, [&] { + return ticket.generation != impl_->generation || + impl_->stop_all_tickets.count(ticket.ticket_id) == 0U || + impl_->targetsIdle(); + }); + const bool ticket_current = + !impl_->accepting && ticket.generation == impl_->generation && + impl_->stop_all_tickets.count(ticket.ticket_id) != 0U; + const bool targets_idle = ticket_current && impl_->targetsIdle(); + if ((!idle || !targets_idle) && error && error->empty()) { + *error = "timed out waiting for active motor RPCs to stop"; + } + return idle && targets_idle; +} + +bool MotorActivityCoordinator::stopAndWait( + const StopAllTicket& ticket, + const std::chrono::milliseconds timeout, + std::string* error) +{ + if (error) { + error->clear(); + } + const bool stop_requested = requestStop(ticket, error); + const bool stopped = waitForStopped(ticket, timeout, error); + return stop_requested && stopped; +} + +bool MotorActivityCoordinator::finishStopAll( + 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; + } + + const bool caller_succeeded = + all_motors_stopped && impl_->targetsIdle(); + if (!caller_succeeded) { + impl_->stop_all_failed = true; + } + if (impl_->stop_all_tickets.empty() && !impl_->stop_all_failed && + impl_->targetsIdle()) { + impl_->accepting = true; + impl_->round_targets.clear(); + } + return caller_succeeded; +} + +void MotorActivityCoordinator::notifyStateChanged() noexcept +{ + try { + impl_->condition.notify_all(); + } catch (...) { + } +} + +void MotorActivityCoordinator::clearForTesting() noexcept +{ + try { + std::lock_guard lock(impl_->mutex); + impl_->accepting = true; + impl_->stop_all_failed = false; + ++impl_->generation; + impl_->round_targets.clear(); + impl_->stop_all_tickets.clear(); + } catch (...) { + } + notifyStateChanged(); +} + +MotorActivityCoordinator& globalMotorActivityCoordinator() +{ + static MotorActivityCoordinator coordinator; + return coordinator; +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/camera_operational_activity_registry_test.cpp b/cmvr-es/service/grpc/tests/camera_operational_activity_registry_test.cpp new file mode 100644 index 00000000..79b52385 --- /dev/null +++ b/cmvr-es/service/grpc/tests/camera_operational_activity_registry_test.cpp @@ -0,0 +1,491 @@ +#include "service/grpc/include/camera_operational_activity_registry.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "service/stop_all/include/stop_all_admission_gate.h" + +namespace cmvr::service { +namespace { + +class TestCamera final : public device::AbstractCamera { +public: + explicit TestCamera(std::string id) + { + id_ = std::move(id); + state_.is_initialized = true; + } + + std::string typeName() const override { return "TestCamera"; } + + void getState(device::CameraState& state) override + { + std::lock_guard lock(mutex_); + state = state_; + } + + bool start() override + { + ++lifecycle_start_calls_; + return true; + } + + bool stop() override + { + ++lifecycle_stop_calls_; + std::lock_guard lock(mutex_); + operational_active_ = false; + state_.is_opened = false; + return !fail_lifecycle_stop_; + } + + bool startOperationalActivity() override + { + ++operational_start_calls_; + { + std::unique_lock lock(mutex_); + start_entered_ = true; + condition_.notify_all(); + condition_.wait(lock, [this] { return !block_start_; }); + if (fail_operational_start_) { + return false; + } + operational_active_ = true; + state_.is_opened = true; + } + return true; + } + + bool stopOperationalActivity() override + { + ++operational_stop_calls_; + std::unique_lock lock(mutex_); + stop_entered_ = true; + condition_.notify_all(); + condition_.wait(lock, [this] { return !block_stop_; }); + if (fail_operational_stop_) { + return false; + } + operational_active_ = false; + state_.is_opened = false; + return true; + } + + void setFailOperationalStart(const bool fail) + { + fail_operational_start_ = fail; + } + + void setFailOperationalStop(const bool fail) + { + fail_operational_stop_ = fail; + } + + void blockStop() + { + std::lock_guard lock(mutex_); + block_stop_ = true; + stop_entered_ = false; + } + + void waitForStopEntered() + { + std::unique_lock lock(mutex_); + condition_.wait(lock, [this] { return stop_entered_; }); + } + + void releaseStop() + { + { + std::lock_guard lock(mutex_); + block_stop_ = false; + } + condition_.notify_all(); + } + + void blockStart() + { + std::lock_guard lock(mutex_); + block_start_ = true; + start_entered_ = false; + } + + void waitForStartEntered() + { + std::unique_lock lock(mutex_); + condition_.wait(lock, [this] { return start_entered_; }); + } + + void releaseStart() + { + { + std::lock_guard lock(mutex_); + block_start_ = false; + } + condition_.notify_all(); + } + + bool operationalActive() const + { + std::lock_guard lock(mutex_); + return operational_active_; + } + + int lifecycleStartCalls() const { return lifecycle_start_calls_; } + int lifecycleStopCalls() const { return lifecycle_stop_calls_; } + int operationalStartCalls() const { return operational_start_calls_; } + int operationalStopCalls() const { return operational_stop_calls_; } + +private: + mutable std::mutex mutex_; + std::condition_variable condition_; + bool operational_active_{false}; + bool fail_operational_start_{false}; + bool fail_operational_stop_{false}; + bool fail_lifecycle_stop_{false}; + bool block_start_{false}; + bool start_entered_{false}; + bool block_stop_{false}; + bool stop_entered_{false}; + std::atomic lifecycle_start_calls_{0}; + std::atomic lifecycle_stop_calls_{0}; + std::atomic operational_start_calls_{0}; + std::atomic operational_stop_calls_{0}; +}; + +class CameraOperationalActivityRegistryTest : public ::testing::Test { +protected: + void SetUp() override + { + globalStopAllAdmissionGate().clearForTesting(); + } + + void TearDown() override + { + globalStopAllAdmissionGate().clearForTesting(); + } +}; + +TEST_F(CameraOperationalActivityRegistryTest, + StopAllStopsTrackedActivityWithoutLifecycleStopAndCanRestart) +{ + CameraOperationalActivityRegistry registry; + auto camera = std::make_shared("camera"); + + EXPECT_EQ( + registry.start(camera->id(), camera), + CameraOperationalActivityRegistry::DispatchResult::Success); + EXPECT_TRUE(camera->operationalActive()); + EXPECT_EQ(registry.activeCameraCount(), 1U); + + auto ticket = globalStopAllAdmissionGate().beginStopAll(); + ASSERT_TRUE(ticket.valid()); + EXPECT_TRUE(registry.stopAllActivities()); + EXPECT_FALSE(camera->operationalActive()); + EXPECT_EQ(camera->lifecycleStopCalls(), 0); + EXPECT_EQ(camera->operationalStopCalls(), 1); + EXPECT_EQ(registry.activeCameraCount(), 0U); + ASSERT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); + + EXPECT_EQ( + registry.start(camera->id(), camera), + CameraOperationalActivityRegistry::DispatchResult::Success); + EXPECT_TRUE(camera->operationalActive()); + EXPECT_EQ(camera->operationalStartCalls(), 2); + EXPECT_EQ(camera->lifecycleStartCalls(), 0); +} + +TEST_F(CameraOperationalActivityRegistryTest, + FailedOperationalStopRemainsTrackedAndKeepsAdmissionClosed) +{ + CameraOperationalActivityRegistry registry; + auto camera = std::make_shared("camera"); + ASSERT_EQ( + registry.start(camera->id(), camera), + CameraOperationalActivityRegistry::DispatchResult::Success); + + camera->setFailOperationalStop(true); + auto ticket = globalStopAllAdmissionGate().beginStopAll(); + std::vector failures; + EXPECT_FALSE(registry.stopAllActivities(&failures)); + EXPECT_EQ(registry.activeCameraCount(), 1U); + ASSERT_EQ(failures.size(), 1U); + EXPECT_EQ( + registry.start(camera->id(), camera), + CameraOperationalActivityRegistry::DispatchResult::RejectedByStopAll); + EXPECT_FALSE(globalStopAllAdmissionGate().finishStopAll(ticket, false)); + + camera->setFailOperationalStop(false); + ticket = globalStopAllAdmissionGate().beginStopAll(); + EXPECT_TRUE(registry.stopAllActivities()); + EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); + EXPECT_EQ(registry.activeCameraCount(), 0U); +} + +TEST_F(CameraOperationalActivityRegistryTest, + StopAllWaitsForRacingStartThenStopsTheStartedActivity) +{ + CameraOperationalActivityRegistry registry; + auto camera = std::make_shared("camera"); + camera->blockStart(); + + CameraOperationalActivityRegistry::DispatchResult start_result = + CameraOperationalActivityRegistry::DispatchResult::DeviceFailure; + std::thread start_thread([&] { + start_result = registry.start(camera->id(), camera); + }); + camera->waitForStartEntered(); + + const auto ticket = globalStopAllAdmissionGate().beginStopAll(); + std::atomic stop_returned{false}; + bool stop_result = false; + std::thread stop_thread([&] { + stop_result = registry.stopAllActivities(); + stop_returned = true; + }); + + std::this_thread::yield(); + EXPECT_FALSE(stop_returned.load()); + camera->releaseStart(); + start_thread.join(); + stop_thread.join(); + + EXPECT_EQ( + start_result, + CameraOperationalActivityRegistry::DispatchResult::Success); + EXPECT_TRUE(stop_result); + EXPECT_FALSE(camera->operationalActive()); + EXPECT_EQ(camera->operationalStopCalls(), 1); + EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); +} + +TEST_F(CameraOperationalActivityRegistryTest, + TrackedIdsExposeAStartThatIsStillInsideDriverIo) +{ + CameraOperationalActivityRegistry registry; + auto camera = std::make_shared("camera.pending"); + camera->blockStart(); + + CameraOperationalActivityRegistry::DispatchResult start_result = + CameraOperationalActivityRegistry::DispatchResult::DeviceFailure; + std::thread start_thread([&] { + start_result = registry.start(camera->id(), camera); + }); + camera->waitForStartEntered(); + + const auto snapshot_started = std::chrono::steady_clock::now(); + EXPECT_EQ( + registry.trackedDeviceIds(), + (std::vector{"camera.pending"})); + EXPECT_LT( + std::chrono::steady_clock::now() - snapshot_started, + std::chrono::milliseconds(100)); + + camera->releaseStart(); + start_thread.join(); + EXPECT_EQ( + start_result, + CameraOperationalActivityRegistry::DispatchResult::Success); +} + +TEST_F(CameraOperationalActivityRegistryTest, + DefaultOperationalStopFailsClosed) +{ + class UnsupportedCamera final : public device::AbstractCamera { + public: + UnsupportedCamera() { id_ = "unsupported"; } + std::string typeName() const override { return "UnsupportedCamera"; } + void getState(device::CameraState& state) override { state = {}; } + }; + + CameraOperationalActivityRegistry registry; + auto camera = std::make_shared(); + ASSERT_EQ( + registry.start(camera->id(), camera), + CameraOperationalActivityRegistry::DispatchResult::Success); + + const auto ticket = globalStopAllAdmissionGate().beginStopAll(); + EXPECT_FALSE(registry.stopAllActivities()); + EXPECT_EQ(registry.activeCameraCount(), 1U); + EXPECT_FALSE(globalStopAllAdmissionGate().finishStopAll(ticket, false)); +} + +TEST_F(CameraOperationalActivityRegistryTest, + StopForDeviceIsSelectiveAndActiveIdsReflectFailures) +{ + CameraOperationalActivityRegistry registry; + auto first = std::make_shared("camera.first"); + auto second = std::make_shared("camera.second"); + ASSERT_EQ( + registry.start(first->id(), first), + CameraOperationalActivityRegistry::DispatchResult::Success); + ASSERT_EQ( + registry.start(second->id(), second), + CameraOperationalActivityRegistry::DispatchResult::Success); + EXPECT_EQ( + registry.activeDeviceIds(), + (std::vector{"camera.first", "camera.second"})); + EXPECT_EQ( + registry.trackedDeviceIds(), + (std::vector{"camera.first", "camera.second"})); + + const auto ticket = globalStopAllAdmissionGate().beginStopAll(); + first->setFailOperationalStop(true); + std::vector failures; + EXPECT_FALSE(registry.stopActivitiesForDevice(first->id(), &failures)); + EXPECT_EQ(failures.size(), 1U); + EXPECT_TRUE(first->operationalActive()); + EXPECT_TRUE(second->operationalActive()); + EXPECT_EQ(second->operationalStopCalls(), 0); + + first->setFailOperationalStop(false); + EXPECT_TRUE(registry.stopActivitiesForDevice(first->id())); + EXPECT_EQ( + registry.activeDeviceIds(), + (std::vector{"camera.second"})); + EXPECT_TRUE(registry.stopActivitiesForDevice("missing")); + EXPECT_TRUE(registry.stopAllActivities()); + EXPECT_TRUE(registry.activeDeviceIds().empty()); + EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); + EXPECT_EQ(first->lifecycleStopCalls(), 0); + EXPECT_EQ(second->lifecycleStopCalls(), 0); +} + +TEST_F(CameraOperationalActivityRegistryTest, + InventoryFallbackStopsOncePerStopAllRoundAndCanStopAgainLater) +{ + CameraOperationalActivityRegistry registry; + auto camera = std::make_shared("inventory-camera"); + ASSERT_TRUE(camera->startOperationalActivity()); + + auto ticket = globalStopAllAdmissionGate().beginStopAll(); + ASSERT_TRUE(ticket.valid()); + EXPECT_TRUE(registry.stopActivitiesForDevice(camera->id(), camera)); + EXPECT_TRUE(registry.stopActivitiesForDevice(camera->id(), camera)); + EXPECT_FALSE(camera->operationalActive()); + EXPECT_EQ(camera->operationalStopCalls(), 1); + EXPECT_EQ(camera->lifecycleStopCalls(), 0); + ASSERT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); + + ASSERT_TRUE(camera->startOperationalActivity()); + ticket = globalStopAllAdmissionGate().beginStopAll(); + ASSERT_TRUE(ticket.valid()); + EXPECT_TRUE(registry.stopActivitiesForDevice(camera->id(), camera)); + EXPECT_FALSE(camera->operationalActive()); + EXPECT_EQ(camera->operationalStopCalls(), 2); + EXPECT_EQ(camera->lifecycleStopCalls(), 0); + EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); +} + +TEST_F(CameraOperationalActivityRegistryTest, + TrackedCameraTakesPrecedenceOverInventoryFallback) +{ + CameraOperationalActivityRegistry registry; + auto tracked = std::make_shared("camera"); + auto fallback = std::make_shared("camera"); + ASSERT_EQ( + registry.start(tracked->id(), tracked), + CameraOperationalActivityRegistry::DispatchResult::Success); + ASSERT_TRUE(fallback->startOperationalActivity()); + + const auto ticket = globalStopAllAdmissionGate().beginStopAll(); + ASSERT_TRUE(ticket.valid()); + EXPECT_TRUE(registry.stopActivitiesForDevice( + tracked->id(), fallback)); + EXPECT_FALSE(tracked->operationalActive()); + EXPECT_EQ(tracked->operationalStopCalls(), 1); + EXPECT_TRUE(fallback->operationalActive()); + EXPECT_EQ(fallback->operationalStopCalls(), 0); + EXPECT_EQ(tracked->lifecycleStopCalls(), 0); + EXPECT_EQ(fallback->lifecycleStopCalls(), 0); + EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); +} + +TEST_F(CameraOperationalActivityRegistryTest, + ActiveDeviceIdCannotBeReboundToAnotherCameraInstance) +{ + CameraOperationalActivityRegistry registry; + auto original = std::make_shared("camera"); + auto replacement = std::make_shared("camera"); + ASSERT_EQ( + registry.start(original->id(), original), + CameraOperationalActivityRegistry::DispatchResult::Success); + + CameraOperationalActivityRegistry::ActivityToken replacement_token; + EXPECT_EQ( + registry.start( + replacement->id(), replacement, &replacement_token), + CameraOperationalActivityRegistry::DispatchResult::DeviceFailure); + EXPECT_FALSE(replacement_token.valid()); + EXPECT_TRUE(original->operationalActive()); + EXPECT_FALSE(replacement->operationalActive()); + EXPECT_EQ(replacement->operationalStartCalls(), 0); + + const auto ticket = globalStopAllAdmissionGate().beginStopAll(); + ASSERT_TRUE(ticket.valid()); + EXPECT_TRUE(registry.stopAllActivities()); + EXPECT_FALSE(original->operationalActive()); + EXPECT_EQ(original->operationalStopCalls(), 1); + EXPECT_EQ(replacement->operationalStopCalls(), 0); + EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); +} + +TEST_F(CameraOperationalActivityRegistryTest, + DifferentDevicesStopConcurrentlyAndSameDeviceStopIsSerialized) +{ + CameraOperationalActivityRegistry registry; + auto first = std::make_shared("camera.first"); + auto second = std::make_shared("camera.second"); + ASSERT_EQ( + registry.start(first->id(), first), + CameraOperationalActivityRegistry::DispatchResult::Success); + ASSERT_EQ( + registry.start(second->id(), second), + CameraOperationalActivityRegistry::DispatchResult::Success); + first->blockStop(); + second->blockStop(); + + const auto ticket = globalStopAllAdmissionGate().beginStopAll(); + std::thread first_stop([&] { + EXPECT_TRUE(registry.stopActivitiesForDevice(first->id())); + }); + first->waitForStopEntered(); + const auto snapshot_started = std::chrono::steady_clock::now(); + EXPECT_EQ( + registry.trackedDeviceIds(), + (std::vector{"camera.first", "camera.second"})); + EXPECT_LT( + std::chrono::steady_clock::now() - snapshot_started, + std::chrono::milliseconds(100)); + std::thread second_stop([&] { + EXPECT_TRUE(registry.stopActivitiesForDevice(second->id())); + }); + second->waitForStopEntered(); + + std::atomic matching_stop_returned{false}; + std::thread matching_stop([&] { + EXPECT_TRUE(registry.stopActivitiesForDevice(first->id())); + matching_stop_returned = true; + }); + std::this_thread::yield(); + EXPECT_FALSE(matching_stop_returned.load()); + + second->releaseStop(); + second_stop.join(); + first->releaseStop(); + first_stop.join(); + matching_stop.join(); + EXPECT_TRUE(matching_stop_returned.load()); + EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); +} + +} // namespace +} // namespace cmvr::service 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 new file mode 100644 index 00000000..a4de10b9 --- /dev/null +++ b/cmvr-es/service/grpc/tests/camera_ptz_activity_registry_test.cpp @@ -0,0 +1,264 @@ +#include "service/grpc/include/camera_ptz_activity_registry.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "service/stop_all/include/stop_all_admission_gate.h" + +namespace cmvr::service { +namespace { + +class TestCamera final : public device::AbstractCamera { +public: + struct Call { + device::PtzCommand command; + bool stop; + int speed; + }; + + explicit TestCamera(std::string id) + { + id_ = std::move(id); + } + + std::string typeName() const override { return "TestCamera"; } + void getState(device::CameraState& state) override { state = {}; } + + bool controlPtz( + const device::PtzCommand command, + const bool stop, + const int speed) override + { + std::unique_lock lock(mutex_); + calls_.push_back({command, stop, speed}); + if (stop) { + stop_entered_ = true; + condition_.notify_all(); + condition_.wait(lock, [this] { return !block_stop_; }); + } + return !(stop && fail_stop_); + } + + bool stop() override + { + ++lifecycle_stop_calls_; + return true; + } + + std::vector calls() const + { + std::lock_guard lock(mutex_); + return calls_; + } + + void setFailStop(const bool fail) { fail_stop_ = fail; } + void blockStop() + { + std::lock_guard lock(mutex_); + block_stop_ = true; + stop_entered_ = false; + } + void waitForStopEntered() + { + std::unique_lock lock(mutex_); + condition_.wait(lock, [this] { return stop_entered_; }); + } + void releaseStop() + { + { + std::lock_guard lock(mutex_); + block_stop_ = false; + } + condition_.notify_all(); + } + int lifecycleStopCalls() const { return lifecycle_stop_calls_; } + +private: + mutable std::mutex mutex_; + std::condition_variable condition_; + std::vector calls_; + bool fail_stop_{false}; + bool block_stop_{false}; + bool stop_entered_{false}; + int lifecycle_stop_calls_{0}; +}; + +class CameraPtzActivityRegistryTest : public ::testing::Test { +protected: + void SetUp() override + { + globalStopAllAdmissionGate().clearForTesting(); + } + + void TearDown() override + { + globalStopAllAdmissionGate().clearForTesting(); + } +}; + +TEST_F(CameraPtzActivityRegistryTest, + StopAllMatchesEveryActiveStartAndNeverStopsLifecycle) +{ + CameraPtzActivityRegistry registry; + auto camera = std::make_shared("camera"); + + EXPECT_EQ( + registry.control( + camera->id(), camera, device::PtzCommand::PanLeft, false, 4), + CameraPtzActivityRegistry::DispatchResult::Success); + EXPECT_EQ( + registry.control( + camera->id(), camera, device::PtzCommand::ZoomIn, false, 7), + CameraPtzActivityRegistry::DispatchResult::Success); + + const auto ticket = globalStopAllAdmissionGate().beginStopAll(); + ASSERT_TRUE(ticket.valid()); + EXPECT_TRUE(registry.stopAllActivities()); + EXPECT_EQ(registry.activeCommandCount(), 0U); + EXPECT_EQ(camera->lifecycleStopCalls(), 0); + ASSERT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); + + const auto calls = camera->calls(); + ASSERT_EQ(calls.size(), 4U); + EXPECT_FALSE(calls[0].stop); + EXPECT_FALSE(calls[1].stop); + EXPECT_TRUE(calls[2].stop); + EXPECT_TRUE(calls[3].stop); +} + +TEST_F(CameraPtzActivityRegistryTest, + FailedStopRemainsTrackedForRetryAndAdmissionRecovers) +{ + CameraPtzActivityRegistry registry; + auto camera = std::make_shared("camera"); + ASSERT_EQ( + registry.control( + camera->id(), camera, device::PtzCommand::TiltUp, false, 3), + CameraPtzActivityRegistry::DispatchResult::Success); + + auto ticket = globalStopAllAdmissionGate().beginStopAll(); + camera->setFailStop(true); + std::vector failures; + EXPECT_FALSE(registry.stopAllActivities(&failures)); + EXPECT_EQ(registry.activeCommandCount(), 1U); + ASSERT_FALSE(failures.empty()); + EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, false) == false); + + EXPECT_EQ( + registry.control( + camera->id(), camera, device::PtzCommand::ZoomOut, false, 5), + CameraPtzActivityRegistry::DispatchResult::RejectedByStopAll); + + ticket = globalStopAllAdmissionGate().beginStopAll(); + camera->setFailStop(false); + EXPECT_TRUE(registry.stopAllActivities()); + EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); + EXPECT_EQ( + registry.control( + camera->id(), camera, device::PtzCommand::ZoomOut, false, 5), + CameraPtzActivityRegistry::DispatchResult::Success); +} + +TEST_F(CameraPtzActivityRegistryTest, + StopForDeviceIsSelectiveAndActiveIdsReflectFailures) +{ + CameraPtzActivityRegistry registry; + auto first = std::make_shared("camera.first"); + auto second = std::make_shared("camera.second"); + ASSERT_EQ( + registry.control( + first->id(), first, device::PtzCommand::PanLeft, false, 4), + CameraPtzActivityRegistry::DispatchResult::Success); + ASSERT_EQ( + registry.control( + second->id(), second, device::PtzCommand::ZoomIn, false, 7), + CameraPtzActivityRegistry::DispatchResult::Success); + EXPECT_EQ( + registry.activeDeviceIds(), + (std::vector{"camera.first", "camera.second"})); + EXPECT_EQ( + registry.trackedDeviceIds(), + (std::vector{"camera.first", "camera.second"})); + + const auto ticket = globalStopAllAdmissionGate().beginStopAll(); + first->setFailStop(true); + std::vector failures; + EXPECT_FALSE(registry.stopActivitiesForDevice(first->id(), &failures)); + EXPECT_EQ(registry.activeCommandCount(), 2U); + EXPECT_EQ(failures.size(), 1U); + EXPECT_EQ(second->calls().size(), 1U); + + first->setFailStop(false); + EXPECT_TRUE(registry.stopActivitiesForDevice(first->id())); + EXPECT_EQ(registry.activeCommandCount(), 1U); + EXPECT_EQ( + registry.activeDeviceIds(), + (std::vector{"camera.second"})); + EXPECT_TRUE(registry.stopActivitiesForDevice("missing")); + EXPECT_TRUE(registry.stopAllActivities()); + EXPECT_TRUE(registry.activeDeviceIds().empty()); + EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); + EXPECT_EQ(first->lifecycleStopCalls(), 0); + EXPECT_EQ(second->lifecycleStopCalls(), 0); +} + +TEST_F(CameraPtzActivityRegistryTest, + DifferentDevicesStopConcurrentlyButMatchingControlIsSerialized) +{ + CameraPtzActivityRegistry registry; + auto first = std::make_shared("camera.first"); + auto second = std::make_shared("camera.second"); + ASSERT_EQ( + registry.control( + first->id(), first, device::PtzCommand::PanLeft, false, 4), + CameraPtzActivityRegistry::DispatchResult::Success); + ASSERT_EQ( + registry.control( + second->id(), second, device::PtzCommand::ZoomIn, false, 7), + CameraPtzActivityRegistry::DispatchResult::Success); + first->blockStop(); + second->blockStop(); + + const auto ticket = globalStopAllAdmissionGate().beginStopAll(); + std::thread first_stop([&] { + EXPECT_TRUE(registry.stopActivitiesForDevice(first->id())); + }); + first->waitForStopEntered(); + const auto snapshot_started = std::chrono::steady_clock::now(); + EXPECT_EQ( + registry.trackedDeviceIds(), + (std::vector{"camera.first", "camera.second"})); + EXPECT_LT( + std::chrono::steady_clock::now() - snapshot_started, + std::chrono::milliseconds(100)); + std::thread second_stop([&] { + EXPECT_TRUE(registry.stopActivitiesForDevice(second->id())); + }); + second->waitForStopEntered(); + + std::atomic matching_stop_returned{false}; + std::thread matching_stop([&] { + EXPECT_TRUE(registry.stopActivitiesForDevice(first->id())); + matching_stop_returned = true; + }); + std::this_thread::yield(); + EXPECT_FALSE(matching_stop_returned.load()); + + second->releaseStop(); + second_stop.join(); + first->releaseStop(); + first_stop.join(); + matching_stop.join(); + EXPECT_TRUE(matching_stop_returned.load()); + EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); +} + +} // namespace +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp index e6c60806..68f76da0 100644 --- a/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp @@ -1,8 +1,10 @@ #include "service/grpc/include/grpc_agv_service.h" #include +#include #include #include +#include #include #include @@ -11,6 +13,7 @@ #include "cmvr/config/device_manager_config/device_manager_config.pb.h" #include "manager/control_authority/include/control_authority_manager.h" #include "manager/device_manager/include/device_manager.h" +#include "service/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { namespace { @@ -42,12 +45,18 @@ public: const device::AgvMotionOptions& options, const device::AgvAdapterParams&) override { - pose_ = pose; - pose_options_ = options; + ++navigate_pose_calls_; pose_cancellation_bound_ = static_cast(options.cancellation_requested); pose_cancellation_requested_during_call_ = pose_cancellation_bound_ && options.cancellation_requested(); + if (pose_cancellation_requested_during_call_) { + return device::AgvResult::failure( + device::AgvErrorCode::TaskCanceled, + "navigation canceled before fake device dispatch"); + } + pose_ = pose; + pose_options_ = options; pose_options_.cancellation_requested = {}; return pose_result_; } @@ -84,6 +93,7 @@ public: device::AgvResult setVelocity(const device::AgvVelocity&) override { + ++set_velocity_calls_; return device::AgvResult::failure( device::AgvErrorCode::CommandFailed, kNativeErrorMessage); @@ -124,7 +134,7 @@ public: return !probe.acquired; } - math::Pose2d pose_; + math::Pose2d pose_{}; device::AgvMotionOptions pose_options_; device::AgvResult pose_result_{device::AgvResult::success()}; std::string station_id_; @@ -142,6 +152,8 @@ public: int cancel_navigation_calls_{0}; int stop_velocity_calls_{0}; int confirm_stopped_calls_{0}; + int set_velocity_calls_{0}; + int navigate_pose_calls_{0}; bool emergency_stop_barrier_observed_{false}; bool cancel_navigation_barrier_observed_{false}; bool stop_velocity_barrier_observed_{false}; @@ -169,6 +181,7 @@ protected: void SetUp() override { control::ControlAuthorityManager::instance().clear(); + globalStopAllAdmissionGate().clearForTesting(); config::DeviceManagerConfig config; auto& manager = device::DeviceManager::getInstance(config); agv_ = std::make_shared(); @@ -182,6 +195,7 @@ protected: agv_.reset(); device::DeviceManager::destroyInstance(); control::ControlAuthorityManager::instance().clear(); + globalStopAllAdmissionGate().clearForTesting(); } std::shared_ptr agv_; @@ -402,6 +416,104 @@ TEST_F(GrpcAgvServiceTest, ActionLeaseBlocksOrdinaryMutatingRpcs) authority.release(action_lease.token); } +TEST_F(GrpcAgvServiceTest, NavigationCancellationIncludesControlLease) +{ + auto& authority = control::ControlAuthorityManager::instance(); + const auto barrier = authority.preemptAcquire( + "test-agv", "stop-all-test", std::chrono::hours(1)); + ASSERT_TRUE(barrier.acquired) << barrier.detail; + + api::AgvNavigateToPoseCommand_Request request; + request.mutable_header()->set_device_id("test-agv"); + api::AgvNavigateToPoseCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->navigateToPose( + &context, &request, &response); + + EXPECT_EQ( + status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_EQ(agv_->navigate_pose_calls_, 0); + authority.release(barrier.token); +} + +TEST_F(GrpcAgvServiceTest, SetVelocityDispatchRunsUnderControlFence) +{ + api::AgvSetVelocityCommand_Request request; + request.mutable_header()->set_device_id("test-agv"); + request.mutable_velocity()->set_vx(0.1); + api::AgvSetVelocityCommand_Feedback response; + grpc::ServerContext context; + + const auto status = service_->setVelocity( + &context, &request, &response); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_EQ(agv_->set_velocity_calls_, 1); +} + +TEST_F(GrpcAgvServiceTest, + StopAllGateRejectsMutatingCommandsButAllowsReadsAndStops) +{ + auto& admission = globalStopAllAdmissionGate(); + const auto ticket = admission.beginStopAll(); + ASSERT_TRUE(ticket.valid()); + + api::AgvNavigateToPoseCommand_Request navigation_request; + navigation_request.mutable_header()->set_device_id("test-agv"); + api::AgvNavigateToPoseCommand_Feedback navigation_response; + grpc::ServerContext navigation_context; + const auto navigation_status = service_->navigateToPose( + &navigation_context, &navigation_request, &navigation_response); + + api::AgvSetVelocityCommand_Request velocity_request; + velocity_request.mutable_header()->set_device_id("test-agv"); + velocity_request.mutable_velocity()->set_vx(0.1); + api::AgvSetVelocityCommand_Feedback velocity_response; + grpc::ServerContext velocity_context; + const auto velocity_status = service_->setVelocity( + &velocity_context, &velocity_request, &velocity_response); + + api::AgvRuntimeStateCommand_Request state_request; + state_request.mutable_header()->set_device_id("test-agv"); + api::AgvRuntimeStateCommand_Feedback state_response; + grpc::ServerContext state_context; + const auto state_status = service_->getRuntimeState( + &state_context, &state_request, &state_response); + + api::CommandHeader_Request stop_request; + stop_request.set_device_id("test-agv"); + api::CommandHeader_Feedback stop_response; + grpc::ServerContext stop_context; + const auto stop_status = service_->cancelNavigation( + &stop_context, &stop_request, &stop_response); + + EXPECT_EQ( + navigation_status.error_code(), + grpc::StatusCode::UNAVAILABLE); + EXPECT_FALSE(navigation_response.header().success()); + EXPECT_EQ(agv_->navigate_pose_calls_, 0); + EXPECT_EQ( + velocity_status.error_code(), + grpc::StatusCode::UNAVAILABLE); + EXPECT_FALSE(velocity_response.header().success()); + EXPECT_EQ(agv_->set_velocity_calls_, 0); + + EXPECT_TRUE(state_status.ok()) << state_status.error_message(); + EXPECT_TRUE(state_response.header().success()); + EXPECT_TRUE(stop_status.ok()) << stop_status.error_message(); + EXPECT_TRUE(stop_response.success()) + << stop_response.error_message(); + EXPECT_EQ(agv_->cancel_navigation_calls_, 2); + + EXPECT_TRUE(admission.finishStopAll(ticket, true)); + const auto resumed_status = service_->navigateToPose( + &navigation_context, &navigation_request, &navigation_response); + EXPECT_TRUE(resumed_status.ok()) << resumed_status.error_message(); + EXPECT_TRUE(navigation_response.header().success()); + EXPECT_EQ(agv_->navigate_pose_calls_, 1); +} + TEST_F(GrpcAgvServiceTest, QueriesBypassAndSafetyStopsPreemptActionLease) { auto& authority = control::ControlAuthorityManager::instance(); @@ -428,60 +540,99 @@ TEST_F(GrpcAgvServiceTest, QueriesBypassAndSafetyStopsPreemptActionLease) api::CommandHeader_Feedback emergency_response; grpc::ServerContext emergency_context; - const auto emergency_status = service_->emergencyStop( - &emergency_context, - &stop_request, - &emergency_response); - EXPECT_TRUE(emergency_status.ok()) << emergency_status.error_message(); + auto emergency = std::async( + std::launch::async, + [this, &emergency_context, &stop_request, &emergency_response]() { + return service_->emergencyStop( + &emergency_context, + &stop_request, + &emergency_response); + }); + const auto emergency_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (authority.validate(action_lease.token) && + std::chrono::steady_clock::now() < emergency_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } EXPECT_FALSE(authority.validate(action_lease.token)); + authority.release(action_lease.token); + const auto emergency_status = emergency.get(); + EXPECT_TRUE(emergency_status.ok()) << emergency_status.error_message(); EXPECT_TRUE(agv_->emergency_stop_barrier_observed_); - const auto lease_after_emergency = authority.tryAcquire( + const auto released_lease_after_emergency = authority.tryAcquire( "test-agv", - "action-sequence:after-emergency", + "action-sequence:after-emergency-release", std::chrono::hours(1)); - ASSERT_TRUE(lease_after_emergency.acquired) - << lease_after_emergency.detail; + ASSERT_TRUE(released_lease_after_emergency.acquired) + << released_lease_after_emergency.detail; api::CommandHeader_Feedback cancel_response; grpc::ServerContext cancel_context; - const auto cancel_status = service_->cancelNavigation( - &cancel_context, - &stop_request, - &cancel_response); + auto cancel = std::async( + std::launch::async, + [this, &cancel_context, &stop_request, &cancel_response]() { + return service_->cancelNavigation( + &cancel_context, + &stop_request, + &cancel_response); + }); + const auto cancel_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (authority.validate(released_lease_after_emergency.token) && + std::chrono::steady_clock::now() < cancel_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_FALSE(authority.validate( + released_lease_after_emergency.token)); + authority.release(released_lease_after_emergency.token); + const auto cancel_status = cancel.get(); EXPECT_TRUE(cancel_status.ok()) << cancel_status.error_message(); - EXPECT_FALSE(authority.validate(lease_after_emergency.token)); EXPECT_TRUE(agv_->cancel_navigation_barrier_observed_); - const auto lease_after_cancel = authority.tryAcquire( + const auto released_lease_after_cancel = authority.tryAcquire( "test-agv", - "action-sequence:after-cancel", + "action-sequence:after-cancel-release", std::chrono::hours(1)); - ASSERT_TRUE(lease_after_cancel.acquired) - << lease_after_cancel.detail; + ASSERT_TRUE(released_lease_after_cancel.acquired) + << released_lease_after_cancel.detail; api::CommandHeader_Feedback velocity_response; grpc::ServerContext velocity_context; - const auto velocity_status = service_->stopVelocityControl( - &velocity_context, - &stop_request, - &velocity_response); + auto velocity = std::async( + std::launch::async, + [this, &velocity_context, &stop_request, &velocity_response]() { + return service_->stopVelocityControl( + &velocity_context, + &stop_request, + &velocity_response); + }); + const auto velocity_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (authority.validate(released_lease_after_cancel.token) && + std::chrono::steady_clock::now() < velocity_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_FALSE(authority.validate( + released_lease_after_cancel.token)); + authority.release(released_lease_after_cancel.token); + const auto velocity_status = velocity.get(); EXPECT_TRUE(velocity_status.ok()) << velocity_status.error_message(); - EXPECT_FALSE(authority.validate(lease_after_cancel.token)); EXPECT_TRUE(agv_->stop_velocity_barrier_observed_); - EXPECT_EQ(agv_->emergency_stop_calls_, 1); - EXPECT_EQ(agv_->cancel_navigation_calls_, 1); - EXPECT_EQ(agv_->stop_velocity_calls_, 1); - EXPECT_EQ(agv_->confirm_stopped_calls_, 3); - EXPECT_TRUE(agv_->confirm_stopped_barrier_observed_); - const auto lease_after_stops = authority.tryAcquire( "test-agv", "action-sequence:after-stops", std::chrono::hours(1)); ASSERT_TRUE(lease_after_stops.acquired) << lease_after_stops.detail; + + EXPECT_EQ(agv_->emergency_stop_calls_, 2); + EXPECT_EQ(agv_->cancel_navigation_calls_, 2); + EXPECT_EQ(agv_->stop_velocity_calls_, 2); + EXPECT_EQ(agv_->confirm_stopped_calls_, 3); + EXPECT_TRUE(agv_->confirm_stopped_barrier_observed_); + authority.release(lease_after_stops.token); } @@ -513,6 +664,39 @@ TEST_F(GrpcAgvServiceTest, "normal-control-after-unconfirmed-stop", std::chrono::hours(1)); EXPECT_FALSE(lease.acquired); + + auto& authority = control::ControlAuthorityManager::instance(); + const auto recovery = authority.preemptAcquire( + "test-agv", "confirmed-stop-recovery", std::chrono::hours(1)); + ASSERT_TRUE(recovery.acquired) << recovery.detail; + ASSERT_TRUE(authority.waitForPreemptedRelease( + recovery.token, std::chrono::milliseconds::zero())); + ASSERT_TRUE(authority.recoverRetiredSafetyHolders(recovery.token)); + authority.release(recovery.token); + const auto recovered = authority.tryAcquire( + "test-agv", "normal-control-after-recovery", std::chrono::hours(1)); + EXPECT_TRUE(recovered.acquired) << recovered.detail; + authority.release(recovered.token); +} + +TEST_F(GrpcAgvServiceTest, StopMappingFailureReleasesOrdinaryControlLease) +{ + api::CommandHeader_Request request; + request.set_device_id("test-agv"); + api::CommandHeader_Feedback response; + grpc::ServerContext context; + + const auto status = service_->stopMapping( + &context, &request, &response); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_FALSE(response.success()); + auto& authority = control::ControlAuthorityManager::instance(); + const auto lease = authority.tryAcquire( + "test-agv", "normal-control-after-mapping-failure", + std::chrono::hours(1)); + EXPECT_TRUE(lease.acquired) << lease.detail; + authority.release(lease.token); } TEST_F(GrpcAgvServiceTest, NavigateToStationForwardsPgvAdapterParams) 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 e44671d3..f5d24016 100644 --- a/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp @@ -18,6 +18,7 @@ #include "cmvr/config/device_manager_config/device_manager_config.pb.h" #include "manager/control_authority/include/control_authority_manager.h" #include "manager/device_manager/include/device_manager.h" +#include "service/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { namespace { @@ -91,9 +92,9 @@ public: bool isFault() const override { return false; } device::Result moveJ(const device::JointPositionCommand&, - const device::MotionOptions&) override + const device::MotionOptions& options) override { - return enterMotion("moveJ", move_j_calls_); + return enterMotion("moveJ", move_j_calls_, options); } device::Result speedJ(const device::JointVelocityCommand&, double, @@ -107,10 +108,10 @@ public: } device::Result moveL( const device::CartesianPose&, - const device::MotionOptions&, + const device::MotionOptions& options, device::FrameType = device::FrameType::Base) override { - return enterMotion("moveL", move_l_calls_); + return enterMotion("moveL", move_l_calls_, options); } device::Result speedL( const device::CartesianVelocity&, @@ -243,6 +244,18 @@ public: return torque_off_calls_; } + bool lastMotionHadCancellation() const + { + std::lock_guard lock(motion_mutex_); + return last_motion_had_cancellation_; + } + + bool lastMotionCancellationRequested() const + { + std::lock_guard lock(motion_mutex_); + return last_motion_cancellation_requested_; + } + device::Result startServoMode(const device::ServoOptions&) override { return device::Result::success(); @@ -344,10 +357,18 @@ public: std::string last_request_json; private: - device::Result enterMotion(const char* operation, int& call_count) + device::Result enterMotion( + const char* operation, + int& call_count, + const device::MotionOptions& options) { std::unique_lock lock(motion_mutex_); ++call_count; + last_motion_had_cancellation_ = + static_cast(options.cancellation_requested); + last_motion_cancellation_requested_ = + last_motion_had_cancellation_ && + options.cancellation_requested(); if (!block_next_motion_) { return device::Result::success(); } @@ -380,6 +401,8 @@ private: int move_l_calls_{0}; int stop_motion_calls_{0}; int torque_off_calls_{0}; + bool last_motion_had_cancellation_{false}; + bool last_motion_cancellation_requested_{false}; }; class JsonCommandNonArmDevice final : public device::AbstractDevice { @@ -407,6 +430,7 @@ protected: void SetUp() override { control::ControlAuthorityManager::instance().clear(); + globalStopAllAdmissionGate().clearForTesting(); device::DeviceManager::destroyInstance(); config::DeviceManagerConfig config; auto& manager = device::DeviceManager::getInstance(config); @@ -428,6 +452,7 @@ protected: left_arm_.reset(); device::DeviceManager::destroyInstance(); control::ControlAuthorityManager::instance().clear(); + globalStopAllAdmissionGate().clearForTesting(); } grpc::Status execute(const std::string& device_id, @@ -610,9 +635,10 @@ TEST_F(GrpcArmServiceTest, grpc::Status torque_off_status; api::CommandHeader_Feedback stop_response; grpc::Status stop_status; - MoveOutcome resumed_move; + MoveOutcome before_retired_handler_release; bool stop_started = false; std::future blocked_stop; + std::future torque_off; if (move_started) { conflict = moveL("aubo_arm"); aubo_arm_->blockNextStopMotion(); @@ -624,13 +650,15 @@ TEST_F(GrpcArmServiceTest, stop_started = aubo_arm_->waitForBlockingStop( std::chrono::seconds(2)); if (stop_started) { - torque_off_status = torqueOff( - "aubo_arm", torque_off_response); + torque_off = std::async( + std::launch::async, + [this, &torque_off_response]() { + return torqueOff("aubo_arm", torque_off_response); + }); during_stop = moveL("aubo_arm"); } aubo_arm_->releaseBlockingStop(); - stop_status = blocked_stop.get(); - resumed_move = moveL("aubo_arm"); + before_retired_handler_release = moveL("aubo_arm"); } // Keep the original RPC active until after the replacement MoveL has @@ -638,6 +666,13 @@ TEST_F(GrpcArmServiceTest, // takes time to unwind and guards the lease hand-off itself. aubo_arm_->releaseBlockingMotion(); const auto original_move = blocked_move.get(); + if (blocked_stop.valid()) { + stop_status = blocked_stop.get(); + } + if (torque_off.valid()) { + torque_off_status = torque_off.get(); + } + const auto resumed_move = moveL("aubo_arm"); ASSERT_TRUE(move_started); ASSERT_TRUE(stop_started); @@ -655,6 +690,9 @@ TEST_F(GrpcArmServiceTest, EXPECT_TRUE(stop_status.ok()) << stop_status.error_message(); EXPECT_TRUE(stop_response.success()) << stop_response.error_message(); + EXPECT_EQ( + before_retired_handler_release.status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); EXPECT_TRUE(resumed_move.status.ok()) << resumed_move.status.error_message(); EXPECT_TRUE(resumed_move.response_success) @@ -665,8 +703,67 @@ TEST_F(GrpcArmServiceTest, << original_move.response_error; EXPECT_EQ(aubo_arm_->moveJCalls(), 1); EXPECT_EQ(aubo_arm_->moveLCalls(), 1); - EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1); - EXPECT_EQ(aubo_arm_->torqueOffCalls(), 1); + EXPECT_EQ(aubo_arm_->stopMotionCalls(), 2); + EXPECT_EQ(aubo_arm_->torqueOffCalls(), 2); +} + +TEST_F(GrpcArmServiceTest, MoveBindsLeaseRevocationCancellation) +{ + const auto outcome = moveJ("aubo_arm"); + + ASSERT_TRUE(outcome.status.ok()) + << outcome.status.error_message(); + EXPECT_TRUE(outcome.response_success) << outcome.response_error; + EXPECT_TRUE(aubo_arm_->lastMotionHadCancellation()); + EXPECT_FALSE(aubo_arm_->lastMotionCancellationRequested()); +} + +TEST_F(GrpcArmServiceTest, + StopAllGateRejectsMutatingCommandsButAllowsReadsAndStops) +{ + auto& admission = globalStopAllAdmissionGate(); + const auto ticket = admission.beginStopAll(); + ASSERT_TRUE(ticket.valid()); + + const auto rejected_move = moveJ("aubo_arm"); + + api::JsonDeviceCommand_Feedback json_response; + const auto json_status = execute( + "aubo_arm", R"({"command":"cabinet_io"})", json_response); + + api::JointRequest state_request; + state_request.mutable_header()->set_device_id("aubo_arm"); + api::JointResponse state_response; + grpc::ServerContext state_context; + const auto state_status = service_->getJointState( + &state_context, &state_request, &state_response); + + api::CommandHeader_Feedback stop_response; + const auto stop_status = stopMotion("aubo_arm", stop_response); + + EXPECT_EQ( + rejected_move.status.error_code(), + grpc::StatusCode::UNAVAILABLE); + EXPECT_FALSE(rejected_move.response_success); + EXPECT_EQ(aubo_arm_->moveJCalls(), 0); + EXPECT_EQ(json_status.error_code(), grpc::StatusCode::UNAVAILABLE); + EXPECT_FALSE(json_response.header().success()); + EXPECT_EQ(aubo_arm_->execute_calls, 0); + + EXPECT_TRUE(state_status.ok()) << state_status.error_message(); + EXPECT_TRUE(state_response.header().success()); + EXPECT_TRUE(stop_status.ok()) << stop_status.error_message(); + EXPECT_TRUE(stop_response.success()) + << stop_response.error_message(); + EXPECT_EQ(aubo_arm_->stopMotionCalls(), 2); + + EXPECT_TRUE(admission.finishStopAll(ticket, true)); + const auto resumed_move = moveJ("aubo_arm"); + EXPECT_TRUE(resumed_move.status.ok()) + << resumed_move.status.error_message(); + EXPECT_TRUE(resumed_move.response_success) + << resumed_move.response_error; + EXPECT_EQ(aubo_arm_->moveJCalls(), 1); } TEST_F(GrpcArmServiceTest, @@ -683,15 +780,22 @@ TEST_F(GrpcArmServiceTest, MoveOutcome conflict; api::CommandHeader_Feedback stop_response; grpc::Status stop_status; - MoveOutcome resumed_move; + MoveOutcome before_retired_handler_release; if (move_started) { conflict = moveJ("aubo_arm"); - stop_status = stopMotion("aubo_arm", stop_response); - resumed_move = moveJ("aubo_arm"); + auto stop = std::async( + std::launch::async, + [this, &stop_response]() { + return stopMotion("aubo_arm", stop_response); + }); + before_retired_handler_release = moveJ("aubo_arm"); + aubo_arm_->releaseBlockingMotion(); + stop_status = stop.get(); } aubo_arm_->releaseBlockingMotion(); const auto original_move = blocked_move.get(); + const auto resumed_move = moveJ("aubo_arm"); ASSERT_TRUE(move_started); EXPECT_EQ(conflict.status.error_code(), @@ -700,6 +804,9 @@ TEST_F(GrpcArmServiceTest, EXPECT_TRUE(stop_status.ok()) << stop_status.error_message(); EXPECT_TRUE(stop_response.success()) << stop_response.error_message(); + EXPECT_EQ( + before_retired_handler_release.status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); EXPECT_TRUE(resumed_move.status.ok()) << resumed_move.status.error_message(); EXPECT_TRUE(resumed_move.response_success) @@ -710,7 +817,7 @@ TEST_F(GrpcArmServiceTest, << original_move.response_error; EXPECT_EQ(aubo_arm_->moveJCalls(), 1); EXPECT_EQ(aubo_arm_->moveLCalls(), 1); - EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1); + EXPECT_EQ(aubo_arm_->stopMotionCalls(), 2); } TEST_F(GrpcArmServiceTest, StopMotionFailureRetainsSafetyBarrier) @@ -734,6 +841,19 @@ TEST_F(GrpcArmServiceTest, StopMotionFailureRetainsSafetyBarrier) grpc::StatusCode::FAILED_PRECONDITION); EXPECT_EQ(aubo_arm_->moveJCalls(), 0); EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1); + + authority.release(action_lease.token); + const auto recovery = authority.preemptAcquire( + "aubo_arm", "confirmed-stop-recovery", std::chrono::hours(1)); + ASSERT_TRUE(recovery.acquired) << recovery.detail; + ASSERT_TRUE(authority.waitForPreemptedRelease( + recovery.token, std::chrono::milliseconds::zero())); + ASSERT_TRUE(authority.recoverRetiredSafetyHolders(recovery.token)); + authority.release(recovery.token); + const auto recovered = authority.tryAcquire( + "aubo_arm", "move-after-recovery", std::chrono::hours(1)); + EXPECT_TRUE(recovered.acquired) << recovered.detail; + authority.release(recovered.token); } TEST_F(GrpcArmServiceTest, StopMotionExceptionRetainsSafetyBarrier) @@ -757,6 +877,15 @@ TEST_F(GrpcArmServiceTest, StopMotionExceptionRetainsSafetyBarrier) grpc::StatusCode::FAILED_PRECONDITION); EXPECT_EQ(aubo_arm_->moveLCalls(), 0); EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1); + + authority.release(action_lease.token); + const auto recovery = authority.preemptAcquire( + "aubo_arm", "confirmed-exception-recovery", std::chrono::hours(1)); + ASSERT_TRUE(recovery.acquired) << recovery.detail; + ASSERT_TRUE(authority.waitForPreemptedRelease( + recovery.token, std::chrono::milliseconds::zero())); + ASSERT_TRUE(authority.recoverRetiredSafetyHolders(recovery.token)); + authority.release(recovery.token); } } // namespace 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 1bbdc818..5e5bf2f5 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 @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -16,6 +17,8 @@ #include #include +#include "service/stop_all/include/stop_all_admission_gate.h" + namespace cmvr::service { namespace { @@ -667,5 +670,110 @@ TEST(ArmTeleopServiceTest, ExpiredSetpointIsNeverDispatched) EXPECT_EQ(applied.front(), 1U); } +TEST(ArmTeleopServiceTest, + StopAllFencesDispatchRejectsAdmissionAndAllowsReuse) +{ + auto& authority = control::ControlAuthorityManager::instance(); + auto& admission = globalStopAllAdmissionGate(); + authority.clear(); + admission.clearForTesting(); + + auto backend = std::make_shared(); + TeleopServerHarness harness(backend); + grpc::ClientContext active_context; + active_context.set_deadline( + std::chrono::system_clock::now() + 3s); + auto active_stream = harness.stub().Teleoperate(&active_context); + + ASSERT_TRUE(active_stream->Write( + makeOpenFrame(makeManifest(), 500, 2000))); + expectOpeningFrames(*active_stream); + + backend->blockApply(); + ASSERT_TRUE(active_stream->Write(makeSetpoint(1, 400000))); + ASSERT_TRUE(backend->waitForApply(1, 1s)); + + const auto stop_all_ticket = admission.beginStopAll(); + ASSERT_TRUE(stop_all_ticket.valid()); + + grpc::ClientContext blocked_context; + blocked_context.set_deadline( + std::chrono::system_clock::now() + 2s); + auto blocked_stream = harness.stub().Teleoperate(&blocked_context); + ASSERT_TRUE(blocked_stream->Write(makeOpenFrame(makeManifest()))); + ASSERT_TRUE(blocked_stream->WritesDone()); + arm_teleop::ServerFrame response; + ASSERT_TRUE(blocked_stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_REJECTED); + const auto blocked_status = blocked_stream->Finish(); + EXPECT_EQ( + blocked_status.error_code(), grpc::StatusCode::UNAVAILABLE); + EXPECT_EQ(backend->openCalls(), 1); + + const auto safety_lease = authority.preemptAcquire( + makeManifest().robot_id(), "stop-all-test", + std::chrono::hours(1)); + ASSERT_TRUE(safety_lease.acquired) << safety_lease.detail; + const auto safety_token = safety_lease.token; + auto dispatch_fence = std::async( + std::launch::async, + [&authority, safety_token] { + return authority.waitForPreemptedRelease(safety_token, 1s); + }); + + EXPECT_EQ( + dispatch_fence.wait_for(50ms), std::future_status::timeout); + backend->releaseApply(); + ASSERT_EQ( + dispatch_fence.wait_for(1s), std::future_status::ready); + EXPECT_TRUE(dispatch_fence.get()); + + ASSERT_TRUE(active_stream->Read(&response)); + EXPECT_EQ(response.status().applied_sequence(), 1U); + ASSERT_TRUE(active_stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_LEASE_LOST); + EXPECT_EQ( + response.status().stop_reason(), + arm_teleop::STOP_REASON_LEASE_REVOKED); + const auto active_status = active_stream->Finish(); + EXPECT_TRUE( + active_status.error_code() == grpc::StatusCode::ABORTED || + active_status.error_code() == grpc::StatusCode::CANCELLED) + << active_status.error_message(); + + const auto applied = backend->appliedSequences(); + ASSERT_EQ(applied.size(), 1U); + EXPECT_EQ(applied.front(), 1U); + const auto stopped = backend->stopReasons(); + ASSERT_FALSE(stopped.empty()); + EXPECT_EQ( + stopped.back(), arm_teleop::STOP_REASON_LEASE_REVOKED); + + authority.release(safety_token); + ASSERT_TRUE(admission.finishStopAll(stop_all_ticket, true)); + + grpc::ClientContext resumed_context; + resumed_context.set_deadline( + std::chrono::system_clock::now() + 2s); + auto resumed_stream = harness.stub().Teleoperate(&resumed_context); + ASSERT_TRUE(resumed_stream->Write(makeOpenFrame(makeManifest()))); + expectOpeningFrames(*resumed_stream); + ASSERT_TRUE(resumed_stream->Write(makeStop())); + ASSERT_TRUE(resumed_stream->WritesDone()); + ASSERT_TRUE(resumed_stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_STOPPED); + EXPECT_TRUE(resumed_stream->Finish().ok()); + EXPECT_EQ(backend->openCalls(), 2); + + authority.clear(); + admission.clearForTesting(); +} + } // namespace } // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_dexhand_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_dexhand_service_test.cpp new file mode 100644 index 00000000..6ebdb2a6 --- /dev/null +++ b/cmvr-es/service/grpc/tests/grpc_dexhand_service_test.cpp @@ -0,0 +1,351 @@ +#include "service/grpc/include/grpc_dexhand_service.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "manager/control_authority/include/control_authority_manager.h" +#include "manager/device_manager/include/device_manager.h" +#include "service/grpc/include/media_activity_coordinator.h" +#include "service/stop_all/include/stop_all_admission_gate.h" + +namespace cmvr::service { +namespace { + +using namespace std::chrono_literals; + +class FakeDexHand final : public device::AbstractDexHand { +public: + FakeDexHand() { + id_ = "test-dexhand"; + sensor_points_[0] = TactilePoint::fromFz(42); + } + + std::string typeName() const override { return "FakeDexHand"; } + Status state() const override { return Status::STREAMING; } + std::string lastError() const override { return {}; } + + bool stopOperationalActivity() override { + ++stop_operational_calls; + return true; + } + + bool resumeOperationalActivity() override { + ++resume_operational_calls; + return true; + } + + void setAngles(const std::vector&) override { + ++set_angle_calls; + std::unique_lock lock(command_mutex_); + command_entered_ = true; + command_cv_.notify_all(); + command_cv_.wait(lock, [this] { return !block_angle_command_; }); + } + + void setPositions(const std::vector&) override { + ++set_position_calls; + } + + void setVelocities(const std::vector&) override { + ++set_speed_calls; + } + + void setForce(const std::vector&) override { + ++set_force_calls; + } + + void setPresetAct(int) override { + ++set_preset_calls; + } + + void setTactilePollingRegions( + const std::vector&) override { + ++configure_sensor_calls; + } + + std::vector getSensorData() override { + ++sensor_read_calls; + return {sensorRegion()}; + } + + TactileRegionData getSensorData( + FingerType finger, TactileRegion region) override { + return finger == FingerType::INDEX && region == TactileRegion::TIP + ? sensorRegion() + : TactileRegionData{}; + } + + ResultantForce getResultantForce( + FingerType finger, TactileRegion region) override { + return finger == FingerType::INDEX && region == TactileRegion::TIP + ? sensor_points_[0] + : ResultantForce{}; + } + + void blockAngleCommand() { + std::lock_guard lock(command_mutex_); + block_angle_command_ = true; + command_entered_ = false; + } + + bool waitForAngleCommand(const std::chrono::milliseconds timeout) { + std::unique_lock lock(command_mutex_); + return command_cv_.wait_for( + lock, timeout, [this] { return command_entered_; }); + } + + void releaseAngleCommand() { + { + std::lock_guard lock(command_mutex_); + block_angle_command_ = false; + } + command_cv_.notify_all(); + } + + std::atomic set_position_calls{0}; + std::atomic set_angle_calls{0}; + std::atomic set_force_calls{0}; + std::atomic set_speed_calls{0}; + std::atomic set_preset_calls{0}; + std::atomic configure_sensor_calls{0}; + std::atomic sensor_read_calls{0}; + std::atomic stop_operational_calls{0}; + std::atomic resume_operational_calls{0}; + +private: + TactileRegionData sensorRegion() { + return TactileRegionData( + FingerType::INDEX, + TactileRegion::TIP, + TactileMatrixView{sensor_points_.data(), 1, 1}, + "fake-tactile"); + } + + std::array sensor_points_{}; + std::mutex command_mutex_; + std::condition_variable command_cv_; + bool block_angle_command_{false}; + bool command_entered_{false}; +}; + +class GrpcDexHandServiceTest : public ::testing::Test { +protected: + void SetUp() override { + device::DeviceManager::destroyInstance(); + control::ControlAuthorityManager::instance().clear(); + globalStopAllAdmissionGate().clearForTesting(); + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + hand_ = std::make_shared(); + manager.registerDevice(hand_); + service_ = std::make_unique(); + } + + void TearDown() override { + hand_->releaseAngleCommand(); + if (server_) { + server_->Shutdown(); + server_->Wait(); + } + stub_.reset(); + service_.reset(); + hand_.reset(); + if (!socket_path_.empty()) { + std::remove(socket_path_.c_str()); + } + device::DeviceManager::destroyInstance(); + control::ControlAuthorityManager::instance().clear(); + globalStopAllAdmissionGate().clearForTesting(); + } + + bool startGrpcServer() { + socket_path_ = + "/tmp/cmvr_dexhand_service_test_" + + std::to_string(static_cast(::getpid())) + ".sock"; + std::remove(socket_path_.c_str()); + const std::string address = "unix:" + socket_path_; + grpc::ServerBuilder builder; + builder.AddListeningPort( + address, + grpc::InsecureServerCredentials()); + builder.RegisterService(service_.get()); + server_ = builder.BuildAndStart(); + if (!server_) { + return false; + } + stub_ = api::DexHandService::NewStub(grpc::CreateChannel( + address, + grpc::InsecureChannelCredentials())); + return stub_ != nullptr; + } + + static api::GetSensorDataStreamCommand_Request sensorStreamRequest() { + api::GetSensorDataStreamCommand_Request request; + request.mutable_header()->set_device_id("test-dexhand"); + return request; + } + + std::shared_ptr hand_; + std::unique_ptr service_; + std::unique_ptr server_; + std::unique_ptr stub_; + std::string socket_path_; +}; + +TEST_F(GrpcDexHandServiceTest, StopAllGateRejectsEveryControlCommand) { + const auto ticket = globalStopAllAdmissionGate().beginStopAll(); + ASSERT_TRUE(ticket.valid()); + + grpc::ServerContext position_context; + api::SetDexHandPositionsCommand_Request position_request; + api::SetDexHandPositionsCommand_Feedback position_response; + position_request.mutable_header()->set_device_id("test-dexhand"); + EXPECT_TRUE(service_->SetDexHandPos( + &position_context, &position_request, &position_response).ok()); + EXPECT_FALSE(position_response.header().success()); + + grpc::ServerContext angle_context; + api::SetDexHandAnglesCommand_Request angle_request; + api::SetDexHandAnglesCommand_Feedback angle_response; + angle_request.mutable_header()->set_device_id("test-dexhand"); + EXPECT_TRUE(service_->SetDexHandAngle( + &angle_context, &angle_request, &angle_response).ok()); + EXPECT_FALSE(angle_response.header().success()); + + grpc::ServerContext force_context; + api::SetDexHandForceCommand_Request force_request; + api::SetDexHandForceCommand_Feedback force_response; + force_request.mutable_header()->set_device_id("test-dexhand"); + EXPECT_TRUE(service_->SetDexHandForce( + &force_context, &force_request, &force_response).ok()); + EXPECT_FALSE(force_response.header().success()); + + grpc::ServerContext speed_context; + api::SetDexHandSpeedCommand_Request speed_request; + api::SetDexHandSpeedCommand_Feedback speed_response; + speed_request.mutable_header()->set_device_id("test-dexhand"); + EXPECT_TRUE(service_->SetDexHandSpeed( + &speed_context, &speed_request, &speed_response).ok()); + EXPECT_FALSE(speed_response.header().success()); + + grpc::ServerContext preset_context; + api::SetDexHandPresetActCommand_Request preset_request; + api::SetDexHandPresetActCommand_Feedback preset_response; + preset_request.mutable_header()->set_device_id("test-dexhand"); + EXPECT_TRUE(service_->SetDexHandPresetAct( + &preset_context, &preset_request, &preset_response).ok()); + EXPECT_FALSE(preset_response.header().success()); + + EXPECT_EQ(hand_->set_position_calls.load(), 0); + EXPECT_EQ(hand_->set_angle_calls.load(), 0); + EXPECT_EQ(hand_->set_force_calls.load(), 0); + EXPECT_EQ(hand_->set_speed_calls.load(), 0); + EXPECT_EQ(hand_->set_preset_calls.load(), 0); + EXPECT_EQ(hand_->resume_operational_calls.load(), 0); + EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); +} + +TEST_F(GrpcDexHandServiceTest, + SafetyPreemptionCannotPassAnExecutingDeviceDispatch) { + hand_->blockAngleCommand(); + grpc::ServerContext context; + api::SetDexHandAnglesCommand_Request request; + api::SetDexHandAnglesCommand_Feedback response; + request.mutable_header()->set_device_id("test-dexhand"); + request.add_values()->set_value(0.5F); + + auto rpc = std::async(std::launch::async, [&] { + return service_->SetDexHandAngle(&context, &request, &response); + }); + ASSERT_TRUE(hand_->waitForAngleCommand(1s)); + + auto& authority = control::ControlAuthorityManager::instance(); + auto safety_result = authority.preemptAcquire( + "test-dexhand", + "test-stop-all", + std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>(1h)); + ASSERT_TRUE(safety_result.acquired); + const auto safety_token = safety_result.token; + auto dispatch_fence = std::async(std::launch::async, + [&authority, safety_token] { + return authority.waitForPreemptedRelease(safety_token, 1s); + }); + EXPECT_EQ( + dispatch_fence.wait_for(50ms), std::future_status::timeout); + + hand_->releaseAngleCommand(); + ASSERT_EQ(rpc.wait_for(1s), std::future_status::ready); + EXPECT_TRUE(rpc.get().ok()); + EXPECT_TRUE(response.header().success()); + ASSERT_EQ(dispatch_fence.wait_for(1s), std::future_status::ready); + EXPECT_TRUE(dispatch_fence.get()); + authority.release(safety_token); +} + +TEST_F(GrpcDexHandServiceTest, + StopAllCancelsOldSensorStreamAndARecoveredStreamResumesActivity) { + ASSERT_TRUE(startGrpcServer()); + grpc::ClientContext context; + context.set_deadline(std::chrono::system_clock::now() + 3s); + auto stream = stub_->GetSensorDataStream(&context); + ASSERT_TRUE(stream->Write(sensorStreamRequest())); + + api::GetSensorDataStreamCommand_Feedback feedback; + ASSERT_TRUE(stream->Read(&feedback)); + ASSERT_TRUE(feedback.header().success()); + ASSERT_GE(hand_->sensor_read_calls.load(), 1); + + const auto admission_ticket = globalStopAllAdmissionGate().beginStopAll(); + const auto media_ticket = globalMediaActivityCoordinator().beginStopAll(); + ASSERT_TRUE(admission_ticket.valid()); + ASSERT_TRUE(media_ticket.valid()); + + stream->WritesDone(); + while (stream->Read(&feedback)) { + } + const auto status = stream->Finish(); + EXPECT_TRUE( + status.ok() || status.error_code() == grpc::StatusCode::CANCELLED); + EXPECT_TRUE(globalMediaActivityCoordinator().waitForStopped( + media_ticket, 1s)); + const int stopped_stream_reads = hand_->sensor_read_calls.load(); + std::this_thread::sleep_for(50ms); + EXPECT_EQ(hand_->sensor_read_calls.load(), stopped_stream_reads); + EXPECT_TRUE(globalMediaActivityCoordinator().finishStopAll( + media_ticket, true)); + EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll( + admission_ticket, true)); + + grpc::ClientContext resumed_context; + resumed_context.set_deadline(std::chrono::system_clock::now() + 3s); + auto resumed = stub_->GetSensorDataStream(&resumed_context); + ASSERT_TRUE(resumed->Write(sensorStreamRequest())); + ASSERT_TRUE(resumed->Read(&feedback)); + EXPECT_TRUE(feedback.header().success()); + EXPECT_GT(hand_->sensor_read_calls.load(), stopped_stream_reads); + EXPECT_GE(hand_->resume_operational_calls.load(), 2); + resumed_context.TryCancel(); + resumed->WritesDone(); + while (resumed->Read(&feedback)) { + } + (void)resumed->Finish(); +} + +} // namespace +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_head_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_head_service_test.cpp new file mode 100644 index 00000000..762dbcc7 --- /dev/null +++ b/cmvr-es/service/grpc/tests/grpc_head_service_test.cpp @@ -0,0 +1,391 @@ +#include "service/grpc/include/grpc_head_service.h" + +#include +#include +#include +#include +#include + +#include +#include + +#include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "manager/device_manager/include/device_manager.h" +#include "service/grpc/include/media_activity_coordinator.h" +#include "service/stop_all/include/stop_all_admission_gate.h" + +namespace cmvr::service { +namespace { + +using namespace std::chrono_literals; + +class FakeBiohead final : public device::AbstractBiohead { +public: + FakeBiohead() { id_ = "test-head"; } + + std::string typeName() const override { return "FakeBiohead"; } + bool stop() override + { + ++lifecycle_stop_calls; + return true; + } + + bool setExpressionPoseIfCurrent( + const OperationalToken token, + device::FacialExpressionState&, + double, + double) override + { + return recordIfCurrent(token, set_expression_calls); + } + + bool streamFacialPoseIfCurrent( + const OperationalToken token, + device::FacialExpressionState&, + double, + double) override + { + return recordIfCurrent(token, stream_expression_calls); + } + + bool speakStartIfCurrent(const OperationalToken token) override + { + return recordIfCurrent(token, speak_start_calls); + } + + bool expressionHappyIfCurrent(const OperationalToken token) override + { + return recordIfCurrent(token, happy_calls); + } + + bool expressionSurprisedIfCurrent(const OperationalToken token) override + { + return recordIfCurrent(token, surprise_calls); + } + + bool expressionTiredIfCurrent(const OperationalToken token) override + { + return recordIfCurrent(token, tired_calls); + } + + bool expressionAngryIfCurrent(const OperationalToken token) override + { + return recordIfCurrent(token, angry_calls); + } + + bool expressionSadnessIfCurrent(const OperationalToken token) override + { + return recordIfCurrent(token, sadness_calls); + } + + bool expressionYawnIfCurrent(const OperationalToken token) override + { + return recordIfCurrent(token, yawn_calls); + } + + void speakstop() override { ++speak_stop_calls; } + + bool stopOperationalActivity() override + { + invalidateOperationalActivities_(); + ++operational_stop_calls; + ++speak_stop_calls; + return stop_confirmed.load(std::memory_order_acquire); + } + + bool recordIfCurrent( + const OperationalToken token, + std::atomic& calls) + { + return runIfOperationalActivityCurrent_(token, [&] { ++calls; }); + } + + std::atomic set_expression_calls{0}; + std::atomic stream_expression_calls{0}; + std::atomic speak_start_calls{0}; + std::atomic speak_stop_calls{0}; + std::atomic happy_calls{0}; + std::atomic surprise_calls{0}; + std::atomic tired_calls{0}; + std::atomic angry_calls{0}; + std::atomic sadness_calls{0}; + std::atomic yawn_calls{0}; + std::atomic operational_stop_calls{0}; + std::atomic lifecycle_stop_calls{0}; + std::atomic stop_confirmed{true}; +}; + +class GrpcHeadServiceTest : public ::testing::Test { +protected: + void SetUp() override + { + device::DeviceManager::destroyInstance(); + globalStopAllAdmissionGate().clearForTesting(); + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + head_ = std::make_shared(); + manager.registerDevice(head_); + service_ = std::make_unique(); + } + + void TearDown() override + { + if (server_) { + server_->Shutdown(); + server_->Wait(); + } + stub_.reset(); + service_.reset(); + head_.reset(); + device::DeviceManager::destroyInstance(); + globalStopAllAdmissionGate().clearForTesting(); + } + + bool startGrpcServer() + { + int selected_port = 0; + grpc::ServerBuilder builder; + builder.AddListeningPort( + "127.0.0.1:0", + grpc::InsecureServerCredentials(), + &selected_port); + builder.RegisterService(service_.get()); + server_ = builder.BuildAndStart(); + if (!server_ || selected_port <= 0) { + return false; + } + const std::string address = + "127.0.0.1:" + std::to_string(selected_port); + stub_ = api::BioHeadService::NewStub(grpc::CreateChannel( + address, grpc::InsecureChannelCredentials())); + return stub_ != nullptr; + } + + static api::StreamFacialExpression_Request streamRequest() + { + api::StreamFacialExpression_Request request; + request.mutable_header()->set_device_id("test-head"); + request.mutable_expr()->mutable_jaw()->set_x(0.5F); + return request; + } + + std::shared_ptr head_; + std::unique_ptr service_; + std::unique_ptr server_; + std::unique_ptr stub_; +}; + +TEST_F(GrpcHeadServiceTest, + OperationalStopInvalidatesOldTokenWithoutStoppingLifecycle) +{ + const auto old_token = head_->beginOperationalActivity(); + ASSERT_TRUE(head_->recordIfCurrent(old_token, head_->happy_calls)); + + EXPECT_TRUE(head_->stopOperationalActivity()); + EXPECT_FALSE(head_->recordIfCurrent(old_token, head_->happy_calls)); + EXPECT_EQ(head_->happy_calls.load(), 1); + EXPECT_EQ(head_->operational_stop_calls.load(), 1); + EXPECT_EQ(head_->lifecycle_stop_calls.load(), 0); + + const auto current_token = head_->beginOperationalActivity(); + EXPECT_NE(current_token, old_token); + EXPECT_TRUE(head_->recordIfCurrent(current_token, head_->happy_calls)); + EXPECT_EQ(head_->happy_calls.load(), 2); +} + +TEST_F(GrpcHeadServiceTest, + StopAllGateRejectsMotionButAllowsOperationalStopRpcs) +{ + const auto ticket = globalStopAllAdmissionGate().beginStopAll(); + ASSERT_TRUE(ticket.valid()); + + api::Happy_Request happy_request; + happy_request.mutable_header()->set_device_id("test-head"); + api::Happy_Feedback happy_response; + grpc::ServerContext happy_context; + EXPECT_TRUE(service_->Happy( + &happy_context, &happy_request, &happy_response).ok()); + EXPECT_FALSE(happy_response.header().success()); + EXPECT_EQ(head_->happy_calls.load(), 0); + + api::Surprise_Request surprise_request; + surprise_request.mutable_header()->set_device_id("test-head"); + api::Surprise_Feedback surprise_response; + grpc::ServerContext surprise_context; + EXPECT_TRUE(service_->Surprise( + &surprise_context, &surprise_request, &surprise_response).ok()); + EXPECT_FALSE(surprise_response.header().success()); + EXPECT_EQ(head_->surprise_calls.load(), 0); + + api::ExpressionTired_Request tired_request; + tired_request.mutable_header()->set_device_id("test-head"); + api::ExpressionTired_Feedback tired_response; + grpc::ServerContext tired_context; + EXPECT_TRUE(service_->ExpressionTired( + &tired_context, &tired_request, &tired_response).ok()); + EXPECT_FALSE(tired_response.header().success()); + EXPECT_EQ(head_->tired_calls.load(), 0); + + api::ExpressionAngry_Request angry_request; + angry_request.mutable_header()->set_device_id("test-head"); + api::ExpressionAngry_Feedback angry_response; + grpc::ServerContext angry_context; + EXPECT_TRUE(service_->ExpressionAngry( + &angry_context, &angry_request, &angry_response).ok()); + EXPECT_FALSE(angry_response.header().success()); + EXPECT_EQ(head_->angry_calls.load(), 0); + + api::ExpressionSadness_Request sadness_request; + sadness_request.mutable_header()->set_device_id("test-head"); + api::ExpressionSadness_Feedback sadness_response; + grpc::ServerContext sadness_context; + EXPECT_TRUE(service_->ExpressionSadness( + &sadness_context, &sadness_request, &sadness_response).ok()); + EXPECT_FALSE(sadness_response.header().success()); + EXPECT_EQ(head_->sadness_calls.load(), 0); + + api::ExpressionYawn_Request yawn_request; + yawn_request.mutable_header()->set_device_id("test-head"); + api::ExpressionYawn_Feedback yawn_response; + grpc::ServerContext yawn_context; + EXPECT_TRUE(service_->ExpressionYawn( + &yawn_context, &yawn_request, &yawn_response).ok()); + EXPECT_FALSE(yawn_response.header().success()); + EXPECT_EQ(head_->yawn_calls.load(), 0); + + api::SetFacialExpression_Request expression_request; + expression_request.mutable_header()->set_device_id("test-head"); + api::SetFacialExpression_Feedback expression_response; + grpc::ServerContext expression_context; + EXPECT_TRUE(service_->SetExpression( + &expression_context, + &expression_request, + &expression_response).ok()); + EXPECT_FALSE(expression_response.header().success()); + EXPECT_EQ(head_->set_expression_calls.load(), 0); + + api::SpeakStart_Request speak_request; + speak_request.mutable_header()->set_device_id("test-head"); + api::SpeakStart_Feedback speak_response; + grpc::ServerContext speak_context; + EXPECT_TRUE(service_->SpeakStart( + &speak_context, &speak_request, &speak_response).ok()); + EXPECT_FALSE(speak_response.header().success()); + EXPECT_EQ(head_->speak_start_calls.load(), 0); + + api::SpeakStop_Request speak_stop_request; + speak_stop_request.mutable_header()->set_device_id("test-head"); + api::SpeakStop_Feedback speak_stop_response; + grpc::ServerContext speak_stop_context; + EXPECT_TRUE(service_->SpeakStop( + &speak_stop_context, + &speak_stop_request, + &speak_stop_response).ok()); + EXPECT_TRUE(speak_stop_response.header().success()); + + api::EmergencyStop_Request stop_request; + stop_request.mutable_header()->set_device_id("test-head"); + api::EmergencyStop_Feedback stop_response; + grpc::ServerContext stop_context; + EXPECT_TRUE(service_->EmergencyStop( + &stop_context, &stop_request, &stop_response).ok()); + EXPECT_TRUE(stop_response.header().success()); + EXPECT_EQ(head_->operational_stop_calls.load(), 1); + EXPECT_EQ(head_->lifecycle_stop_calls.load(), 0); + + EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); +} + +TEST_F(GrpcHeadServiceTest, + StreamIsCancelledByStopAllAndNewStreamWorksAfterRecovery) +{ + ASSERT_TRUE(startGrpcServer()); + grpc::ClientContext context; + context.set_deadline(std::chrono::system_clock::now() + 2s); + auto stream = stub_->StreamExpression(&context); + + ASSERT_TRUE(stream->Write(streamRequest())); + api::StreamFacialExpression_Feedback feedback; + ASSERT_TRUE(stream->Read(&feedback)); + ASSERT_TRUE(feedback.header().success()); + ASSERT_EQ(head_->stream_expression_calls.load(), 1); + + const auto admission_ticket = globalStopAllAdmissionGate().beginStopAll(); + const auto media_ticket = globalMediaActivityCoordinator().beginStopAll(); + ASSERT_TRUE(admission_ticket.valid()); + ASSERT_TRUE(media_ticket.valid()); + + (void)stream->Write(streamRequest()); + stream->WritesDone(); + while (stream->Read(&feedback)) { + } + const auto status = stream->Finish(); + EXPECT_TRUE( + status.ok() || status.error_code() == grpc::StatusCode::CANCELLED); + EXPECT_TRUE(globalMediaActivityCoordinator().waitForStopped( + media_ticket, 1s)); + EXPECT_EQ(head_->stream_expression_calls.load(), 1); + EXPECT_TRUE(globalMediaActivityCoordinator().finishStopAll( + media_ticket, true)); + EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll( + admission_ticket, true)); + + grpc::ClientContext resumed_context; + resumed_context.set_deadline(std::chrono::system_clock::now() + 2s); + auto resumed = stub_->StreamExpression(&resumed_context); + ASSERT_TRUE(resumed->Write(streamRequest())); + ASSERT_TRUE(resumed->Read(&feedback)); + EXPECT_TRUE(feedback.header().success()); + EXPECT_EQ(head_->stream_expression_calls.load(), 2); + resumed_context.TryCancel(); + resumed->WritesDone(); + while (resumed->Read(&feedback)) { + } + (void)resumed->Finish(); +} + +TEST_F(GrpcHeadServiceTest, + StopAllCancelsStreamBeforeItsFirstFrameCanClearStopState) +{ + ASSERT_TRUE(startGrpcServer()); + grpc::ClientContext context; + context.set_deadline(std::chrono::system_clock::now() + 2s); + auto stream = stub_->StreamExpression(&context); + + std::this_thread::sleep_for(20ms); + const auto admission_ticket = globalStopAllAdmissionGate().beginStopAll(); + const auto media_ticket = globalMediaActivityCoordinator().beginStopAll(); + ASSERT_TRUE(admission_ticket.valid()); + ASSERT_TRUE(media_ticket.valid()); + + (void)stream->Write(streamRequest()); + stream->WritesDone(); + api::StreamFacialExpression_Feedback feedback; + while (stream->Read(&feedback)) { + } + (void)stream->Finish(); + EXPECT_TRUE(globalMediaActivityCoordinator().waitForStopped( + media_ticket, 1s)); + EXPECT_EQ(head_->stream_expression_calls.load(), 0); + EXPECT_TRUE(globalMediaActivityCoordinator().finishStopAll( + media_ticket, true)); + EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll( + admission_ticket, true)); +} + +TEST_F(GrpcHeadServiceTest, UnconfirmedOperationalStopIsReported) +{ + head_->stop_confirmed = false; + api::EmergencyStop_Request request; + request.mutable_header()->set_device_id("test-head"); + api::EmergencyStop_Feedback response; + grpc::ServerContext context; + + EXPECT_TRUE(service_->EmergencyStop(&context, &request, &response).ok()); + EXPECT_FALSE(response.header().success()); + EXPECT_EQ(head_->operational_stop_calls.load(), 1); + EXPECT_EQ(head_->lifecycle_stop_calls.load(), 0); +} + +} // 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 df4a56fe..dae08370 100644 --- a/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp @@ -18,6 +18,8 @@ #include "devices/motor/manager/include/motor_manager.h" #include "devices/motor/motor_protocol_interface.h" #include "manager/device_manager/include/device_manager.h" +#include "service/grpc/include/motor_activity_coordinator.h" +#include "service/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { @@ -222,11 +224,13 @@ private: class FakeMotor final : public device::AbstractMotor { public: - explicit FakeMotor(const std::uint8_t node_id) + explicit FakeMotor( + const std::uint8_t node_id, + std::string joint_name = "test_joint") : AbstractMotor(node_id) { info_.id = node_id; - info_.joint_name = "test_joint"; + info_.joint_name = std::move(joint_name); } std::string typeName() const override { return "FakeMotor"; } @@ -236,6 +240,8 @@ class MotorServiceTest : public ::testing::Test { protected: void SetUp() override { + globalStopAllAdmissionGate().clearForTesting(); + globalMotorActivityCoordinator().clearForTesting(); config::DeviceManagerConfig device_config; auto& device_manager = device::DeviceManager::getInstance(device_config); @@ -267,6 +273,8 @@ protected: motor_.reset(); protocol_.reset(); device::DeviceManager::destroyInstance(); + globalMotorActivityCoordinator().clearForTesting(); + globalStopAllAdmissionGate().clearForTesting(); } static api::MotorTarget makeTarget() @@ -408,6 +416,135 @@ TEST_F(MotorServiceTest, ProfilePositionReturnsOnlyAfterTargetIsReached) EXPECT_EQ(response.status().active_control(), api::MOTOR_CONTROL_NONE); } +TEST_F(MotorServiceTest, DirectControlRejectsMotorClaimedByRobotArm) +{ + std::uint64_t claim_id = 0; + std::string claim_error; + ASSERT_TRUE(manager_->claimArmJoints( + "test_arm", {"test_joint"}, claim_id, &claim_error)) + << claim_error; + + api::ProfilePositionRequest request; + *request.mutable_target() = makeTarget(); + request.set_target_position_rad(1.25); + request.set_max_velocity_rad_s(1.0); + request.set_acceleration_rad_s2(2.0); + grpc::ServerContext context; + api::MotorCommandResponse response; + + const auto status = service_->profilePosition( + &context, &request, &response); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_FALSE(response.header().success()); + EXPECT_NE(status.error_message().find("test_arm"), std::string::npos); + EXPECT_NE(status.error_message().find("ArmService"), std::string::npos); + EXPECT_FALSE(protocol_->command_started_.load()); + EXPECT_EQ(protocol_->quick_stop_count_.load(), 0); +} + +TEST_F(MotorServiceTest, ClaimedArmMotorRemainsObservableWithoutStopRegistration) +{ + std::uint64_t claim_id = 0; + ASSERT_TRUE(manager_->claimArmJoints( + "test_arm", {"test_joint"}, claim_id)); + + api::GetMotorStatusRequest request; + *request.mutable_target() = makeTarget(); + grpc::ServerContext context; + api::GetMotorStatusResponse response; + const auto status = service_->getStatus(&context, &request, &response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_TRUE(response.header().success()); + EXPECT_EQ(response.status().joint_name(), "test_joint"); + + auto& coordinator = globalMotorActivityCoordinator(); + const auto ticket = coordinator.beginStopAll(); + ASSERT_TRUE(ticket.valid()); + std::string stop_error; + EXPECT_TRUE(coordinator.requestStop(ticket, &stop_error)) << stop_error; + EXPECT_EQ(protocol_->quick_stop_count_.load(), 0); + EXPECT_TRUE(coordinator.waitForStopped( + ticket, std::chrono::milliseconds(50), &stop_error)) << stop_error; + EXPECT_TRUE(coordinator.finishStopAll(ticket, true)); +} + +TEST_F(MotorServiceTest, ReleasingArmClaimRestoresDirectMotorControl) +{ + std::uint64_t claim_id = 0; + ASSERT_TRUE(manager_->claimArmJoints( + "test_arm", {"test_joint"}, claim_id)); + manager_->releaseArmJoints(claim_id); + + api::ProfilePositionRequest request; + *request.mutable_target() = makeTarget(); + request.set_target_position_rad(0.75); + request.set_max_velocity_rad_s(1.0); + request.set_acceleration_rad_s2(2.0); + request.mutable_wait()->set_settle_sample_count(1); + request.mutable_wait()->set_poll_period_ms(1); + grpc::ServerContext context; + api::MotorCommandResponse response; + + const auto status = service_->profilePosition( + &context, &request, &response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_TRUE(response.header().success()); + EXPECT_DOUBLE_EQ(response.status().position_rad(), 0.75); +} + +TEST_F(MotorServiceTest, ArmClaimCannotBeDisplacedOrReleasedByAnotherToken) +{ + std::uint64_t first_claim = 0; + ASSERT_TRUE(manager_->claimArmJoints( + "first_arm", {"test_joint"}, first_claim)); + + std::uint64_t conflicting_claim = 0; + std::string claim_error; + EXPECT_FALSE(manager_->claimArmJoints( + "second_arm", {"test_joint"}, conflicting_claim, &claim_error)); + EXPECT_EQ(conflicting_claim, 0U); + EXPECT_NE(claim_error.find("first_arm"), std::string::npos); + + manager_->releaseArmJoints(first_claim + 1U); + EXPECT_EQ(manager_->armOwnerForJoint("test_joint"), "first_arm"); + + manager_->releaseArmJoints(first_claim); + EXPECT_TRUE(manager_->armOwnerForJoint("test_joint").empty()); +} + +TEST_F(MotorServiceTest, ArmClaimDoesNotRejectUnclaimedMotor) +{ + auto other_protocol = std::make_shared(); + auto other_motor = std::make_shared(2, "other_joint"); + other_motor->setProtocol(other_protocol); + ASSERT_TRUE(manager_->addMotor(other_motor)); + + std::uint64_t claim_id = 0; + ASSERT_TRUE(manager_->claimArmJoints( + "test_arm", {"test_joint"}, claim_id)); + + api::ProfilePositionRequest request; + *request.mutable_target() = makeTarget(); + request.mutable_target()->set_motor_id(2); + request.set_target_position_rad(0.5); + request.set_max_velocity_rad_s(1.0); + request.set_acceleration_rad_s2(2.0); + request.mutable_wait()->set_settle_sample_count(1); + request.mutable_wait()->set_poll_period_ms(1); + grpc::ServerContext context; + api::MotorCommandResponse response; + + const auto status = service_->profilePosition( + &context, &request, &response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_TRUE(response.header().success()); + EXPECT_DOUBLE_EQ(response.status().position_rad(), 0.5); +} + TEST_F(MotorServiceTest, ProfileVelocityReturnsAfterTargetSettles) { api::ProfileVelocityRequest request; @@ -1582,5 +1719,124 @@ TEST_F(MotorServiceTest, CyclicPositionStreamWatchdogStopsSilentClient) EXPECT_GE(protocol_->quick_stop_count_.load(), 1); } +TEST_F(MotorServiceTest, SystemStopAllPreemptsProfileAndResumesControl) +{ + protocol_->hold_position_ = true; + + api::ProfilePositionRequest motion_request; + *motion_request.mutable_target() = makeTarget(); + motion_request.set_target_position_rad(2.0); + motion_request.set_max_velocity_rad_s(1.0); + motion_request.set_acceleration_rad_s2(1.0); + motion_request.mutable_wait()->set_timeout_ms(5000); + motion_request.mutable_wait()->set_poll_period_ms(1); + + grpc::ServerContext motion_context; + api::MotorCommandResponse motion_response; + grpc::Status motion_status; + std::thread motion([&]() { + motion_status = service_->profilePosition( + &motion_context, &motion_request, &motion_response); + }); + + const auto command_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (!protocol_->command_started_.load() && + std::chrono::steady_clock::now() < command_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + if (!protocol_->command_started_.load()) { + motion_context.TryCancel(); + motion.join(); + FAIL() << "profile command did not start"; + return; + } + + auto& coordinator = globalMotorActivityCoordinator(); + const auto ticket = coordinator.beginStopAll(); + ASSERT_TRUE(ticket.valid()); + std::string stop_error; + EXPECT_TRUE(coordinator.stopAndWait( + ticket, std::chrono::seconds(1), &stop_error)) + << stop_error; + EXPECT_TRUE(coordinator.finishStopAll(ticket, true)); + + motion.join(); + EXPECT_EQ(motion_status.error_code(), grpc::StatusCode::ABORTED); + EXPECT_FALSE(motion_response.header().success()); + EXPECT_FALSE(motion_response.status().emergency_stopped()); + EXPECT_GE(protocol_->quick_stop_count_.load(), 1); + EXPECT_GT(protocol_->last_quick_stop_order_.load(), + protocol_->profile_command_order_.load()); + + protocol_->hold_position_ = false; + api::ProfilePositionRequest resumed_request; + *resumed_request.mutable_target() = makeTarget(); + resumed_request.set_target_position_rad(0.5); + resumed_request.set_max_velocity_rad_s(1.0); + resumed_request.set_acceleration_rad_s2(1.0); + resumed_request.mutable_wait()->set_settle_sample_count(1); + grpc::ServerContext resumed_context; + api::MotorCommandResponse resumed_response; + const auto resumed_status = service_->profilePosition( + &resumed_context, &resumed_request, &resumed_response); + ASSERT_TRUE(resumed_status.ok()) << resumed_status.error_message(); + EXPECT_TRUE(resumed_response.header().success()); + EXPECT_FALSE(resumed_response.status().emergency_stopped()); +} + +TEST_F(MotorServiceTest, FailedSystemStopAllRemainsFailClosedUntilRecovery) +{ + // Resolve the motor once so its control state is registered even though no + // motion RPC is active when StopAll starts. + api::GetMotorStatusRequest status_request; + *status_request.mutable_target() = makeTarget(); + grpc::ServerContext status_context; + api::GetMotorStatusResponse status_response; + ASSERT_TRUE(service_->getStatus( + &status_context, &status_request, &status_response).ok()); + + auto& coordinator = globalMotorActivityCoordinator(); + protocol_->quick_stop_success_ = false; + const auto failed_ticket = coordinator.beginStopAll(); + ASSERT_TRUE(failed_ticket.valid()); + std::string stop_error; + EXPECT_FALSE(coordinator.stopAndWait( + failed_ticket, std::chrono::seconds(1), &stop_error)); + EXPECT_FALSE(stop_error.empty()); + EXPECT_FALSE(coordinator.finishStopAll(failed_ticket, false)); + + api::ProfilePositionRequest blocked_request; + *blocked_request.mutable_target() = makeTarget(); + blocked_request.set_target_position_rad(1.0); + blocked_request.set_max_velocity_rad_s(1.0); + blocked_request.set_acceleration_rad_s2(1.0); + grpc::ServerContext blocked_context; + api::MotorCommandResponse blocked_response; + const auto blocked_status = service_->profilePosition( + &blocked_context, &blocked_request, &blocked_response); + EXPECT_EQ(blocked_status.error_code(), grpc::StatusCode::ABORTED); + EXPECT_NE(blocked_status.error_message().find("StopAll"), + std::string::npos); + + protocol_->quick_stop_success_ = true; + const auto recovery_ticket = coordinator.beginStopAll(); + ASSERT_TRUE(recovery_ticket.valid()); + stop_error.clear(); + ASSERT_TRUE(coordinator.stopAndWait( + recovery_ticket, std::chrono::seconds(1), &stop_error)) + << stop_error; + ASSERT_TRUE(coordinator.finishStopAll(recovery_ticket, true)); + + blocked_request.mutable_wait()->set_settle_sample_count(1); + grpc::ServerContext resumed_context; + api::MotorCommandResponse resumed_response; + const auto resumed_status = service_->profilePosition( + &resumed_context, &blocked_request, &resumed_response); + ASSERT_TRUE(resumed_status.ok()) << resumed_status.error_message(); + EXPECT_TRUE(resumed_response.header().success()); + EXPECT_FALSE(resumed_response.status().emergency_stopped()); +} + } // 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 70e991a6..2c43afca 100644 --- a/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -19,11 +20,24 @@ #include #include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "cmvr/config/task_manager_config/task_manager_config.pb.h" #include "devices/agv/abstract_agv.h" #include "devices/arm/robot_arm.h" +#include "devices/camera/abstract_camera.h" +#include "devices/microphone/abstract_microphone.h" +#include "devices/speaker/abstract_speaker.h" #include "manager/control_authority/include/control_authority_manager.h" #include "manager/device_manager/include/device_manager.h" +#include "manager/media_source_hub/include/device_media_source_adapter.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/grpc_camera_service.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 "task/task_factory.h" namespace cmvr::service { namespace { @@ -101,6 +115,419 @@ private: bool release_stop_{false}; }; +class StopAllTestCamera final : public device::AbstractCamera { +public: + StopAllTestCamera(std::string id, + const bool stop_clears_recording = true, + const bool stop_throws = false) + : stop_clears_recording_(stop_clears_recording), + stop_throws_(stop_throws) + { + id_ = std::move(id); + state_.is_initialized = true; + state_.is_recording = true; + } + + std::string typeName() const override { return "StopAllTestCamera"; } + + void getState(device::CameraState& state) override + { + std::lock_guard lock(mutex_); + state = state_; + } + + void stopRecording() override + { + { + std::lock_guard lock(mutex_); + ++stop_recording_calls_; + if (stop_throws_) { + stop_recording_condition_.notify_all(); + throw std::runtime_error( + "injected camera recording stop failure"); + } + if (stop_clears_recording_) { + state_.is_recording = false; + } + } + stop_recording_condition_.notify_all(); + } + + bool startOperationalActivity() override + { + std::lock_guard lock(mutex_); + operational_active_ = true; + return true; + } + + bool stopOperationalActivity() override + { + std::lock_guard lock(mutex_); + ++operational_stop_calls_; + if (state_.is_recording) { + return false; + } + operational_active_ = false; + return true; + } + + bool controlPtz(device::PtzCommand, + const bool stop, + int) override + { + std::lock_guard lock(mutex_); + ++ptz_calls_; + ptz_active_ = !stop; + return true; + } + + bool stop() override + { + std::lock_guard lock(mutex_); + ++lifecycle_stop_calls_; + return true; + } + + bool isRecording() const + { + std::lock_guard lock(mutex_); + return state_.is_recording; + } + + void setRecording(const bool recording) + { + std::lock_guard lock(mutex_); + state_.is_recording = recording; + } + + void startRecording(const std::string&) override + { + { + std::unique_lock lock(start_recording_control_mutex_); + if (block_next_start_recording_) { + block_next_start_recording_ = false; + start_recording_entered_ = true; + start_recording_entered_condition_.notify_all(); + start_recording_release_condition_.wait( + lock, [this] { return release_start_recording_; }); + } + } + { + std::lock_guard lock(mutex_); + state_.is_recording = true; + ++start_recording_calls_; + } + start_recording_condition_.notify_all(); + } + + void blockNextStartRecording() + { + std::lock_guard lock(start_recording_control_mutex_); + block_next_start_recording_ = true; + start_recording_entered_ = false; + release_start_recording_ = false; + } + + bool waitForStartRecordingEntered( + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(start_recording_control_mutex_); + return start_recording_entered_condition_.wait_for( + lock, timeout, [this] { return start_recording_entered_; }); + } + + void releaseBlockedStartRecording() + { + { + std::lock_guard lock(start_recording_control_mutex_); + release_start_recording_ = true; + } + start_recording_release_condition_.notify_all(); + } + + bool waitForStartRecordingCalls( + const int expected, + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return start_recording_condition_.wait_for( + lock, + timeout, + [this, expected] { + return start_recording_calls_ >= expected; + }); + } + + int stopRecordingCalls() const + { + std::lock_guard lock(mutex_); + return stop_recording_calls_; + } + + bool waitForStopRecordingCalls( + const int expected, + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return stop_recording_condition_.wait_for( + lock, + timeout, + [this, expected] { + return stop_recording_calls_ >= expected; + }); + } + + int lifecycleStopCalls() const + { + std::lock_guard lock(mutex_); + return lifecycle_stop_calls_; + } + + int operationalStopCalls() const + { + std::lock_guard lock(mutex_); + return operational_stop_calls_; + } + + bool operationalActive() const + { + std::lock_guard lock(mutex_); + return operational_active_; + } + + bool ptzActive() const + { + std::lock_guard lock(mutex_); + return ptz_active_; + } + + int ptzCalls() const + { + std::lock_guard lock(mutex_); + return ptz_calls_; + } + +private: + mutable std::mutex mutex_; + std::condition_variable stop_recording_condition_; + std::condition_variable start_recording_condition_; + mutable std::mutex start_recording_control_mutex_; + std::condition_variable start_recording_entered_condition_; + std::condition_variable start_recording_release_condition_; + int start_recording_calls_{0}; + int stop_recording_calls_{0}; + int lifecycle_stop_calls_{0}; + int operational_stop_calls_{0}; + int ptz_calls_{0}; + bool operational_active_{false}; + bool ptz_active_{false}; + bool block_next_start_recording_{false}; + bool start_recording_entered_{false}; + bool release_start_recording_{false}; + bool stop_clears_recording_{true}; + bool stop_throws_{false}; +}; + +class StopAllTestMicrophone final : public device::AbstractMicrophone { +public: + StopAllTestMicrophone(std::string id, + const bool stop_clears_recording = true, + const bool stop_throws = false) + : stop_clears_recording_(stop_clears_recording), + stop_throws_(stop_throws) + { + id_ = std::move(id); + state_.is_initialized = true; + state_.is_recording = true; + } + + std::string typeName() const override + { + return "StopAllTestMicrophone"; + } + + void getState(device::MicrophoneState& state) override + { + std::lock_guard lock(mutex_); + state = state_; + } + + void stopRecording() override + { + std::lock_guard lock(mutex_); + ++stop_recording_calls_; + if (stop_throws_) { + throw std::runtime_error( + "injected microphone recording stop failure"); + } + if (stop_clears_recording_) { + state_.is_recording = false; + } + } + + bool stop() override + { + std::lock_guard lock(mutex_); + ++lifecycle_stop_calls_; + return true; + } + + bool isRecording() const + { + std::lock_guard lock(mutex_); + return state_.is_recording; + } + + int stopRecordingCalls() const + { + std::lock_guard lock(mutex_); + return stop_recording_calls_; + } + + int lifecycleStopCalls() const + { + std::lock_guard lock(mutex_); + return lifecycle_stop_calls_; + } + +private: + mutable std::mutex mutex_; + int stop_recording_calls_{0}; + int lifecycle_stop_calls_{0}; + bool stop_clears_recording_{true}; + bool stop_throws_{false}; +}; + +class StopAllTestSpeaker final : public device::AbstractSpeaker { +public: + explicit StopAllTestSpeaker(std::string id, + const bool playback_stop_result = true) + : playback_stop_result_(playback_stop_result) + { + id_ = std::move(id); + state_.is_initialized = true; + state_.is_running = true; + state_.is_decoding = true; + } + + std::string typeName() const override { return "StopAllTestSpeaker"; } + + void getState(device::SpeakerState& state) override + { + std::lock_guard lock(mutex_); + state = state_; + } + + bool stopPlayback() override + { + std::lock_guard lock(mutex_); + ++stop_playback_calls_; + if (playback_stop_result_) { + state_.is_running = false; + state_.is_decoding = false; + } + return playback_stop_result_; + } + + bool stop() override + { + std::lock_guard lock(mutex_); + ++lifecycle_stop_calls_; + return true; + } + + int stopPlaybackCalls() const + { + std::lock_guard lock(mutex_); + return stop_playback_calls_; + } + + int lifecycleStopCalls() const + { + std::lock_guard lock(mutex_); + return lifecycle_stop_calls_; + } + +private: + mutable std::mutex mutex_; + int stop_playback_calls_{0}; + int lifecycle_stop_calls_{0}; + bool playback_stop_result_{true}; +}; + +class BlockingStopTask final : public task::Task { +public: + explicit BlockingStopTask(std::string id) + : id_(std::move(id)) + { + } + + const std::string& id() const override { return id_; } + task::TaskRunMode runMode() const override + { + return task::TaskRunMode::BLOCKING_SERVICE; + } + bool init() override { return true; } + bool step(double) override { return true; } + void stop() override { lifecycle_stop_calls_.fetch_add(1); } + bool stopActivity() override + { + std::unique_lock lock(mutex_); + ++stop_activity_calls_; + stop_entered_ = true; + condition_.notify_all(); + condition_.wait(lock, [this] { return release_stop_; }); + return true; + } + task::TaskState state() const override + { + return task::TaskState::IDLE; + } + bool isBusy() const override { return false; } + bool isFinished() const override { return false; } + bool isFailed() const override { return false; } + std::string stateString() const override { return "IDLE"; } + std::string detailStatusString() const override { return "IDLE"; } + + bool waitForStopActivity(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return condition_.wait_for( + lock, timeout, [this] { return stop_entered_; }); + } + + void releaseStopActivity() + { + { + std::lock_guard lock(mutex_); + release_stop_ = true; + } + condition_.notify_all(); + } + + int stopActivityCalls() const + { + std::lock_guard lock(mutex_); + return stop_activity_calls_; + } + + int lifecycleStopCalls() const + { + return lifecycle_stop_calls_.load(); + } + +private: + std::string id_; + mutable std::mutex mutex_; + std::condition_variable condition_; + std::atomic lifecycle_stop_calls_{0}; + int stop_activity_calls_{0}; + bool stop_entered_{false}; + bool release_stop_{false}; +}; + +std::shared_ptr blocking_stop_task; + class ActionTrace final { public: using TimePoint = std::chrono::steady_clock::time_point; @@ -152,6 +579,21 @@ public: } std::string typeName() const override { return "ActionTestArm"; } + bool stop() override + { + lifecycle_stop_calls_.fetch_add(1, std::memory_order_relaxed); + return true; + } + device::DeviceHealthSnapshot healthSnapshot() override + { + std::unique_lock lock(health_mutex_); + ++health_snapshot_calls_; + health_snapshot_entered_ = true; + health_condition_.notify_all(); + health_condition_.wait( + lock, [this] { return !block_health_snapshot_; }); + return {device::DeviceHealthState::Healthy, {}}; + } bool supportsActionQueueMotion() const noexcept override { return true; } device::RobotModel getRobotModel() const override { @@ -240,14 +682,29 @@ public: } device::Result stopMotion() override { + bool fail = false; { - std::lock_guard lock(mutex_); + std::unique_lock lock(mutex_); ++stop_motion_calls_; stop_requested_ = true; + fail = stop_motion_fails_; + motion_condition_.notify_all(); + if (block_next_stop_motion_) { + block_next_stop_motion_ = false; + stop_motion_blocked_ = true; + stop_motion_blocked_condition_.notify_all(); + stop_motion_release_condition_.wait( + lock, + [this]() { return release_blocked_stop_motion_; }); + } } trace_->add("arm:stop:" + id_); motion_condition_.notify_all(); - return device::Result::success(); + return fail + ? device::Result::failure( + device::ArmErrorCode::CommandFailed, + "injected RobotArm stop failure") + : device::Result::success(); } device::Result startServoMode(const device::ServoOptions&) override @@ -289,7 +746,11 @@ public: device::Result powerOn() override { return device::Result::success(); } device::Result powerOff() override { return device::Result::success(); } device::Result brakeRelease() override { return device::Result::success(); } - device::Result shutdown() override { return device::Result::success(); } + device::Result shutdown() override + { + shutdown_calls_.fetch_add(1, std::memory_order_relaxed); + return device::Result::success(); + } device::Result clearFault() override { return device::Result::success(); } device::Result unlockProtectiveStop() override { @@ -325,6 +786,10 @@ public: bool busy() const override { std::lock_guard lock(mutex_); + if (busy_throws_after_stop_ && stop_motion_calls_ != 0) { + throw std::runtime_error( + "injected RobotArm busy-state failure"); + } return active_motions_ != 0; } @@ -378,6 +843,38 @@ public: fail_on_motion_call_ = call_index; } + void failStopAndBusyConfirmation() + { + std::lock_guard lock(mutex_); + stop_motion_fails_ = true; + busy_throws_after_stop_ = true; + } + + void blockNextStopMotion() + { + std::lock_guard lock(mutex_); + block_next_stop_motion_ = true; + stop_motion_blocked_ = false; + release_blocked_stop_motion_ = false; + } + + bool waitForBlockedStopMotion( + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return stop_motion_blocked_condition_.wait_for( + lock, timeout, [this]() { return stop_motion_blocked_; }); + } + + void releaseBlockedStopMotion() + { + { + std::lock_guard lock(mutex_); + release_blocked_stop_motion_ = true; + } + stop_motion_release_condition_.notify_all(); + } + bool waitForMotionCalls( const int expected, const std::chrono::milliseconds timeout) @@ -400,6 +897,56 @@ public: return stop_motion_calls_; } + int lifecycleStopCalls() const + { + return lifecycle_stop_calls_.load(std::memory_order_relaxed); + } + + int shutdownCalls() const + { + return shutdown_calls_.load(std::memory_order_relaxed); + } + + void blockHealthSnapshot() + { + std::lock_guard lock(health_mutex_); + block_health_snapshot_ = true; + health_snapshot_entered_ = false; + } + + bool waitForHealthSnapshot( + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(health_mutex_); + return health_condition_.wait_for( + lock, timeout, [this] { return health_snapshot_entered_; }); + } + + void releaseHealthSnapshot() + { + { + std::lock_guard lock(health_mutex_); + block_health_snapshot_ = false; + } + health_condition_.notify_all(); + } + + int healthSnapshotCalls() const + { + std::lock_guard lock(health_mutex_); + return health_snapshot_calls_; + } + + bool waitForStopMotionCalls( + const int expected, + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return motion_condition_.wait_for( + lock, timeout, + [this, expected]() { return stop_motion_calls_ >= expected; }); + } + int maxActiveMotions() const { std::lock_guard lock(mutex_); @@ -482,6 +1029,12 @@ private: std::condition_variable motion_condition_; std::condition_variable motion_started_condition_; std::condition_variable canceled_motion_return_condition_; + std::condition_variable stop_motion_blocked_condition_; + std::condition_variable stop_motion_release_condition_; + mutable std::mutex health_mutex_; + std::condition_variable health_condition_; + std::atomic lifecycle_stop_calls_{0}; + std::atomic shutdown_calls_{0}; int motion_calls_{0}; int stop_motion_calls_{0}; int active_motions_{0}; @@ -493,6 +1046,14 @@ private: bool block_canceled_motion_return_{false}; bool canceled_motion_return_blocked_{false}; bool release_canceled_motion_return_{false}; + bool stop_motion_fails_{false}; + bool busy_throws_after_stop_{false}; + bool block_next_stop_motion_{false}; + bool stop_motion_blocked_{false}; + bool release_blocked_stop_motion_{false}; + int health_snapshot_calls_{0}; + bool block_health_snapshot_{false}; + bool health_snapshot_entered_{false}; }; class ActionTestAgv final : public device::AbstractAGV { @@ -731,19 +1292,36 @@ class GrpcSystemServiceTest : public ::testing::Test { protected: void SetUp() override { + task::TaskManager::destroyInstance(); + blocking_stop_task.reset(); + (void)media::globalMediaSourceHub().stopAllSources(); control::ControlAuthorityManager::instance().clear(); + globalStopAllAdmissionGate().clearForTesting(); + globalCameraOperationalActivityRegistry().clearForTesting(); + globalCameraPtzActivityRegistry().clearForTesting(); + globalMotorActivityCoordinator().clearForTesting(); device::DeviceManager::destroyInstance(); } void TearDown() override { + if (blocking_stop_task) { + blocking_stop_task->releaseStopActivity(); + } service_.reset(); + task::TaskManager::destroyInstance(); + blocking_stop_task.reset(); action_agv_.reset(); action_arm_.reset(); action_trace_.reset(); owned_devices_.clear(); device::DeviceManager::destroyInstance(); control::ControlAuthorityManager::instance().clear(); + globalStopAllAdmissionGate().clearForTesting(); + globalCameraOperationalActivityRegistry().clearForTesting(); + globalCameraPtzActivityRegistry().clearForTesting(); + globalMotorActivityCoordinator().clearForTesting(); + (void)media::globalMediaSourceHub().stopAllSources(); } api::GetDeviceListCommand_Feedback getDeviceList() @@ -1176,12 +1754,29 @@ TEST_F(GrpcSystemServiceTest, EXPECT_NE( response.header().error_message().find("remain quarantined"), std::string::npos); - const auto lease = - control::ControlAuthorityManager::instance().tryAcquire( + auto& authority = control::ControlAuthorityManager::instance(); + const auto lease = authority.tryAcquire( action_agv_->id(), "normal-control-after-unconfirmed-action-stop", std::chrono::hours(1)); EXPECT_FALSE(lease.acquired); + + const auto recovery = authority.preemptAcquire( + action_agv_->id(), + "confirmed-agv-recovery", + std::chrono::hours(1)); + ASSERT_TRUE(recovery.acquired) << recovery.detail; + ASSERT_TRUE(authority.waitForPreemptedRelease( + recovery.token, std::chrono::milliseconds::zero())); + ASSERT_TRUE(authority.recoverRetiredSafetyHolders(recovery.token)); + authority.release(recovery.token); + + const auto recovered_lease = authority.tryAcquire( + action_agv_->id(), + "normal-control-after-confirmed-recovery", + std::chrono::hours(1)); + EXPECT_TRUE(recovered_lease.acquired) << recovered_lease.detail; + authority.release(recovered_lease.token); } TEST_F(GrpcSystemServiceTest, @@ -1596,6 +2191,45 @@ TEST_F(GrpcSystemServiceTest, EXPECT_GT(action_arm_->stopMotionCalls(), stops_after_total); } +TEST_F(GrpcSystemServiceTest, + ActionQueueRetiresUnconfirmedTimedOutArmStopForRecovery) +{ + initializeActionDevices(); + action_arm_->blockNextMotion(); + action_arm_->failStopAndBusyConfirmation(); + + api::ActionQueueCommand_Request request; + request.set_action_id("unconfirmed-timeout-stop"); + request.set_total_timeout_ms(40U); + addMoveLStep(request, "times-out", action_arm_->id(), 1.0); + + const auto response = executeAction(request); + + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_TIMED_OUT); + EXPECT_NE( + response.header().error_message().find("remains quarantined"), + std::string::npos); + auto& authority = control::ControlAuthorityManager::instance(); + EXPECT_FALSE(authority.tryAcquire( + action_arm_->id(), "move-before-recovery", std::chrono::hours(1)) + .acquired); + + const auto recovery = authority.preemptAcquire( + action_arm_->id(), + "confirmed-arm-recovery", + std::chrono::hours(1)); + ASSERT_TRUE(recovery.acquired) << recovery.detail; + ASSERT_TRUE(authority.waitForPreemptedRelease( + recovery.token, std::chrono::milliseconds::zero())); + ASSERT_TRUE(authority.recoverRetiredSafetyHolders(recovery.token)); + authority.release(recovery.token); + + const auto recovered_lease = authority.tryAcquire( + action_arm_->id(), "move-after-recovery", std::chrono::hours(1)); + EXPECT_TRUE(recovered_lease.acquired) << recovered_lease.detail; + authority.release(recovered_lease.token); +} + TEST_F(GrpcSystemServiceTest, DelayedActionCancellationCannotStopSuccessorControlLease) { @@ -1630,17 +2264,10 @@ TEST_F(GrpcSystemServiceTest, const bool old_driver_ready_to_return = action_arm_->waitForCanceledMotionReturn( std::chrono::milliseconds(500)); - if (direct_stop.acquired) { - authority.release(direct_stop.token); - } - - control::ControlAcquireResult successor; - if (old_driver_ready_to_return) { - successor = authority.tryAcquire( - action_arm_->id(), - "successor-move", - std::chrono::seconds(30)); - } + const auto successor_before_handler_exit = authority.tryAcquire( + action_arm_->id(), + "successor-before-old-handler-exit", + std::chrono::seconds(30)); action_arm_->releaseCanceledMotionReturn(); const auto action_status = action.wait_for(std::chrono::seconds(1)); @@ -1649,6 +2276,16 @@ TEST_F(GrpcSystemServiceTest, } ASSERT_EQ(action_status, std::future_status::ready); const auto response = action.get(); + const bool old_handler_released = direct_stop.acquired && + authority.waitForPreemptedRelease( + direct_stop.token, std::chrono::seconds(1)); + if (direct_stop.acquired) { + authority.release(direct_stop.token); + } + const auto successor = authority.tryAcquire( + action_arm_->id(), + "successor-move", + std::chrono::seconds(30)); const bool successor_still_current = successor.acquired && authority.validate(successor.token); if (successor.acquired) { @@ -1658,6 +2295,8 @@ TEST_F(GrpcSystemServiceTest, ASSERT_TRUE(motion_started); ASSERT_TRUE(direct_stop.acquired) << direct_stop.detail; ASSERT_TRUE(old_driver_ready_to_return); + EXPECT_FALSE(successor_before_handler_exit.acquired); + EXPECT_TRUE(old_handler_released); ASSERT_TRUE(successor.acquired) << successor.detail; EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_CANCELED); EXPECT_TRUE(successor_still_current); @@ -1688,6 +2327,12 @@ TEST_F(GrpcSystemServiceTest, &stop_context, &stop_request, &stop_response); const auto action_response = action.get(); + api::ActionQueueCommand_Request resumed_request; + resumed_request.set_action_id("action-after-stop-all"); + addMoveLStep( + resumed_request, "resumed", action_arm_->id(), 3.0); + const auto resumed_response = executeAction(resumed_request); + EXPECT_TRUE(started); ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); EXPECT_TRUE(stop_response.header().success()) @@ -1696,10 +2341,345 @@ TEST_F(GrpcSystemServiceTest, EXPECT_EQ(action_response.completed_steps(), 0U); ASSERT_TRUE(action_response.has_failed_step_index()); EXPECT_EQ(action_response.failed_step_index(), 0U); - EXPECT_EQ(action_arm_->motionCalls(), 1); + EXPECT_TRUE(resumed_response.header().success()) + << resumed_response.header().error_message(); + EXPECT_EQ( + resumed_response.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(resumed_response.completed_steps(), 1U); + EXPECT_EQ( + resumed_response.service_instance_id(), service_instance_id_); + EXPECT_EQ(action_arm_->motionCalls(), 2); EXPECT_GE(action_arm_->stopMotionCalls(), 1); } +TEST_F(GrpcSystemServiceTest, + StopAllRequestsMotionStopBeforeWaitingForMediaHandlers) +{ + initializeActionDevices(); + auto media_session = globalMediaActivityCoordinator().beginSession(); + ASSERT_TRUE(media_session); + + api::StopAllCommand_Request request; + auto stop_all = std::async(std::launch::async, [this, &request] { + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll(&context, &request, &response); + return std::make_pair(status, response); + }); + + const bool motion_stop_requested = action_arm_->waitForStopMotionCalls( + 1, std::chrono::milliseconds(500)); + const auto status_while_media_active = + stop_all.wait_for(std::chrono::milliseconds(20)); + media_session.reset(); + + ASSERT_EQ( + stop_all.wait_for(std::chrono::seconds(1)), + std::future_status::ready); + const auto [status, response] = stop_all.get(); + + EXPECT_TRUE(motion_stop_requested); + EXPECT_EQ( + status_while_media_active, + std::future_status::timeout); + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_TRUE(response.header().success()) + << response.header().error_message(); +} + +TEST_F(GrpcSystemServiceTest, + StopAllDoesNotReportSuccessWhenAnotherAdmissionParticipantFails) +{ + initializeActionDevices(); + auto media_session = globalMediaActivityCoordinator().beginSession(); + ASSERT_TRUE(media_session); + + api::StopAllCommand_Request request; + auto stop_all = std::async(std::launch::async, [this, &request] { + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll(&context, &request, &response); + return std::make_pair(status, response); + }); + + ASSERT_TRUE(action_arm_->waitForStopMotionCalls( + 1, std::chrono::milliseconds(500))); + auto& admission = globalStopAllAdmissionGate(); + const auto failed_participant = admission.beginStopAll(); + ASSERT_TRUE(failed_participant.valid()); + EXPECT_FALSE(admission.finishStopAll(failed_participant, false)); + media_session.reset(); + + ASSERT_EQ( + stop_all.wait_for(std::chrono::seconds(1)), + std::future_status::ready); + const auto [status, response] = stop_all.get(); + + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_FALSE(response.header().success()); + EXPECT_NE( + response.header().error_message().find( + "could not safely resume system admission"), + std::string::npos); + EXPECT_FALSE(admission.lockAdmission().accepting()); +} + +TEST_F(GrpcSystemServiceTest, + StopAllDoesNotWaitForADeviceHealthSnapshot) +{ + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + 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(); }); + const bool health_call_blocked = arm->waitForHealthSnapshot( + std::chrono::milliseconds(500)); + + api::StopAllCommand_Request request; + auto stop_all = std::async(std::launch::async, [this, &request] { + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll( + &context, &request, &response); + return std::make_pair(status, response); + }); + + const bool stop_dispatched_while_health_blocked = + arm->waitForStopMotionCalls( + 1, std::chrono::milliseconds(500)); + arm->releaseHealthSnapshot(); + + ASSERT_EQ( + health_snapshot.wait_for(std::chrono::seconds(1)), + std::future_status::ready); + (void)health_snapshot.get(); + ASSERT_EQ( + stop_all.wait_for(std::chrono::seconds(1)), + std::future_status::ready); + const auto [status, response] = stop_all.get(); + + EXPECT_TRUE(health_call_blocked); + EXPECT_TRUE(stop_dispatched_while_health_blocked); + EXPECT_EQ(arm->healthSnapshotCalls(), 1); + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_TRUE(response.header().success()) + << response.header().error_message(); +} + +TEST_F(GrpcSystemServiceTest, + StopAllDispatchesDeviceStopsWhileTaskActivityStopIsBlocked) +{ + task::TaskFactory::registerCreator( + config::TaskConfigEntry::TASK_TYPE_UME_TELEOP, + [](const config::TaskConfigEntry& entry) { + blocking_stop_task = + std::make_shared(entry.id()); + return blocking_stop_task; + }); + config::TaskManagerConfig task_config; + auto* task_entry = task_config.add_tasks(); + task_entry->set_id("blocking-stop-task"); + task_entry->set_type( + config::TaskConfigEntry::TASK_TYPE_UME_TELEOP); + task_entry->set_enable(true); + task_entry->set_run_mode( + config::TaskConfigEntry::TASK_RUN_MODE_BLOCKING_SERVICE); + auto& task_manager = task::TaskManager::getInstance(task_config); + ASSERT_TRUE(task_manager.initialized()); + ASSERT_NE(blocking_stop_task, nullptr); + + initializeActionDevices(); + auto camera = std::make_shared( + "camera-while-task-stop-blocked"); + device::DeviceManager::getInstance().registerDevice(camera); + api::StopAllCommand_Request request; + auto stop_all = std::async(std::launch::async, [this, &request] { + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll( + &context, &request, &response); + return std::make_pair(status, response); + }); + + const bool task_stop_entered = + blocking_stop_task->waitForStopActivity( + std::chrono::milliseconds(500)); + const bool arm_stop_dispatched = action_arm_->waitForStopMotionCalls( + 1, std::chrono::milliseconds(500)); + const bool camera_stop_dispatched = camera->waitForStopRecordingCalls( + 1, std::chrono::milliseconds(500)); + const auto status_while_task_blocked = + stop_all.wait_for(std::chrono::milliseconds::zero()); + blocking_stop_task->releaseStopActivity(); + + ASSERT_EQ( + stop_all.wait_for(std::chrono::seconds(1)), + std::future_status::ready); + const auto [status, response] = stop_all.get(); + + EXPECT_TRUE(task_stop_entered); + EXPECT_TRUE(arm_stop_dispatched); + EXPECT_TRUE(camera_stop_dispatched); + EXPECT_EQ(status_while_task_blocked, std::future_status::timeout); + EXPECT_EQ(blocking_stop_task->stopActivityCalls(), 1); + EXPECT_EQ(blocking_stop_task->lifecycleStopCalls(), 0); + EXPECT_EQ(camera->lifecycleStopCalls(), 0); + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_TRUE(response.header().success()) + << response.header().error_message(); +} + +TEST_F(GrpcSystemServiceTest, + StopAllDispatchesPeerArmStopWhileAnotherDriverIsBlocked) +{ + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + auto trace = std::make_shared(); + auto blocking_arm = std::make_shared( + "a-blocking-arm", trace); + auto peer_arm = std::make_shared( + "b-peer-arm", trace); + auto peer_camera = std::make_shared( + "c-peer-camera"); + manager.registerDevice(blocking_arm); + manager.registerDevice(peer_arm); + manager.registerDevice(peer_camera); + blocking_arm->blockNextStopMotion(); + service_ = std::make_unique(); + + api::StopAllCommand_Request request; + auto stop_all = std::async(std::launch::async, [this, &request] { + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll( + &context, &request, &response); + return std::make_pair(status, response); + }); + + const bool blocking_stop_entered = + blocking_arm->waitForBlockedStopMotion( + std::chrono::milliseconds(500)); + const bool peer_stop_dispatched = peer_arm->waitForStopMotionCalls( + 1, std::chrono::milliseconds(500)); + const bool peer_camera_stop_dispatched = + peer_camera->waitForStopRecordingCalls( + 1, std::chrono::milliseconds(500)); + const auto status_before_release = + stop_all.wait_for(std::chrono::milliseconds(0)); + blocking_arm->releaseBlockedStopMotion(); + const auto completion_status = + stop_all.wait_for(std::chrono::seconds(1)); + const auto [status, response] = stop_all.get(); + + EXPECT_TRUE(blocking_stop_entered); + EXPECT_TRUE(peer_stop_dispatched); + EXPECT_TRUE(peer_camera_stop_dispatched); + EXPECT_EQ(status_before_release, std::future_status::timeout); + EXPECT_EQ(completion_status, std::future_status::ready); + EXPECT_TRUE(status.ok()) << status.error_message(); + EXPECT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_GE(blocking_arm->stopMotionCalls(), 2); + EXPECT_GE(peer_arm->stopMotionCalls(), 2); + EXPECT_EQ(peer_camera->lifecycleStopCalls(), 0); +} + +TEST_F(GrpcSystemServiceTest, + StopAllRoundsAreSerializedAcrossSystemServiceInstances) +{ + initializeActionDevices(); + auto second_service = std::make_unique(); + action_arm_->blockNextStopMotion(); + + api::StopAllCommand_Request request; + auto first_stop = std::async(std::launch::async, [this, &request] { + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll(&context, &request, &response); + return std::make_pair(status, response); + }); + + const bool first_stop_entered = action_arm_->waitForBlockedStopMotion( + std::chrono::milliseconds(500)); + auto second_stop = std::async( + std::launch::async, + [&second_service, &request] { + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = second_service->StopAll( + &context, &request, &response); + return std::make_pair(status, response); + }); + + const auto second_before_release = + second_stop.wait_for(std::chrono::milliseconds(50)); + const int stop_calls_before_release = action_arm_->stopMotionCalls(); + action_arm_->releaseBlockedStopMotion(); + + EXPECT_TRUE(first_stop_entered); + EXPECT_EQ(second_before_release, std::future_status::timeout); + EXPECT_EQ(stop_calls_before_release, 1); + + const auto [first_status, first_response] = first_stop.get(); + const auto [second_status, second_response] = second_stop.get(); + EXPECT_TRUE(first_status.ok()) << first_status.error_message(); + EXPECT_TRUE(first_response.header().success()) + << first_response.header().error_message(); + 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); +} + +TEST_F(GrpcSystemServiceTest, + PrepareForShutdownDoesNotWaitForBlockedBusinessStopAll) +{ + initializeActionDevices(); + auto media_session = globalMediaActivityCoordinator().beginSession(); + ASSERT_TRUE(media_session); + + api::StopAllCommand_Request request; + auto stop_all = std::async(std::launch::async, [this, &request] { + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll(&context, &request, &response); + return std::make_pair(status, response); + }); + + ASSERT_TRUE(action_arm_->waitForStopMotionCalls( + 1, std::chrono::milliseconds(500))); + ASSERT_EQ( + stop_all.wait_for(std::chrono::milliseconds(20)), + std::future_status::timeout); + + auto shutdown = std::async(std::launch::async, [this] { + service_->prepareForShutdown(); + }); + EXPECT_EQ( + shutdown.wait_for(std::chrono::milliseconds(500)), + std::future_status::ready); + shutdown.get(); + + media_session.reset(); + ASSERT_EQ( + stop_all.wait_for(std::chrono::seconds(1)), + std::future_status::ready); + const auto [status, response] = stop_all.get(); + + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_FALSE(response.header().success()); + EXPECT_NE( + response.header().error_message().find( + "could not safely resume ActionQueue admission"), + std::string::npos); +} + TEST_F(GrpcSystemServiceTest, StopAllFailsClosedWhenAgvStoppedStateIsUnconfirmed) { @@ -1725,6 +2705,258 @@ TEST_F(GrpcSystemServiceTest, EXPECT_FALSE(lease.acquired); } +TEST_F(GrpcSystemServiceTest, + StopAllStopsActiveMediaWithoutStoppingDeviceLifecycles) +{ + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + auto camera = std::make_shared("recording-camera"); + auto microphone = + std::make_shared("recording-microphone"); + auto speaker = std::make_shared("playing-speaker"); + manager.registerDevice(camera); + manager.registerDevice(microphone); + manager.registerDevice(speaker); + ASSERT_EQ( + globalCameraOperationalActivityRegistry().start( + camera->id(), camera), + CameraOperationalActivityRegistry::DispatchResult::Success); + service_ = std::make_unique(); + + api::StopAllCommand_Request request; + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll( + &context, &request, &response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ(camera->stopRecordingCalls(), 1); + EXPECT_FALSE(camera->isRecording()); + EXPECT_EQ(microphone->stopRecordingCalls(), 1); + EXPECT_FALSE(microphone->isRecording()); + EXPECT_EQ(speaker->stopPlaybackCalls(), 2); + EXPECT_EQ(camera->lifecycleStopCalls(), 0); + EXPECT_EQ(camera->operationalStopCalls(), 1); + EXPECT_FALSE(camera->operationalActive()); + EXPECT_EQ(microphone->lifecycleStopCalls(), 0); + EXPECT_EQ(speaker->lifecycleStopCalls(), 0); +} + +TEST_F(GrpcSystemServiceTest, + StopAllStopsUntrackedInventoryCameraOperationalActivity) +{ + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + auto camera = std::make_shared( + "untracked-inventory-camera"); + camera->setRecording(false); + ASSERT_TRUE(camera->startOperationalActivity()); + manager.registerDevice(camera); + service_ = std::make_unique(); + + api::StopAllCommand_Request request; + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll( + &context, &request, &response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ(camera->operationalStopCalls(), 1); + EXPECT_FALSE(camera->operationalActive()); + EXPECT_EQ(camera->lifecycleStopCalls(), 0); +} + +TEST_F(GrpcSystemServiceTest, + StopAllRepeatsOperationalStopAfterAnOldMediaStartFinishes) +{ + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + auto camera = std::make_shared("raced-recording-camera"); + camera->blockNextStartRecording(); + manager.registerDevice(camera); + service_ = std::make_unique(); + + gRPCCameraServiceImpl camera_service; + api::StartCameraRecordingCommand_Request start_request; + start_request.mutable_header()->set_device_id(camera->id()); + start_request.set_video_path("raced.mp4"); + auto old_start = std::async( + std::launch::async, + [&] { + api::StartCameraRecordingCommand_Feedback response; + grpc::ServerContext context; + const auto status = camera_service.StartRecording( + &context, &start_request, &response); + return std::make_pair(status, response); + }); + ASSERT_TRUE(camera->waitForStartRecordingEntered( + std::chrono::milliseconds(500))); + + api::StopAllCommand_Request request; + auto stop_all = std::async(std::launch::async, [this, &request] { + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll(&context, &request, &response); + return std::make_pair(status, response); + }); + + ASSERT_TRUE(camera->waitForStopRecordingCalls( + 1, std::chrono::milliseconds(500))); + EXPECT_EQ( + stop_all.wait_for(std::chrono::milliseconds(20)), + std::future_status::timeout); + camera->releaseBlockedStartRecording(); + + ASSERT_EQ( + old_start.wait_for(std::chrono::seconds(1)), + std::future_status::ready); + const auto [start_status, start_response] = old_start.get(); + ASSERT_TRUE(start_status.ok()) << start_status.error_message(); + EXPECT_TRUE(start_response.header().success()) + << start_response.header().error_message(); + ASSERT_TRUE(camera->waitForStartRecordingCalls( + 1, std::chrono::milliseconds(500))); + ASSERT_EQ( + stop_all.wait_for(std::chrono::seconds(1)), + std::future_status::ready); + const auto [status, response] = stop_all.get(); + + ASSERT_TRUE(status.ok()) << status.error_message(); + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ(camera->stopRecordingCalls(), 2); + EXPECT_FALSE(camera->isRecording()); + EXPECT_EQ(camera->lifecycleStopCalls(), 0); +} + +TEST_F(GrpcSystemServiceTest, + StopAllStopsMediaActivitiesAbsentFromDeviceManager) +{ + config::DeviceManagerConfig config; + device::DeviceManager::getInstance(config); + + constexpr const char* hub_source_id = "orphan-hub-camera"; + constexpr const char* hub_track_id = "orphan-hub-camera/video/color"; + media::TrackDescriptor::Config track_config; + track_config.id = hub_track_id; + track_config.source_id = hub_source_id; + track_config.kind = media::MediaKind::VIDEO; + track_config.time_base = {1, 90000}; + auto descriptor = media::makeTrackDescriptor(std::move(track_config)); + auto hub_stop_calls = std::make_shared>(0); + media::MediaSourceHub::SourceCallbacks callbacks; + callbacks.start = []( + const media::MediaSourceHub::FrameSink&, + const media::MediaSourceHub::CancelPredicate&) { + return true; + }; + callbacks.stop_confirmed = [hub_stop_calls] { + hub_stop_calls->fetch_add(1, std::memory_order_relaxed); + return true; + }; + auto& hub = media::globalMediaSourceHub(); + ASSERT_TRUE(hub.registerSource( + descriptor, std::move(callbacks), 2)); + auto subscription = hub.subscribe(hub_track_id); + ASSERT_TRUE(subscription.valid()); + + auto operational_camera = std::make_shared( + "orphan-operational-camera"); + operational_camera->setRecording(false); + ASSERT_EQ( + globalCameraOperationalActivityRegistry().start( + operational_camera->id(), operational_camera), + CameraOperationalActivityRegistry::DispatchResult::Success); + + auto ptz_camera = std::make_shared( + "orphan-ptz-camera"); + ASSERT_EQ( + globalCameraPtzActivityRegistry().control( + ptz_camera->id(), + ptz_camera, + device::PtzCommand::PanLeft, + false, + 3), + CameraPtzActivityRegistry::DispatchResult::Success); + + service_ = std::make_unique(); + api::StopAllCommand_Request request; + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll( + &context, &request, &response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ(hub_stop_calls->load(std::memory_order_relaxed), 1); + EXPECT_FALSE(subscription.valid()); + EXPECT_TRUE(hub.trackedSourceIds().empty()); + EXPECT_EQ(operational_camera->operationalStopCalls(), 1); + EXPECT_FALSE(operational_camera->operationalActive()); + EXPECT_EQ(operational_camera->lifecycleStopCalls(), 0); + EXPECT_EQ(operational_camera->stopRecordingCalls(), 0); + EXPECT_EQ(ptz_camera->ptzCalls(), 2); + EXPECT_FALSE(ptz_camera->ptzActive()); + EXPECT_EQ(ptz_camera->lifecycleStopCalls(), 0); + EXPECT_EQ(ptz_camera->stopRecordingCalls(), 0); +} + +TEST_F(GrpcSystemServiceTest, + StopAllMediaFailuresLeaveActionQueuePaused) +{ + initializeActionDevices(); + auto& manager = device::DeviceManager::getInstance(); + auto camera = std::make_shared( + "camera-remains-recording", false); + auto microphone = std::make_shared( + "microphone-stop-throws", true, true); + auto speaker = std::make_shared( + "speaker-stop-fails", false); + manager.registerDevice(camera); + manager.registerDevice(microphone); + manager.registerDevice(speaker); + + api::StopAllCommand_Request stop_request; + api::StopAllCommand_Feedback stop_response; + grpc::ServerContext stop_context; + const auto stop_status = service_->StopAll( + &stop_context, &stop_request, &stop_response); + + api::ActionQueueCommand_Request action_request; + action_request.set_action_id("action-after-media-stop-failure"); + addMoveLStep( + action_request, "must-not-run", action_arm_->id(), 1.0); + const auto action_response = executeAction(action_request); + + 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); + EXPECT_EQ(camera->stopRecordingCalls(), 2); + EXPECT_TRUE(camera->isRecording()); + EXPECT_EQ(microphone->stopRecordingCalls(), 2); + EXPECT_TRUE(microphone->isRecording()); + EXPECT_EQ(speaker->stopPlaybackCalls(), 2); + EXPECT_EQ(camera->lifecycleStopCalls(), 0); + EXPECT_EQ(microphone->lifecycleStopCalls(), 0); + EXPECT_EQ(speaker->lifecycleStopCalls(), 0); + EXPECT_FALSE(action_response.header().success()); + EXPECT_EQ( + action_response.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_NE( + action_response.header().error_message().find( + "temporarily paused by StopAll"), + std::string::npos); + EXPECT_EQ(action_arm_->motionCalls(), 0); +} + TEST_F(GrpcSystemServiceTest, ActionQueueCancellationStopsActiveLaterArmBeforeEarlierArm) { @@ -1761,6 +2993,13 @@ TEST_F(GrpcSystemServiceTest, service_->prepareForShutdown(); const auto response = action.get(); + + api::ActionQueueCommand_Request rejected_request; + rejected_request.set_action_id("action-after-service-shutdown"); + addMoveLStep( + rejected_request, "must-not-run", earlier_arm->id(), 3.0); + const auto rejected_response = executeAction(rejected_request); + const auto trace = action_trace_->names(); const auto active_stop = std::find( trace.begin(), trace.end(), "arm:stop:active-arm"); @@ -1775,22 +3014,29 @@ TEST_F(GrpcSystemServiceTest, ASSERT_NE(active_stop, trace.end()); ASSERT_NE(earlier_stop, trace.end()); EXPECT_LT(active_stop, earlier_stop); + EXPECT_FALSE(rejected_response.header().success()); + EXPECT_EQ( + rejected_response.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_NE( + rejected_response.header().error_message().find("shutting down"), + std::string::npos); + EXPECT_EQ(earlier_arm->motionCalls(), 1); + EXPECT_EQ(active_arm->motionCalls(), 1); } TEST_F(GrpcSystemServiceTest, - StopAllStopsRegisteredDevicesAndRevokesOnlyArmLease) + StopAllDoesNotStopDeviceLifecyclesAndRevokesOnlyArmLease) { config::DeviceManagerConfig config; auto& manager = device::DeviceManager::getInstance(config); - auto arm = std::make_shared( - "leased_arm", device::DeviceKind::Arm, "TestArm"); - auto camera = std::make_shared( - "leased_camera", device::DeviceKind::Camera, "TestCamera"); - auto already_stopping_arm = std::make_shared( - "already_stopping_arm", device::DeviceKind::Arm, "TestArm"); - registerDevice(manager, arm); - registerDevice(manager, camera); - registerDevice(manager, already_stopping_arm); + auto trace = std::make_shared(); + auto arm = std::make_shared("leased_arm", trace); + auto camera = std::make_shared("leased_camera"); + auto already_stopping_arm = std::make_shared( + "already_stopping_arm", trace); + manager.registerDevice(arm); + manager.registerDevice(camera); + manager.registerDevice(already_stopping_arm); auto& authority = control::ControlAuthorityManager::instance(); const auto arm_lease = authority.tryAcquire( @@ -1809,30 +3055,38 @@ TEST_F(GrpcSystemServiceTest, ASSERT_TRUE(authority.validate(camera_lease.token)); service_ = std::make_unique(); - arm->blockNextStop(); api::StopAllCommand_Request request; - api::StopAllCommand_Feedback response; - auto stop_all = std::async( - std::launch::async, - [this, &request, &response]() { - grpc::ServerContext context; - return service_->StopAll(&context, &request, &response); - }); - const bool stop_started = arm->waitForStop( - std::chrono::seconds(2)); - const auto move_during_stop = authority.tryAcquire( - arm->id(), "move-during-stop", std::chrono::seconds(30)); - arm->releaseStop(); - const auto status = stop_all.get(); + auto stop_all = std::async(std::launch::async, [this, &request] { + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll( + &context, &request, &response); + return std::make_pair(status, response); + }); + ASSERT_TRUE(arm->waitForStopMotionCalls( + 1, std::chrono::milliseconds(500))); + EXPECT_FALSE(authority.validate(arm_lease.token)); + EXPECT_EQ( + stop_all.wait_for(std::chrono::milliseconds(20)), + std::future_status::timeout); + authority.release(arm_lease.token); + ASSERT_EQ( + stop_all.wait_for(std::chrono::seconds(1)), + std::future_status::ready); + const auto [status, response] = stop_all.get(); - ASSERT_TRUE(stop_started); - EXPECT_FALSE(move_during_stop.acquired); ASSERT_TRUE(status.ok()) << status.error_message(); ASSERT_TRUE(response.header().success()) << response.header().error_message(); - EXPECT_EQ(arm->stopCalls(), 1); - EXPECT_EQ(camera->stopCalls(), 1); - EXPECT_EQ(already_stopping_arm->stopCalls(), 1); + EXPECT_GE(arm->stopMotionCalls(), 2); + EXPECT_EQ(arm->lifecycleStopCalls(), 0); + EXPECT_EQ(arm->shutdownCalls(), 0); + EXPECT_EQ(camera->lifecycleStopCalls(), 0); + EXPECT_EQ(camera->stopRecordingCalls(), 1); + EXPECT_FALSE(camera->isRecording()); + EXPECT_GE(already_stopping_arm->stopMotionCalls(), 2); + EXPECT_EQ(already_stopping_arm->lifecycleStopCalls(), 0); + EXPECT_EQ(already_stopping_arm->shutdownCalls(), 0); EXPECT_FALSE(authority.validate(arm_lease.token)); EXPECT_FALSE(authority.isLeased(arm->id())); EXPECT_TRUE(authority.validate(camera_lease.token)); diff --git a/cmvr-es/service/grpc/tests/media_activity_coordinator_test.cpp b/cmvr-es/service/grpc/tests/media_activity_coordinator_test.cpp new file mode 100644 index 00000000..eef4d910 --- /dev/null +++ b/cmvr-es/service/grpc/tests/media_activity_coordinator_test.cpp @@ -0,0 +1,262 @@ +#include "service/grpc/include/media_activity_coordinator.h" + +#include +#include +#include +#include +#include +#include + +namespace { + +bool check(const bool condition, const char* expression, const int line) +{ + if (condition) { + return true; + } + std::cerr << "CHECK failed at line " << line << ": " << expression << '\n'; + return false; +} + +#define CHECK_TRUE(expression) \ + do { \ + if (!check(static_cast(expression), #expression, __LINE__)) { \ + return 1; \ + } \ + } while (false) + +} // namespace + +int main() +{ + using namespace std::chrono_literals; + + { + cmvr::service::MediaActivityCoordinator dispatch_coordinator; + auto dispatch_session = dispatch_coordinator.beginSession(); + CHECK_TRUE(dispatch_session); + + std::promise dispatch_entered; + auto dispatch_entered_future = dispatch_entered.get_future(); + std::promise release_dispatch; + auto release_dispatch_future = release_dispatch.get_future(); + auto dispatch_future = std::async(std::launch::async, [&] { + return dispatch_session.runIfCurrent([&] { + dispatch_entered.set_value(); + release_dispatch_future.wait(); + }); + }); + dispatch_entered_future.wait(); + + const auto dispatch_ticket = dispatch_coordinator.beginStopAll(); + CHECK_TRUE(dispatch_ticket.valid()); + CHECK_TRUE(!dispatch_coordinator.waitForStopped( + dispatch_ticket, 20ms)); + + release_dispatch.set_value(); + CHECK_TRUE(dispatch_future.get()); + + dispatch_session.reset(); + CHECK_TRUE(dispatch_coordinator.waitForStopped(dispatch_ticket, 100ms)); + CHECK_TRUE(dispatch_coordinator.finishStopAll(dispatch_ticket, true)); + } + + { + cmvr::service::MediaActivityCoordinator generation_coordinator; + auto old_generation_session = generation_coordinator.beginSession(); + CHECK_TRUE(old_generation_session); + + const auto generation_ticket = generation_coordinator.beginStopAll(); + CHECK_TRUE(generation_ticket.valid()); + + std::atomic dispatched_operations{0}; + CHECK_TRUE(!old_generation_session.runIfCurrent( + [&] { ++dispatched_operations; })); + CHECK_TRUE(dispatched_operations.load(std::memory_order_acquire) == 0); + + old_generation_session.reset(); + CHECK_TRUE( + generation_coordinator.waitForStopped(generation_ticket, 100ms)); + CHECK_TRUE(generation_coordinator.finishStopAll(generation_ticket, true)); + + auto current_generation_session = generation_coordinator.beginSession(); + CHECK_TRUE(current_generation_session); + CHECK_TRUE(current_generation_session.runIfCurrent( + [&] { ++dispatched_operations; })); + CHECK_TRUE(dispatched_operations.load(std::memory_order_acquire) == 1); + } + + { + cmvr::service::MediaActivityCoordinator deferred_coordinator; + std::atomic deferred_cancel_calls{0}; + auto deferred_session = deferred_coordinator.beginSession( + [&] { ++deferred_cancel_calls; }); + CHECK_TRUE(deferred_session); + + const auto deferred_ticket = + deferred_coordinator.beginStopAll(true); + CHECK_TRUE(deferred_ticket.valid()); + CHECK_TRUE(deferred_session.cancelled()); + CHECK_TRUE(!deferred_session.runIfCurrent([] {})); + CHECK_TRUE( + deferred_cancel_calls.load(std::memory_order_acquire) == 0); + CHECK_TRUE( + deferred_coordinator.requestCancellation(deferred_ticket)); + CHECK_TRUE( + deferred_cancel_calls.load(std::memory_order_acquire) == 1); + CHECK_TRUE( + deferred_coordinator.requestCancellation(deferred_ticket)); + CHECK_TRUE( + deferred_cancel_calls.load(std::memory_order_acquire) == 1); + + deferred_session.reset(); + CHECK_TRUE( + deferred_coordinator.waitForStopped(deferred_ticket, 100ms)); + CHECK_TRUE( + deferred_coordinator.finishStopAll(deferred_ticket, true)); + CHECK_TRUE( + !deferred_coordinator.requestCancellation(deferred_ticket)); + } + + { + cmvr::service::MediaActivityCoordinator parallel_coordinator; + std::promise blocked_cancel_entered; + auto blocked_cancel_entered_future = + blocked_cancel_entered.get_future(); + std::promise release_blocked_cancel; + auto release_blocked_cancel_future = + release_blocked_cancel.get_future().share(); + std::promise peer_cancel_entered; + auto peer_cancel_entered_future = peer_cancel_entered.get_future(); + std::atomic blocked_cancel_calls{0}; + std::atomic peer_cancel_calls{0}; + + auto blocked_session = parallel_coordinator.beginSession([&] { + ++blocked_cancel_calls; + blocked_cancel_entered.set_value(); + release_blocked_cancel_future.wait(); + }); + auto peer_session = parallel_coordinator.beginSession([&] { + ++peer_cancel_calls; + peer_cancel_entered.set_value(); + }); + CHECK_TRUE(blocked_session && peer_session); + + const auto ticket = parallel_coordinator.beginStopAll(true); + std::vector operations; + std::string collect_error; + CHECK_TRUE(parallel_coordinator.collectCancellationOperations( + ticket, operations, &collect_error)); + CHECK_TRUE(collect_error.empty()); + CHECK_TRUE(operations.size() == 2); + CHECK_TRUE(operations[0].resource_key != operations[1].resource_key); + + auto first = std::async(std::launch::async, operations[0].operation); + auto second = std::async(std::launch::async, operations[1].operation); + CHECK_TRUE(blocked_cancel_entered_future.wait_for(100ms) == + std::future_status::ready); + CHECK_TRUE(peer_cancel_entered_future.wait_for(100ms) == + std::future_status::ready); + release_blocked_cancel.set_value(); + CHECK_TRUE(first.get().success); + CHECK_TRUE(second.get().success); + CHECK_TRUE(operations[0].operation().success); + CHECK_TRUE(operations[1].operation().success); + CHECK_TRUE(blocked_cancel_calls.load() == 1); + CHECK_TRUE(peer_cancel_calls.load() == 1); + + blocked_session.reset(); + peer_session.reset(); + CHECK_TRUE(parallel_coordinator.waitForStopped(ticket, 100ms)); + CHECK_TRUE(parallel_coordinator.finishStopAll(ticket, true)); + CHECK_TRUE(!parallel_coordinator.collectCancellationOperations( + ticket, operations, &collect_error)); + CHECK_TRUE(!collect_error.empty()); + } + + { + cmvr::service::MediaActivityCoordinator exception_coordinator; + std::atomic throwing_cancel_calls{0}; + auto throwing_session = exception_coordinator.beginSession([&] { + ++throwing_cancel_calls; + throw std::runtime_error("cancel failed"); + }); + CHECK_TRUE(throwing_session); + const auto ticket = exception_coordinator.beginStopAll(true); + std::vector operations; + CHECK_TRUE(exception_coordinator.collectCancellationOperations( + ticket, operations)); + CHECK_TRUE(operations.size() == 1); + const auto result = operations[0].operation(); + const auto cached_result = operations[0].operation(); + CHECK_TRUE(!result.success); + CHECK_TRUE(result.detail.find("cancel failed") != std::string::npos); + CHECK_TRUE(cached_result.detail == result.detail); + CHECK_TRUE(throwing_cancel_calls.load() == 1); + throwing_session.reset(); + CHECK_TRUE(exception_coordinator.waitForStopped(ticket, 100ms)); + CHECK_TRUE(!exception_coordinator.finishStopAll(ticket, false)); + } + + { + cmvr::service::DeferredStopOperation detached_operation; + cmvr::service::MediaActivityCoordinator::Session retained_session; + std::atomic detached_cancel_calls{0}; + { + cmvr::service::MediaActivityCoordinator ephemeral_coordinator; + retained_session = ephemeral_coordinator.beginSession( + [&] { ++detached_cancel_calls; }); + const auto ticket = + ephemeral_coordinator.beginStopAll(true); + std::vector operations; + CHECK_TRUE( + ephemeral_coordinator.collectCancellationOperations( + ticket, operations)); + CHECK_TRUE(operations.size() == 1); + detached_operation = std::move(operations[0]); + } + CHECK_TRUE(detached_operation.operation().success); + CHECK_TRUE(detached_cancel_calls.load() == 1); + retained_session.reset(); + } + + cmvr::service::MediaActivityCoordinator coordinator; + std::atomic cancel_calls{0}; + + auto old_session = coordinator.beginSession([&] { ++cancel_calls; }); + CHECK_TRUE(old_session); + CHECK_TRUE(old_session.claimExclusiveResource("speaker:test")); + + auto competing_session = coordinator.beginSession(); + CHECK_TRUE(competing_session); + CHECK_TRUE(!competing_session.claimExclusiveResource("speaker:test")); + competing_session.reset(); + + const auto ticket = coordinator.beginStopAll(); + CHECK_TRUE(ticket.valid()); + CHECK_TRUE(cancel_calls.load(std::memory_order_acquire) == 1); + CHECK_TRUE(old_session.cancelled()); + CHECK_TRUE(!old_session.runIfCurrent([] {})); + CHECK_TRUE(!coordinator.beginSession()); + CHECK_TRUE(!coordinator.waitForStopped(ticket, 5ms)); + + old_session.reset(); + CHECK_TRUE(coordinator.waitForStopped(ticket, 100ms)); + CHECK_TRUE(coordinator.finishStopAll(ticket, true)); + + auto resumed_session = coordinator.beginSession(); + CHECK_TRUE(resumed_session); + CHECK_TRUE(resumed_session.claimExclusiveResource("speaker:test")); + + const auto concurrent_ticket_a = coordinator.beginStopAll(); + const auto concurrent_ticket_b = coordinator.beginStopAll(); + resumed_session.reset(); + CHECK_TRUE(coordinator.waitForStopped(concurrent_ticket_a, 100ms)); + CHECK_TRUE(coordinator.finishStopAll(concurrent_ticket_a, true)); + CHECK_TRUE(!coordinator.beginSession()); + CHECK_TRUE(coordinator.finishStopAll(concurrent_ticket_b, true)); + CHECK_TRUE(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 new file mode 100644 index 00000000..8a6d9c8d --- /dev/null +++ b/cmvr-es/service/grpc/tests/motor_activity_coordinator_test.cpp @@ -0,0 +1,266 @@ +#include "service/grpc/include/motor_activity_coordinator.h" + +#include +#include +#include +#include +#include +#include + +namespace { + +bool check(const bool condition, const char* expression, const int line) +{ + if (condition) { + return true; + } + std::cerr << "CHECK failed at line " << line << ": " << expression << '\n'; + return false; +} + +#define CHECK_TRUE(expression) \ + do { \ + if (!check(static_cast(expression), #expression, __LINE__)) { \ + return 1; \ + } \ + } while (false) + +} // namespace + +int main() +{ + using namespace std::chrono_literals; + using cmvr::service::MotorActivityCoordinator; + + MotorActivityCoordinator coordinator; + std::atomic cancel_calls{0}; + std::atomic quick_stop_calls{0}; + std::atomic busy{true}; + + auto registration = coordinator.registerControl( + [&] { ++cancel_calls; }, + [&] { + ++quick_stop_calls; + return true; + }, + [&] { return !busy.load(std::memory_order_acquire); }, + "test motor"); + CHECK_TRUE(registration); + + const auto ticket = coordinator.beginStopAll(); + CHECK_TRUE(ticket.valid()); + CHECK_TRUE(cancel_calls.load(std::memory_order_acquire) == 1); + CHECK_TRUE(!coordinator.lockAdmission().accepting()); + + std::string initial_error; + CHECK_TRUE(coordinator.requestStop(ticket, &initial_error)); + CHECK_TRUE(initial_error.empty()); + CHECK_TRUE(quick_stop_calls.load(std::memory_order_acquire) == 1); + + auto stop_future = std::async(std::launch::async, [&] { + std::string error; + return coordinator.waitForStopped(ticket, 1s, &error); + }); + CHECK_TRUE(stop_future.wait_for(20ms) == std::future_status::timeout); + + busy.store(false, std::memory_order_release); + coordinator.notifyStateChanged(); + CHECK_TRUE(stop_future.get()); + CHECK_TRUE(coordinator.finishStopAll(ticket, true)); + CHECK_TRUE(coordinator.lockAdmission().accepting()); + + std::atomic quick_stops_in_flight{0}; + std::atomic maximum_quick_stops{0}; + auto make_serial_registration = [&](const char* description) { + return coordinator.registerControl( + [] {}, + [&] { + const int active = ++quick_stops_in_flight; + int maximum = maximum_quick_stops.load(); + while (maximum < active && + !maximum_quick_stops.compare_exchange_weak( + maximum, active)) { + } + std::this_thread::sleep_for(5ms); + --quick_stops_in_flight; + return true; + }, + [] { return true; }, + description); + }; + auto serial_a = make_serial_registration("serial motor a"); + auto serial_b = make_serial_registration("serial motor b"); + CHECK_TRUE(serial_a && serial_b); + + const auto serial_ticket = coordinator.beginStopAll(); + CHECK_TRUE(serial_ticket.valid()); + CHECK_TRUE(coordinator.stopAndWait(serial_ticket, 1s)); + CHECK_TRUE(maximum_quick_stops.load(std::memory_order_acquire) == 1); + CHECK_TRUE(coordinator.finishStopAll(serial_ticket, true)); + + { + MotorActivityCoordinator parallel_coordinator; + std::promise blocked_stop_entered; + auto blocked_stop_entered_future = + blocked_stop_entered.get_future(); + std::promise release_blocked_stop; + auto release_blocked_stop_future = + release_blocked_stop.get_future().share(); + std::promise peer_stop_entered; + auto peer_stop_entered_future = peer_stop_entered.get_future(); + std::atomic blocked_cancel_calls{0}; + std::atomic blocked_stop_calls{0}; + std::atomic peer_cancel_calls{0}; + std::atomic peer_stop_calls{0}; + + auto blocked = parallel_coordinator.registerControl( + [&] { ++blocked_cancel_calls; }, + [&] { + if (blocked_cancel_calls.load() != 1) { + return false; + } + ++blocked_stop_calls; + blocked_stop_entered.set_value(); + release_blocked_stop_future.wait(); + return true; + }, + [] { return true; }, + "blocked motor"); + auto peer = parallel_coordinator.registerControl( + [&] { ++peer_cancel_calls; }, + [&] { + if (peer_cancel_calls.load() != 1) { + return false; + } + ++peer_stop_calls; + peer_stop_entered.set_value(); + return true; + }, + [] { return true; }, + "peer motor"); + CHECK_TRUE(blocked && peer); + + const auto parallel_ticket = + parallel_coordinator.beginStopAll(true); + CHECK_TRUE(parallel_ticket.valid()); + CHECK_TRUE(blocked_cancel_calls.load() == 0); + CHECK_TRUE(peer_cancel_calls.load() == 0); + std::vector operations; + std::string collect_error; + CHECK_TRUE(parallel_coordinator.collectStopOperations( + parallel_ticket, operations, &collect_error)); + CHECK_TRUE(collect_error.empty()); + CHECK_TRUE(operations.size() == 2); + CHECK_TRUE(operations[0].resource_key != operations[1].resource_key); + + auto first = std::async(std::launch::async, operations[0].operation); + auto second = std::async(std::launch::async, operations[1].operation); + CHECK_TRUE(blocked_stop_entered_future.wait_for(100ms) == + std::future_status::ready); + CHECK_TRUE(peer_stop_entered_future.wait_for(100ms) == + std::future_status::ready); + release_blocked_stop.set_value(); + CHECK_TRUE(first.get().success); + CHECK_TRUE(second.get().success); + CHECK_TRUE(operations[0].operation().success); + CHECK_TRUE(operations[1].operation().success); + CHECK_TRUE(blocked_cancel_calls.load() == 1); + CHECK_TRUE(blocked_stop_calls.load() == 1); + CHECK_TRUE(peer_cancel_calls.load() == 1); + CHECK_TRUE(peer_stop_calls.load() == 1); + CHECK_TRUE(parallel_coordinator.finishStopAll( + parallel_ticket, true)); + } + + { + MotorActivityCoordinator exception_coordinator; + std::atomic cancel_calls_for_exception{0}; + std::atomic stop_calls_after_exception{0}; + auto throwing = exception_coordinator.registerControl( + [&] { + ++cancel_calls_for_exception; + throw std::runtime_error("cancel failed"); + }, + [&] { + ++stop_calls_after_exception; + throw std::runtime_error("quick-stop failed"); + return false; + }, + [] { return true; }, + "throwing motor"); + CHECK_TRUE(throwing); + const auto exception_ticket = + exception_coordinator.beginStopAll(true); + std::vector operations; + CHECK_TRUE(exception_coordinator.collectStopOperations( + exception_ticket, operations)); + CHECK_TRUE(operations.size() == 1); + const auto first_result = operations[0].operation(); + const auto cached_result = operations[0].operation(); + CHECK_TRUE(!first_result.success); + CHECK_TRUE(first_result.detail.find("cancel failed") != + std::string::npos); + CHECK_TRUE(first_result.detail.find("quick-stop failed") != + std::string::npos); + CHECK_TRUE(cached_result.detail == first_result.detail); + CHECK_TRUE(cancel_calls_for_exception.load() == 1); + CHECK_TRUE(stop_calls_after_exception.load() == 1); + CHECK_TRUE(!exception_coordinator.finishStopAll( + exception_ticket, false)); + } + + { + cmvr::service::DeferredStopOperation detached_operation; + MotorActivityCoordinator::Registration retained_registration; + std::atomic detached_cancel_calls{0}; + std::atomic detached_stop_calls{0}; + { + MotorActivityCoordinator ephemeral_coordinator; + retained_registration = ephemeral_coordinator.registerControl( + [&] { ++detached_cancel_calls; }, + [&] { + ++detached_stop_calls; + return true; + }, + [] { return true; }, + "detached motor"); + const auto ticket = + ephemeral_coordinator.beginStopAll(true); + std::vector operations; + CHECK_TRUE(ephemeral_coordinator.collectStopOperations( + ticket, operations)); + CHECK_TRUE(operations.size() == 1); + detached_operation = std::move(operations[0]); + } + CHECK_TRUE(detached_operation.operation().success); + CHECK_TRUE(detached_cancel_calls.load() == 1); + CHECK_TRUE(detached_stop_calls.load() == 1); + retained_registration.reset(); + } + + std::atomic stop_succeeds{false}; + auto failing = coordinator.registerControl( + [] {}, + [&] { return stop_succeeds.load(std::memory_order_acquire); }, + [] { return true; }, + "failing motor"); + CHECK_TRUE(failing); + + const auto failed_ticket = coordinator.beginStopAll(); + CHECK_TRUE(failed_ticket.valid()); + std::string error; + CHECK_TRUE(!coordinator.stopAndWait(failed_ticket, 100ms, &error)); + CHECK_TRUE(!error.empty()); + CHECK_TRUE(!coordinator.finishStopAll(failed_ticket, false)); + CHECK_TRUE(!coordinator.lockAdmission().accepting()); + + stop_succeeds.store(true, std::memory_order_release); + const auto recovery_ticket = coordinator.beginStopAll(); + CHECK_TRUE(recovery_ticket.valid()); + CHECK_TRUE(coordinator.stopAndWait(recovery_ticket, 1s, &error)); + CHECK_TRUE(coordinator.finishStopAll(recovery_ticket, true)); + CHECK_TRUE(coordinator.lockAdmission().accepting()); + + std::cout << "motor_activity_coordinator_test: PASS\n"; + return 0; +} diff --git a/cmvr-es/service/quic_edge/CMakeLists.txt b/cmvr-es/service/quic_edge/CMakeLists.txt index 6dbe62bd..a6562f83 100644 --- a/cmvr-es/service/quic_edge/CMakeLists.txt +++ b/cmvr-es/service/quic_edge/CMakeLists.txt @@ -23,6 +23,28 @@ target_link_libraries(quic_edge_service ) add_library(cmvr_es::quic_edge_service ALIAS quic_edge_service) + +if(BUILD_TESTING) + add_executable(quic_edge_protocol_test tests/quic_edge_protocol_test.cpp) + target_compile_features(quic_edge_protocol_test PRIVATE cxx_std_17) + target_link_libraries(quic_edge_protocol_test PRIVATE + cmvr_es::quic_edge_service + cmvr_es::media_source_hub + cmvr_es::stop_all_admission_gate + Threads::Threads + ) + add_test(NAME quic_edge_protocol_test COMMAND quic_edge_protocol_test) + set(_quic_edge_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}") + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _quic_edge_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(quic_edge_protocol_test PROPERTIES + TIMEOUT 15 + ENVIRONMENT "${_quic_edge_test_environment}") +endif() + install(TARGETS quic_edge_service ARCHIVE DESTINATION lib LIBRARY DESTINATION lib) diff --git a/cmvr-es/service/quic_edge/include/quic_edge_service.h b/cmvr-es/service/quic_edge/include/quic_edge_service.h index e4618499..23eae61b 100644 --- a/cmvr-es/service/quic_edge/include/quic_edge_service.h +++ b/cmvr-es/service/quic_edge/include/quic_edge_service.h @@ -88,6 +88,10 @@ public: bool initialize(std::string* error); bool start(std::string* error); + // Releases the current media subscriptions without stopping the QUIC + // transport, node registration, heartbeat loop, or service worker. The + // media worker automatically subscribes again when admission permits it. + bool interruptMediaActivities(); void stop(); QuicEdgeServiceState state() const; @@ -124,8 +128,10 @@ private: bool dispatchControlFrame(const std::vector& frame, std::string* error); bool openMediaSession(std::vector* tracks, + std::uint64_t activity_generation, std::string* error); - void refreshMediaTracks(std::vector* tracks); + void refreshMediaTracks(std::vector* tracks, + std::uint64_t activity_generation); bool hasEnabledMediaTracks() const; bool ensureSourceRegistered(const config::QuicEdgeTrackConfig& track, const std::string& source_track_id, @@ -161,6 +167,7 @@ private: mutable std::mutex mutex_; std::condition_variable stop_cv_; std::condition_variable media_stop_cv_; + std::condition_variable media_activity_cv_; QuicEdgeServiceState state_{QuicEdgeServiceState::UNINITIALIZED}; std::string last_error_; std::string last_media_error_; @@ -174,6 +181,9 @@ private: std::size_t active_media_tracks_{0}; bool stop_requested_{false}; bool media_stop_requested_{false}; + bool media_worker_running_{false}; + std::uint64_t media_activity_generation_{1U}; + std::uint64_t media_activity_quiesced_generation_{1U}; bool media_connection_failed_{false}; std::string media_connection_error_; std::thread worker_; 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 7145f3a0..7081a9df 100644 --- a/cmvr-es/service/quic_edge/src/quic_edge_service.cpp +++ b/cmvr-es/service/quic_edge/src/quic_edge_service.cpp @@ -635,6 +635,31 @@ bool QuicEdgeService::start(std::string* error) return true; } +bool QuicEdgeService::interruptMediaActivities() +{ + std::unique_lock lock(mutex_); + if (state_ == QuicEdgeServiceState::FAILED) { + return false; + } + if (media_activity_generation_ == + std::numeric_limits::max()) { + media_activity_generation_ = 1U; + media_activity_quiesced_generation_ = 0U; + } else { + ++media_activity_generation_; + } + const std::uint64_t requested_generation = media_activity_generation_; + media_stop_cv_.notify_all(); + + media_activity_cv_.wait(lock, [this, requested_generation] { + return !media_worker_running_ || stop_requested_ || + media_activity_quiesced_generation_ >= requested_generation; + }); + return state_ != QuicEdgeServiceState::FAILED && + (!media_worker_running_ || + media_activity_quiesced_generation_ >= requested_generation); +} + void QuicEdgeService::stop() { std::lock_guard lifecycle_lock(lifecycle_mutex_); @@ -931,10 +956,18 @@ bool QuicEdgeService::startMediaWorker(std::string* error) media_connection_failed_ = false; media_connection_error_.clear(); active_media_tracks_ = 0U; + media_worker_running_ = true; } try { media_worker_ = std::thread(&QuicEdgeService::runMedia, this); } catch (const std::exception& exception) { + { + std::lock_guard lock(mutex_); + media_worker_running_ = false; + media_activity_quiesced_generation_ = + media_activity_generation_; + } + media_activity_cv_.notify_all(); const std::string message = std::string("failed to start QUIC media worker: ") + exception.what(); recordMediaConnectionFailure(message); @@ -952,8 +985,13 @@ void QuicEdgeService::stopMediaWorker() } media_stop_cv_.notify_all(); if (media_worker_.joinable()) media_worker_.join(); - std::lock_guard lock(mutex_); - active_media_tracks_ = 0U; + { + std::lock_guard lock(mutex_); + active_media_tracks_ = 0U; + media_worker_running_ = false; + media_activity_quiesced_generation_ = media_activity_generation_; + } + media_activity_cv_.notify_all(); } void QuicEdgeService::recordMediaConnectionFailure(const std::string& error) @@ -974,12 +1012,36 @@ void QuicEdgeService::recordMediaConnectionFailure(const std::string& error) void QuicEdgeService::runMedia() { std::vector tracks; + std::uint64_t activity_generation = 0U; + { + std::lock_guard lock(mutex_); + activity_generation = media_activity_generation_; + media_activity_quiesced_generation_ = activity_generation; + } + media_activity_cv_.notify_all(); try { next_media_source_retry_ = std::chrono::steady_clock::now(); while (true) { + std::uint64_t requested_generation = 0U; { std::lock_guard lock(mutex_); if (stop_requested_ || media_stop_requested_) break; + requested_generation = media_activity_generation_; + } + if (requested_generation != activity_generation) { + // Clearing the tracks is the publication barrier for this + // generation: subscriptions are released before StopAll is + // told that old QUIC media activity has quiesced. + tracks.clear(); + { + std::lock_guard lock(mutex_); + active_media_tracks_ = 0U; + activity_generation = requested_generation; + media_activity_quiesced_generation_ = + requested_generation; + } + media_activity_cv_.notify_all(); + continue; } if (!transport_->isConnected()) break; @@ -992,7 +1054,8 @@ void QuicEdgeService::runMedia() return !track.subscription.valid(); }), tracks.end()); - if (!openMediaSession(&tracks, &error)) { + if (!openMediaSession( + &tracks, activity_generation, &error)) { recordMediaConnectionFailure(error); break; } @@ -1039,8 +1102,13 @@ void QuicEdgeService::runMedia() recordMediaConnectionFailure("unknown QUIC media worker exception"); } tracks.clear(); - std::lock_guard lock(mutex_); - active_media_tracks_ = 0U; + { + std::lock_guard lock(mutex_); + active_media_tracks_ = 0U; + media_worker_running_ = false; + media_activity_quiesced_generation_ = media_activity_generation_; + } + media_activity_cv_.notify_all(); } bool QuicEdgeService::performRegistration(ControlFrameDecoder* decoder, @@ -1326,7 +1394,9 @@ bool QuicEdgeService::dispatchControlFrame( return false; } -bool QuicEdgeService::openMediaSession(std::vector* tracks, +bool QuicEdgeService::openMediaSession( + std::vector* tracks, + const std::uint64_t activity_generation, std::string* error) { if (!tracks) { @@ -1351,11 +1421,13 @@ bool QuicEdgeService::openMediaSession(std::vector* tracks, ++stats_.media_sessions_opened; } } - refreshMediaTracks(tracks); + refreshMediaTracks(tracks, activity_generation); return true; } -void QuicEdgeService::refreshMediaTracks(std::vector* tracks) +void QuicEdgeService::refreshMediaTracks( + std::vector* tracks, + const std::uint64_t activity_generation) { if (!tracks) return; for (const auto& track_config : config_.tracks()) { @@ -1379,9 +1451,10 @@ void QuicEdgeService::refreshMediaTracks(std::vector* tracks) track.subscription = media_hub_->subscribe( track.source_track_id, media::MediaSourceHub::StartPosition::LATEST_AVAILABLE, - [this] { + [this, activity_generation] { std::lock_guard lock(mutex_); - return stop_requested_ || media_stop_requested_; + return stop_requested_ || media_stop_requested_ || + media_activity_generation_ != activity_generation; }); if (!track.subscription.valid()) { recordMediaError( diff --git a/cmvr-es/service/quic_edge/tests/quic_edge_protocol_test.cpp b/cmvr-es/service/quic_edge/tests/quic_edge_protocol_test.cpp index 2aa5d876..f607d16f 100644 --- a/cmvr-es/service/quic_edge/tests/quic_edge_protocol_test.cpp +++ b/cmvr-es/service/quic_edge/tests/quic_edge_protocol_test.cpp @@ -18,6 +18,7 @@ #include "service/quic_edge/include/control_framing.h" #include "service/quic_edge/include/datagram_packetizer.h" #include "service/quic_edge/include/quic_edge_service.h" +#include "service/stop_all/include/stop_all_admission_gate.h" namespace { @@ -576,6 +577,154 @@ bool testServiceWithSharedHub() return true; } +bool testMediaActivityInterruptPreservesPresenceAndResumes() +{ + const std::string track_id = "camera-stop-all/video/color"; + service::StopAllAdmissionGate admission; + media::MediaSourceHub hub(&admission); + media::MediaSourceHub::FrameSink sink; + std::mutex sink_mutex; + std::atomic source_started{false}; + std::atomic source_starts{0U}; + std::atomic source_stops{0U}; + media::MediaSourceHub::SourceCallbacks callbacks; + callbacks.start = [&](const media::MediaSourceHub::FrameSink& value, + const media::MediaSourceHub::CancelPredicate&) { + { + std::lock_guard lock(sink_mutex); + sink = value; + } + source_started.store(true); + ++source_starts; + return true; + }; + callbacks.stop = [&]() { + source_started.store(false); + ++source_stops; + }; + callbacks.request_key_frame = [] { return true; }; + const auto descriptor = videoDescriptor(track_id); + CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 8U)); + + auto transport = std::make_unique(); + FakeTransport* transport_view = transport.get(); + quic_edge::QuicEdgeService edge_service( + validConfig(track_id), std::move(transport), hub); + std::string error; + CHECK_TRUE(edge_service.initialize(&error)); + CHECK_TRUE(edge_service.start(&error)); + CHECK_TRUE(waitUntil([&] { + return edge_service.state() == quic_edge::QuicEdgeServiceState::ONLINE && + edge_service.status().registered && source_started.load() && + hub.subscriberCount(track_id) == 1U; + })); + + auto publish = [&](const std::uint64_t sequence) { + media::MediaSourceHub::FrameSink publisher; + { + std::lock_guard lock(sink_mutex); + publisher = sink; + } + if (!publisher) return false; + media::MediaFrame::Config frame; + frame.descriptor = descriptor; + frame.payload.resize(256U, 0x5aU); + frame.sequence = sequence; + frame.capture_time_ns = sequence * 1000000U; + frame.key_frame = true; + publisher(media::makeMediaFrame(std::move(frame))); + return true; + }; + + CHECK_TRUE(publish(1U)); + CHECK_TRUE(waitUntil([&] { + return transport_view->datagramBatchCount() == 1U; + })); + const auto heartbeat_before_stop = transport_view->heartbeatCount(); + + const auto ticket = admission.beginStopAll(); + CHECK_TRUE(edge_service.interruptMediaActivities()); + CHECK_TRUE(hub.subscriberCount(track_id) == 0U); + CHECK_TRUE(!source_started.load()); + CHECK_TRUE(source_stops.load() == 1U); + CHECK_TRUE(edge_service.state() == quic_edge::QuicEdgeServiceState::ONLINE); + CHECK_TRUE(edge_service.status().registered); + CHECK_TRUE(transport_view->connectCount() == 1U); + + // A publisher retained by the stopped generation must no longer enqueue + // frames while StopAll admission is closed. + CHECK_TRUE(publish(2U)); + std::this_thread::sleep_for(std::chrono::milliseconds(30)); + CHECK_TRUE(transport_view->datagramBatchCount() == 1U); + CHECK_TRUE(admission.finishStopAll(ticket, true)); + + CHECK_TRUE(waitUntil([&] { + return source_starts.load() == 2U && source_started.load() && + hub.subscriberCount(track_id) == 1U; + })); + CHECK_TRUE(publish(3U)); + CHECK_TRUE(waitUntil([&] { + return transport_view->datagramBatchCount() == 2U; + })); + CHECK_TRUE(waitUntil([&] { + return transport_view->heartbeatCount() > heartbeat_before_stop; + })); + CHECK_TRUE(edge_service.stats().registrations_accepted == 1U); + CHECK_TRUE(transport_view->connectCount() == 1U); + CHECK_TRUE(edge_service.state() == quic_edge::QuicEdgeServiceState::ONLINE); + edge_service.stop(); + return true; +} + +bool testMediaActivityInterruptCancelsStartingSubscription() +{ + const std::string track_id = "slow-stop-all/video/color"; + service::StopAllAdmissionGate admission; + media::MediaSourceHub hub(&admission); + std::atomic start_entered{false}; + std::atomic start_cancelled{false}; + media::MediaSourceHub::SourceCallbacks callbacks; + callbacks.start = [&](const media::MediaSourceHub::FrameSink&, + const media::MediaSourceHub::CancelPredicate& cancelled) { + start_entered.store(true); + while (!cancelled()) { + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + start_cancelled.store(true); + return false; + }; + callbacks.stop = [] {}; + CHECK_TRUE(hub.registerSource( + videoDescriptor(track_id), std::move(callbacks), 8U)); + + auto transport = std::make_unique(); + FakeTransport* transport_view = transport.get(); + quic_edge::QuicEdgeService edge_service( + validConfig(track_id), std::move(transport), hub); + std::string error; + CHECK_TRUE(edge_service.initialize(&error)); + CHECK_TRUE(edge_service.start(&error)); + CHECK_TRUE(waitUntil([&] { + return start_entered.load() && edge_service.status().registered; + })); + + const auto ticket = admission.beginStopAll(); + auto interrupt = std::async(std::launch::async, [&edge_service] { + return edge_service.interruptMediaActivities(); + }); + CHECK_TRUE(interrupt.wait_for(std::chrono::milliseconds(500)) == + std::future_status::ready); + CHECK_TRUE(interrupt.get()); + CHECK_TRUE(start_cancelled.load()); + CHECK_TRUE(hub.subscriberCount(track_id) == 0U); + CHECK_TRUE(edge_service.state() == quic_edge::QuicEdgeServiceState::ONLINE); + CHECK_TRUE(edge_service.status().registered); + CHECK_TRUE(transport_view->connectCount() == 1U); + CHECK_TRUE(admission.finishStopAll(ticket, true)); + edge_service.stop(); + return true; +} + bool testMissingInjectedSourceRetriesSafely() { media::MediaSourceHub hub; @@ -1048,7 +1197,10 @@ bool testRobotIdIsRequired() int main() { if (!testControlFraming() || !testPacketizer() || - !testServiceWithSharedHub() || !testMissingInjectedSourceRetriesSafely() || + !testServiceWithSharedHub() || + !testMediaActivityInterruptPreservesPresenceAndResumes() || + !testMediaActivityInterruptCancelsStartingSubscription() || + !testMissingInjectedSourceRetriesSafely() || !testPresenceOnlyWithoutMedia() || !testDeviceManagerSnapshotInHeartbeat() || !testAllDeviceKindAndStateMappings() || diff --git a/cmvr-es/service/stop_all/CMakeLists.txt b/cmvr-es/service/stop_all/CMakeLists.txt new file mode 100644 index 00000000..a50b9cc7 --- /dev/null +++ b/cmvr-es/service/stop_all/CMakeLists.txt @@ -0,0 +1,28 @@ +add_library(stop_all_admission_gate STATIC + src/stop_all_admission_gate.cpp +) + +target_compile_features(stop_all_admission_gate PUBLIC cxx_std_17) +target_include_directories(stop_all_admission_gate + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../.. +) + +add_library(cmvr_es::stop_all_admission_gate ALIAS stop_all_admission_gate) + +add_library(camera_operational_activity_registry STATIC + ../grpc/src/camera_operational_activity_registry.cpp +) + +target_compile_features(camera_operational_activity_registry PUBLIC cxx_std_17) +target_include_directories(camera_operational_activity_registry + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../.. +) +target_link_libraries(camera_operational_activity_registry + PUBLIC + cmvr_es::stop_all_admission_gate +) + +add_library(cmvr_es::camera_operational_activity_registry ALIAS + camera_operational_activity_registry) diff --git a/cmvr-es/service/stop_all/include/deferred_stop_operation.h b/cmvr-es/service/stop_all/include/deferred_stop_operation.h new file mode 100644 index 00000000..0b94fb5c --- /dev/null +++ b/cmvr-es/service/stop_all/include/deferred_stop_operation.h @@ -0,0 +1,29 @@ +#ifndef CMVR_ES_DEFERRED_STOP_OPERATION_H +#define CMVR_ES_DEFERRED_STOP_OPERATION_H + +#include +#include + +namespace cmvr::service { + +struct DeferredStopResult { + bool success{false}; + std::string detail; +}; + +// A coordinator-owned stop callback which can be submitted to any executor. +// The resource key is stable for the lifetime of the underlying registration +// or session, and the callback owns all state needed after collection returns. +struct DeferredStopOperation { + std::string resource_key; + std::function operation; + + explicit operator bool() const noexcept + { + return !resource_key.empty() && static_cast(operation); + } +}; + +} // namespace cmvr::service + +#endif // CMVR_ES_DEFERRED_STOP_OPERATION_H diff --git a/cmvr-es/service/stop_all/include/stop_all_admission_gate.h b/cmvr-es/service/stop_all/include/stop_all_admission_gate.h new file mode 100644 index 00000000..4ade4fde --- /dev/null +++ b/cmvr-es/service/stop_all/include/stop_all_admission_gate.h @@ -0,0 +1,89 @@ +#ifndef CMVR_ES_STOP_ALL_ADMISSION_GATE_H +#define CMVR_ES_STOP_ALL_ADMISSION_GATE_H + +#include +#include +#include + +namespace cmvr::service { + +// Provides the cross-domain admission boundary for SystemService::StopAll. +// Action and media coordinators may finish independently, but neither domain +// can admit new work until this gate is completed last. +class StopAllAdmissionGate final { +public: + struct FinishResult { + bool ticket_consumed{false}; + bool admission_reopened{false}; + }; + + struct StopAllTicket { + std::uint64_t generation{0}; + std::uint64_t ticket_id{0}; + + bool valid() const noexcept + { + return generation != 0U && ticket_id != 0U; + } + }; + + class AdmissionGuard final { + public: + AdmissionGuard(AdmissionGuard&&) noexcept = default; + AdmissionGuard& operator=(AdmissionGuard&&) noexcept = default; + + AdmissionGuard(const AdmissionGuard&) = delete; + AdmissionGuard& operator=(const AdmissionGuard&) = delete; + + bool accepting() const noexcept { return accepting_; } + std::uint64_t generation() const noexcept { return generation_; } + + private: + friend class StopAllAdmissionGate; + + AdmissionGuard( + std::unique_lock&& lock, + bool accepting, + std::uint64_t generation) noexcept; + + std::unique_lock lock_; + bool accepting_{false}; + std::uint64_t generation_{0}; + }; + + // The returned guard linearizes a bounded admission operation against + // beginStopAll(). Do not retain it while executing device work. + AdmissionGuard lockAdmission(); + + // Concurrent callers join one round. A failed round remains closed until + // a later StopAll round successfully confirms every domain is stopped. + StopAllTicket beginStopAll(); + bool finishStopAll( + const StopAllTicket& ticket, + bool all_domains_stop_confirmed); + + // Consumes one participant ticket and reports whether the complete round + // actually reopened admission. A participant can confirm its own work + // while another participant has failed or is still outstanding. + FinishResult finishStopAllDetailed( + const StopAllTicket& ticket, + bool all_domains_stop_confirmed); + + // Test/process teardown hook. Runtime code must recover a failed gate with + // a new successful StopAll round instead of bypassing fail-closed state. + void clearForTesting() noexcept; + +private: + std::mutex mutex_; + bool accepting_{true}; + bool stop_all_failed_{false}; + std::uint64_t generation_{1U}; + std::uint64_t next_ticket_id_{0U}; + std::unordered_set outstanding_tickets_; +}; + +StopAllAdmissionGate& globalStopAllAdmissionGate(); + +} // namespace cmvr::service + +#endif // CMVR_ES_STOP_ALL_ADMISSION_GATE_H diff --git a/cmvr-es/service/stop_all/include/stop_operation_dispatcher.h b/cmvr-es/service/stop_all/include/stop_operation_dispatcher.h new file mode 100644 index 00000000..1dc0b8c4 --- /dev/null +++ b/cmvr-es/service/stop_all/include/stop_operation_dispatcher.h @@ -0,0 +1,77 @@ +#ifndef CMVR_ES_STOP_OPERATION_DISPATCHER_H +#define CMVR_ES_STOP_OPERATION_DISPATCHER_H + +#include +#include +#include +#include +#include + +namespace cmvr::service { + +// Runs potentially blocking operational-stop calls without allowing one +// resource to delay stop requests for unrelated resources. The dispatcher +// owns worker handles while it is alive. StopAll RPCs may stop waiting at +// their deadline, but dispatcher destruction joins every outstanding worker +// before device lifecycle teardown is allowed to continue. Native threads +// cannot be canceled safely while they may still be inside a device driver. +class StopOperationDispatcher final { +private: + struct JobState; + struct Impl; + +public: + using Clock = std::chrono::steady_clock; + using Deadline = Clock::time_point; + + struct OperationResult { + bool success{false}; + std::string detail; + }; + + struct WaitResult { + bool completed{false}; + bool result{false}; + std::string detail; + }; + + using Operation = std::function; + + class Handle final { + public: + Handle() = default; + + bool valid() const noexcept; + WaitResult waitUntil(Deadline deadline) const; + + private: + friend class StopOperationDispatcher; + + explicit Handle(std::shared_ptr state) noexcept; + + std::shared_ptr state_; + }; + + StopOperationDispatcher(); + ~StopOperationDispatcher(); + + StopOperationDispatcher(const StopOperationDispatcher&) = delete; + StopOperationDispatcher& operator=(const StopOperationDispatcher&) = delete; + StopOperationDispatcher(StopOperationDispatcher&&) = delete; + StopOperationDispatcher& operator=(StopOperationDispatcher&&) = delete; + + // A running job is shared by every submission for the same resource key. + // Once it has completed, the next submission joins/reaps the old worker + // and starts a new job. Empty keys or operations return an invalid handle. + Handle submit(std::string resource_key, Operation operation); + + // Exposed only to verify dispatcher lifecycle behavior in tests. + std::size_t jobCountForTesting() const; + +private: + std::unique_ptr impl_; +}; + +} // namespace cmvr::service + +#endif // CMVR_ES_STOP_OPERATION_DISPATCHER_H diff --git a/cmvr-es/service/stop_all/src/stop_all_admission_gate.cpp b/cmvr-es/service/stop_all/src/stop_all_admission_gate.cpp new file mode 100644 index 00000000..8920a732 --- /dev/null +++ b/cmvr-es/service/stop_all/src/stop_all_admission_gate.cpp @@ -0,0 +1,90 @@ +#include "service/stop_all/include/stop_all_admission_gate.h" + +#include + +namespace cmvr::service { + +StopAllAdmissionGate::AdmissionGuard::AdmissionGuard( + std::unique_lock&& lock, + const bool accepting, + const std::uint64_t generation) noexcept + : lock_(std::move(lock)), + accepting_(accepting), + generation_(generation) +{ +} + +StopAllAdmissionGate::AdmissionGuard +StopAllAdmissionGate::lockAdmission() +{ + std::unique_lock lock(mutex_); + return AdmissionGuard( + std::move(lock), accepting_, generation_); +} + +StopAllAdmissionGate::StopAllTicket +StopAllAdmissionGate::beginStopAll() +{ + std::lock_guard lock(mutex_); + if (accepting_ || outstanding_tickets_.empty()) { + accepting_ = false; + stop_all_failed_ = false; + ++generation_; + } + + StopAllTicket ticket; + ticket.generation = generation_; + ticket.ticket_id = ++next_ticket_id_; + outstanding_tickets_.emplace(ticket.ticket_id); + return ticket; +} + +bool StopAllAdmissionGate::finishStopAll( + const StopAllTicket& ticket, + const bool all_domains_stop_confirmed) +{ + const auto result = finishStopAllDetailed( + ticket, all_domains_stop_confirmed); + return result.ticket_consumed && all_domains_stop_confirmed; +} + +StopAllAdmissionGate::FinishResult +StopAllAdmissionGate::finishStopAllDetailed( + const StopAllTicket& ticket, + const bool all_domains_stop_confirmed) +{ + std::lock_guard lock(mutex_); + if (!ticket.valid() || accepting_ || + ticket.generation != generation_ || + outstanding_tickets_.erase(ticket.ticket_id) == 0U) { + return {}; + } + + if (!all_domains_stop_confirmed) { + stop_all_failed_ = true; + } + if (outstanding_tickets_.empty() && !stop_all_failed_) { + accepting_ = true; + } + return {true, accepting_}; +} + +void StopAllAdmissionGate::clearForTesting() noexcept +{ + try { + std::lock_guard lock(mutex_); + accepting_ = true; + stop_all_failed_ = false; + ++generation_; + outstanding_tickets_.clear(); + } catch (...) { + } +} + +StopAllAdmissionGate& globalStopAllAdmissionGate() +{ + static StopAllAdmissionGate gate; + return gate; +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/stop_all/src/stop_operation_dispatcher.cpp b/cmvr-es/service/stop_all/src/stop_operation_dispatcher.cpp new file mode 100644 index 00000000..351d5c9f --- /dev/null +++ b/cmvr-es/service/stop_all/src/stop_operation_dispatcher.cpp @@ -0,0 +1,190 @@ +#include "service/stop_all/include/stop_operation_dispatcher.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cmvr::service { + +struct StopOperationDispatcher::JobState final { + std::mutex mutex; + std::condition_variable condition; + bool completed{false}; + OperationResult outcome; +}; + +struct StopOperationDispatcher::Impl final { + struct Job final { + std::shared_ptr state; + std::thread worker; + }; + + std::mutex mutex; + std::unordered_map jobs; +}; + +StopOperationDispatcher::Handle::Handle( + std::shared_ptr state) noexcept + : state_(std::move(state)) +{ +} + +bool StopOperationDispatcher::Handle::valid() const noexcept +{ + return static_cast(state_); +} + +StopOperationDispatcher::WaitResult +StopOperationDispatcher::Handle::waitUntil(const Deadline deadline) const +{ + if (!state_) { + return {false, false, "invalid stop operation handle"}; + } + + std::unique_lock lock(state_->mutex); + if (!state_->condition.wait_until(lock, deadline, [this] { + return state_->completed; + })) { + return { + false, + false, + "stop operation did not complete before the deadline"}; + } + + return {true, state_->outcome.success, state_->outcome.detail}; +} + +StopOperationDispatcher::StopOperationDispatcher() + : impl_(std::make_unique()) +{ +} + +StopOperationDispatcher::~StopOperationDispatcher() +{ + std::vector workers; + { + std::lock_guard lock(impl_->mutex); + workers.reserve(impl_->jobs.size()); + for (auto& entry : impl_->jobs) { + if (entry.second.worker.joinable()) { + workers.emplace_back(std::move(entry.second.worker)); + } + } + impl_->jobs.clear(); + } + for (auto& worker : workers) { + worker.join(); + } +} + +StopOperationDispatcher::Handle StopOperationDispatcher::submit( + std::string resource_key, + Operation operation) +{ + if (resource_key.empty() || !operation) { + return {}; + } + + std::vector completed_workers; + { + std::lock_guard lock(impl_->mutex); + completed_workers.reserve(impl_->jobs.size()); + for (auto job = impl_->jobs.begin(); job != impl_->jobs.end();) { + bool completed = false; + { + std::lock_guard state_lock(job->second.state->mutex); + completed = job->second.state->completed; + } + if (!completed) { + ++job; + continue; + } + + if (job->second.worker.joinable()) { + completed_workers.emplace_back( + std::move(job->second.worker)); + } + job = impl_->jobs.erase(job); + } + } + for (auto& worker : completed_workers) { + worker.join(); + } + + for (;;) { + std::thread completed_worker; + std::unique_lock lock(impl_->mutex); + const auto existing = impl_->jobs.find(resource_key); + if (existing != impl_->jobs.end()) { + bool completed = false; + { + std::lock_guard state_lock(existing->second.state->mutex); + completed = existing->second.state->completed; + } + if (!completed) { + return Handle(existing->second.state); + } + + if (existing->second.worker.joinable()) { + completed_worker = std::move(existing->second.worker); + } + impl_->jobs.erase(existing); + lock.unlock(); + if (completed_worker.joinable()) { + completed_worker.join(); + } + continue; + } + + auto state = std::make_shared(); + const auto inserted = impl_->jobs.emplace( + std::piecewise_construct, + std::forward_as_tuple(std::move(resource_key)), + std::forward_as_tuple()); + auto& job = inserted.first->second; + job.state = state; + try { + job.worker = std::thread( + [state, operation = std::move(operation)]() mutable { + OperationResult outcome; + try { + outcome = operation(); + } catch (const std::exception& error) { + outcome.success = false; + outcome.detail = + std::string("stop operation threw: ") + + error.what(); + } catch (...) { + outcome.success = false; + outcome.detail = + "stop operation threw an unknown exception"; + } + + { + std::lock_guard state_lock(state->mutex); + state->outcome = std::move(outcome); + state->completed = true; + } + state->condition.notify_all(); + }); + } catch (...) { + impl_->jobs.erase(inserted.first); + throw; + } + + return Handle(std::move(state)); + } +} + +std::size_t StopOperationDispatcher::jobCountForTesting() const +{ + std::lock_guard lock(impl_->mutex); + return impl_->jobs.size(); +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/stop_all/tests/stop_all_admission_gate_test.cpp b/cmvr-es/service/stop_all/tests/stop_all_admission_gate_test.cpp new file mode 100644 index 00000000..14300844 --- /dev/null +++ b/cmvr-es/service/stop_all/tests/stop_all_admission_gate_test.cpp @@ -0,0 +1,104 @@ +#include "service/stop_all/include/stop_all_admission_gate.h" + +#include + +namespace cmvr::service { +namespace { + +TEST(StopAllAdmissionGateTest, SuccessfulRoundReopensAdmission) +{ + StopAllAdmissionGate gate; + { + const auto before = gate.lockAdmission(); + EXPECT_TRUE(before.accepting()); + } + + const auto ticket = gate.beginStopAll(); + ASSERT_TRUE(ticket.valid()); + { + const auto blocked = gate.lockAdmission(); + EXPECT_FALSE(blocked.accepting()); + EXPECT_EQ(blocked.generation(), ticket.generation); + } + EXPECT_TRUE(gate.finishStopAll(ticket, true)); + EXPECT_TRUE(gate.lockAdmission().accepting()); +} + +TEST(StopAllAdmissionGateTest, FailedRoundStaysClosedAndCanBeRecovered) +{ + StopAllAdmissionGate gate; + const auto failed_ticket = gate.beginStopAll(); + ASSERT_TRUE(failed_ticket.valid()); + EXPECT_FALSE(gate.finishStopAll(failed_ticket, false)); + EXPECT_FALSE(gate.lockAdmission().accepting()); + + const auto recovery_ticket = gate.beginStopAll(); + ASSERT_TRUE(recovery_ticket.valid()); + EXPECT_TRUE(gate.finishStopAll(recovery_ticket, true)); + EXPECT_TRUE(gate.lockAdmission().accepting()); +} + +TEST(StopAllAdmissionGateTest, EveryConcurrentParticipantMustSucceed) +{ + StopAllAdmissionGate gate; + const auto first = gate.beginStopAll(); + const auto second = gate.beginStopAll(); + ASSERT_TRUE(first.valid()); + ASSERT_TRUE(second.valid()); + + EXPECT_TRUE(gate.finishStopAll(first, true)); + EXPECT_FALSE(gate.lockAdmission().accepting()); + EXPECT_FALSE(gate.finishStopAll(second, false)); + EXPECT_FALSE(gate.lockAdmission().accepting()); +} + +TEST(StopAllAdmissionGateTest, + DetailedFinishDoesNotReportRecoveryAfterAnotherParticipantFailed) +{ + StopAllAdmissionGate gate; + const auto system_ticket = gate.beginStopAll(); + const auto failed_participant = gate.beginStopAll(); + + const auto failed_result = + gate.finishStopAllDetailed(failed_participant, false); + EXPECT_TRUE(failed_result.ticket_consumed); + EXPECT_FALSE(failed_result.admission_reopened); + + const auto system_result = + gate.finishStopAllDetailed(system_ticket, true); + EXPECT_TRUE(system_result.ticket_consumed); + EXPECT_FALSE(system_result.admission_reopened); + EXPECT_FALSE(gate.lockAdmission().accepting()); +} + +TEST(StopAllAdmissionGateTest, + DetailedFinishSeparatesConsumptionFromOutstandingParticipants) +{ + StopAllAdmissionGate gate; + const auto first = gate.beginStopAll(); + const auto second = gate.beginStopAll(); + + const auto first_result = gate.finishStopAllDetailed(first, true); + EXPECT_TRUE(first_result.ticket_consumed); + EXPECT_FALSE(first_result.admission_reopened); + + const auto second_result = gate.finishStopAllDetailed(second, true); + EXPECT_TRUE(second_result.ticket_consumed); + EXPECT_TRUE(second_result.admission_reopened); + EXPECT_TRUE(gate.lockAdmission().accepting()); +} + +TEST(StopAllAdmissionGateTest, TestClearInvalidatesOutstandingTickets) +{ + StopAllAdmissionGate gate; + const auto stale = gate.beginStopAll(); + ASSERT_TRUE(stale.valid()); + + gate.clearForTesting(); + + EXPECT_TRUE(gate.lockAdmission().accepting()); + EXPECT_FALSE(gate.finishStopAll(stale, true)); +} + +} // namespace +} // namespace cmvr::service diff --git a/cmvr-es/service/stop_all/tests/stop_operation_dispatcher_test.cpp b/cmvr-es/service/stop_all/tests/stop_operation_dispatcher_test.cpp new file mode 100644 index 00000000..4f469dd7 --- /dev/null +++ b/cmvr-es/service/stop_all/tests/stop_operation_dispatcher_test.cpp @@ -0,0 +1,317 @@ +#include "service/stop_all/include/stop_operation_dispatcher.h" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cmvr::service { +namespace { + +using namespace std::chrono_literals; + +TEST(StopOperationDispatcherTest, RejectsEmptyKeysAndOperations) +{ + StopOperationDispatcher dispatcher; + StopOperationDispatcher::Operation empty_operation; + + const auto empty_key = dispatcher.submit("", [] { + return StopOperationDispatcher::OperationResult{true, {}}; + }); + const auto empty_callback = dispatcher.submit( + "arm:one", std::move(empty_operation)); + + EXPECT_FALSE(empty_key.valid()); + EXPECT_FALSE(empty_callback.valid()); + const auto invalid_result = empty_key.waitUntil( + StopOperationDispatcher::Clock::now()); + EXPECT_FALSE(invalid_result.completed); + EXPECT_FALSE(invalid_result.result); + EXPECT_FALSE(invalid_result.detail.empty()); +} + +TEST(StopOperationDispatcherTest, ReturnsOperationOutcome) +{ + StopOperationDispatcher dispatcher; + const auto handle = dispatcher.submit("arm:one", [] { + return StopOperationDispatcher::OperationResult{ + false, "driver did not confirm idle"}; + }); + + ASSERT_TRUE(handle.valid()); + const auto result = handle.waitUntil( + StopOperationDispatcher::Clock::now() + 1s); + EXPECT_TRUE(result.completed); + EXPECT_FALSE(result.result); + EXPECT_EQ(result.detail, "driver did not confirm idle"); +} + +TEST(StopOperationDispatcherTest, TimeoutDoesNotCancelTheJob) +{ + StopOperationDispatcher dispatcher; + std::promise release; + auto released = release.get_future().share(); + const auto handle = dispatcher.submit("agv:one", [released] { + released.wait(); + return StopOperationDispatcher::OperationResult{true, "stopped"}; + }); + + const auto timed_out = handle.waitUntil( + StopOperationDispatcher::Clock::now() + 20ms); + EXPECT_FALSE(timed_out.completed); + EXPECT_FALSE(timed_out.result); + + release.set_value(); + const auto completed = handle.waitUntil( + StopOperationDispatcher::Clock::now() + 1s); + EXPECT_TRUE(completed.completed); + EXPECT_TRUE(completed.result); + EXPECT_EQ(completed.detail, "stopped"); +} + +TEST(StopOperationDispatcherTest, RunningSubmissionsForAKeyShareOneJob) +{ + StopOperationDispatcher dispatcher; + std::promise started; + std::promise release; + auto released = release.get_future().share(); + std::atomic first_calls{0}; + std::atomic duplicate_calls{0}; + + const auto first = dispatcher.submit("arm:one", [&] { + ++first_calls; + started.set_value(); + released.wait(); + return StopOperationDispatcher::OperationResult{true, "first"}; + }); + ASSERT_EQ(started.get_future().wait_for(1s), std::future_status::ready); + + const auto duplicate = dispatcher.submit("arm:one", [&] { + ++duplicate_calls; + return StopOperationDispatcher::OperationResult{false, "duplicate"}; + }); + release.set_value(); + + const auto first_result = first.waitUntil( + StopOperationDispatcher::Clock::now() + 1s); + const auto duplicate_result = duplicate.waitUntil( + StopOperationDispatcher::Clock::now() + 1s); + EXPECT_TRUE(first_result.completed); + EXPECT_TRUE(first_result.result); + EXPECT_EQ(first_result.detail, "first"); + EXPECT_TRUE(duplicate_result.completed); + EXPECT_TRUE(duplicate_result.result); + EXPECT_EQ(duplicate_result.detail, "first"); + EXPECT_EQ(first_calls.load(), 1); + EXPECT_EQ(duplicate_calls.load(), 0); +} + +TEST(StopOperationDispatcherTest, ConcurrentSubmissionsForAKeyShareOneJob) +{ + StopOperationDispatcher dispatcher; + constexpr int submitter_count = 12; + std::promise release; + auto released = release.get_future().share(); + std::atomic operation_calls{0}; + std::mutex handles_mutex; + std::vector handles; + std::vector submitters; + handles.reserve(submitter_count); + submitters.reserve(submitter_count); + + for (int index = 0; index < submitter_count; ++index) { + submitters.emplace_back([&] { + auto handle = dispatcher.submit("dexhand:one", [&] { + ++operation_calls; + released.wait(); + return StopOperationDispatcher::OperationResult{true, {}}; + }); + std::lock_guard lock(handles_mutex); + handles.emplace_back(std::move(handle)); + }); + } + for (auto& submitter : submitters) { + submitter.join(); + } + release.set_value(); + + ASSERT_EQ(handles.size(), static_cast(submitter_count)); + const auto deadline = StopOperationDispatcher::Clock::now() + 1s; + for (const auto& handle : handles) { + const auto result = handle.waitUntil(deadline); + EXPECT_TRUE(result.completed); + EXPECT_TRUE(result.result); + } + EXPECT_EQ(operation_calls.load(), 1); +} + +TEST(StopOperationDispatcherTest, CompletedJobIsReapedBeforeNextSubmission) +{ + StopOperationDispatcher dispatcher; + std::atomic calls{0}; + + const auto first = dispatcher.submit("arm:one", [&] { + ++calls; + return StopOperationDispatcher::OperationResult{true, "round one"}; + }); + const auto first_result = first.waitUntil( + StopOperationDispatcher::Clock::now() + 1s); + ASSERT_TRUE(first_result.completed); + ASSERT_TRUE(first_result.result); + + const auto second = dispatcher.submit("arm:one", [&] { + ++calls; + return StopOperationDispatcher::OperationResult{true, "round two"}; + }); + const auto second_result = second.waitUntil( + StopOperationDispatcher::Clock::now() + 1s); + + EXPECT_TRUE(second_result.completed); + EXPECT_TRUE(second_result.result); + EXPECT_EQ(second_result.detail, "round two"); + EXPECT_EQ(calls.load(), 2); + EXPECT_EQ(first.waitUntil( + StopOperationDispatcher::Clock::now()).detail, "round one"); +} + +TEST(StopOperationDispatcherTest, SubmissionReapsAllCompletedJobs) +{ + StopOperationDispatcher dispatcher; + constexpr int completed_job_count = 24; + std::promise release; + auto released = release.get_future().share(); + std::vector handles; + handles.reserve(completed_job_count); + + for (int index = 0; index < completed_job_count; ++index) { + handles.emplace_back(dispatcher.submit( + "arm:" + std::to_string(index), [released] { + released.wait(); + return StopOperationDispatcher::OperationResult{true, {}}; + })); + } + ASSERT_EQ(dispatcher.jobCountForTesting(), + static_cast(completed_job_count)); + + release.set_value(); + const auto deadline = StopOperationDispatcher::Clock::now() + 1s; + for (const auto& handle : handles) { + ASSERT_TRUE(handle.waitUntil(deadline).completed); + } + ASSERT_EQ(dispatcher.jobCountForTesting(), + static_cast(completed_job_count)); + + std::promise trigger_started; + std::promise release_trigger; + auto trigger_released = release_trigger.get_future().share(); + const auto trigger = dispatcher.submit( + "agv:cleanup-trigger", [&] { + trigger_started.set_value(); + trigger_released.wait(); + return StopOperationDispatcher::OperationResult{true, {}}; + }); + + ASSERT_EQ(trigger_started.get_future().wait_for(1s), + std::future_status::ready); + EXPECT_EQ(dispatcher.jobCountForTesting(), 1U); + + release_trigger.set_value(); + EXPECT_TRUE(trigger.waitUntil( + StopOperationDispatcher::Clock::now() + 1s).result); +} + +TEST(StopOperationDispatcherTest, DifferentKeysRunIndependently) +{ + StopOperationDispatcher dispatcher; + std::promise release_first; + auto first_released = release_first.get_future().share(); + std::promise second_started; + + const auto first = dispatcher.submit("camera:one", [first_released] { + first_released.wait(); + return StopOperationDispatcher::OperationResult{true, {}}; + }); + const auto second = dispatcher.submit("speaker:one", [&] { + second_started.set_value(); + return StopOperationDispatcher::OperationResult{true, {}}; + }); + + EXPECT_EQ( + second_started.get_future().wait_for(1s), + std::future_status::ready); + const auto second_result = second.waitUntil( + StopOperationDispatcher::Clock::now() + 1s); + EXPECT_TRUE(second_result.completed); + EXPECT_TRUE(second_result.result); + + release_first.set_value(); + EXPECT_TRUE(first.waitUntil( + StopOperationDispatcher::Clock::now() + 1s).result); +} + +TEST(StopOperationDispatcherTest, ConvertsOperationExceptionsToFailures) +{ + StopOperationDispatcher dispatcher; + const auto standard = dispatcher.submit("arm:one", []() + -> StopOperationDispatcher::OperationResult { + throw std::runtime_error("transport failed"); + }); + const auto unknown = dispatcher.submit("agv:one", []() + -> StopOperationDispatcher::OperationResult { + throw 42; + }); + + const auto deadline = StopOperationDispatcher::Clock::now() + 1s; + const auto standard_result = standard.waitUntil(deadline); + const auto unknown_result = unknown.waitUntil(deadline); + EXPECT_TRUE(standard_result.completed); + EXPECT_FALSE(standard_result.result); + EXPECT_NE(standard_result.detail.find("transport failed"), + std::string::npos); + EXPECT_TRUE(unknown_result.completed); + EXPECT_FALSE(unknown_result.result); + EXPECT_NE(unknown_result.detail.find("unknown exception"), + std::string::npos); +} + +TEST(StopOperationDispatcherTest, + DestructorWaitsForOutstandingWorkers) +{ + auto dispatcher = std::make_unique(); + std::promise started; + std::promise release; + auto released = release.get_future().share(); + const auto handle = dispatcher->submit("arm:one", [&] { + started.set_value(); + released.wait(); + return StopOperationDispatcher::OperationResult{ + true, "completed after dispatcher destruction"}; + }); + ASSERT_EQ(started.get_future().wait_for(1s), std::future_status::ready); + + auto destroy = std::async(std::launch::async, [&] { + dispatcher.reset(); + }); + EXPECT_EQ(destroy.wait_for(20ms), std::future_status::timeout); + + release.set_value(); + EXPECT_EQ(destroy.wait_for(1s), std::future_status::ready); + destroy.get(); + const auto result = handle.waitUntil( + StopOperationDispatcher::Clock::now() + 1s); + EXPECT_TRUE(result.completed); + EXPECT_TRUE(result.result); + EXPECT_EQ(result.detail, "completed after dispatcher destruction"); +} + +} // namespace +} // namespace cmvr::service diff --git a/cmvr-es/task/CMakeLists.txt b/cmvr-es/task/CMakeLists.txt index e16bbb5f..d9952ab0 100644 --- a/cmvr-es/task/CMakeLists.txt +++ b/cmvr-es/task/CMakeLists.txt @@ -12,13 +12,44 @@ target_link_libraries(task cmvr_es::ik_solver cmvr_es::base_motion cmvr_es::self_collision_checker + cmvr_es::control_authority PRIVATE cmvr_es::device_manager + cmvr_es::stop_all_admission_gate + cmvr_es::camera_operational_activity_registry ) add_library(cmvr_es::task ALIAS task) install(TARGETS task LIBRARY DESTINATION lib) +if(BUILD_TESTING) + add_executable(touch_screen_admission_test + touch_screen_task/src/touch_screen_admission_test.cpp + ) + target_link_libraries(touch_screen_admission_test PRIVATE + cmvr_es::task + cmvr_es::device_manager + cmvr_es::stop_all_admission_gate + cmvr_es::camera_operational_activity_registry + gtest + gtest_main + pthread + ) + add_test( + NAME touch_screen_admission_test + COMMAND touch_screen_admission_test + ) + set(_touch_screen_admission_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}") + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _touch_screen_admission_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(touch_screen_admission_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_touch_screen_admission_test_environment}") +endif() + #add_executable(touch_screen_task_test # touch_screen_task/src/touch_screen_task_test.cpp #) 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 fb3efbcc..bf8a5513 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 @@ -24,6 +24,10 @@ public: const std::string& id() const override { return id_; } TaskRunMode runMode() const override { return TaskRunMode::BLOCKING_SERVICE; } + TaskShutdownPhase shutdownPhase() const override + { + return TaskShutdownPhase::COMMAND_INGRESS; + } bool init() override; bool start() override; bool step(double dt) override; 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 74815d2a..34325bd9 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 @@ -316,6 +316,10 @@ void GrpcServerTask::waitLoop() void GrpcServerTask::clearServices() { + // SystemService owns StopAll workers which can still be draining calls + // into operational backends after an RPC deadline. Join them before any + // peer service releases its activity registrations or backend state. + system_service_.reset(); hlc_service_.reset(); agv_service_.reset(); motor_service_.reset(); @@ -325,7 +329,6 @@ void GrpcServerTask::clearServices() dexhand_service_.reset(); microphone_service_.reset(); speaker_service_.reset(); - system_service_.reset(); camera_service_.reset(); } diff --git a/cmvr-es/task/quic_edge_task/CMakeLists.txt b/cmvr-es/task/quic_edge_task/CMakeLists.txt index 48d66107..8b2db539 100644 --- a/cmvr-es/task/quic_edge_task/CMakeLists.txt +++ b/cmvr-es/task/quic_edge_task/CMakeLists.txt @@ -9,13 +9,17 @@ target_link_libraries(quic_edge_task cmvr_es::proto cmvr_es::logging cmvr_es::device_manager + cmvr_es::stop_all_admission_gate ) add_library(cmvr_es::quic_edge_task ALIAS quic_edge_task) if(BUILD_TESTING) add_executable(quic_edge_task_test tests/quic_edge_task_test.cpp) target_compile_features(quic_edge_task_test PRIVATE cxx_std_17) - target_link_libraries(quic_edge_task_test PRIVATE cmvr_es::quic_edge_task) + target_link_libraries(quic_edge_task_test PRIVATE + cmvr_es::quic_edge_task + cmvr_es::stop_all_admission_gate + ) add_test(NAME quic_edge_task_test COMMAND quic_edge_task_test) if(UNIX AND NOT APPLE) get_property(_quic_task_test_library_dirs DIRECTORY PROPERTY LINK_DIRECTORIES) diff --git a/cmvr-es/task/quic_edge_task/include/quic_edge_task.h b/cmvr-es/task/quic_edge_task/include/quic_edge_task.h index b5c760b5..8e10575d 100644 --- a/cmvr-es/task/quic_edge_task/include/quic_edge_task.h +++ b/cmvr-es/task/quic_edge_task/include/quic_edge_task.h @@ -22,6 +22,7 @@ public: bool start() override; bool step(double dt) override; void stop() override; + bool stopActivity() override; TaskState state() const override; bool isBusy() const override; diff --git a/cmvr-es/task/quic_edge_task/src/quic_edge_task.cpp b/cmvr-es/task/quic_edge_task/src/quic_edge_task.cpp index 7d32ecde..941615e7 100644 --- a/cmvr-es/task/quic_edge_task/src/quic_edge_task.cpp +++ b/cmvr-es/task/quic_edge_task/src/quic_edge_task.cpp @@ -5,6 +5,7 @@ #include "cmvr/config/task_manager_config/task_manager_config.pb.h" #include "common/base/logging/logger.h" +#include "service/stop_all/include/stop_all_admission_gate.h" #include "common/config/config_files.h" #include "manager/device_manager/include/device_manager.h" #include "task/task_factory.h" @@ -82,23 +83,52 @@ bool QuicEdgeTask::init() bool QuicEdgeTask::start() { - std::lock_guard lock(mutex_); - if (state_ == TaskState::RUNNING) return true; - if ((state_ != TaskState::IDLE && state_ != TaskState::STOPPED) || !service_) { - last_error_ = "QUIC edge task is not initialized"; - state_ = TaskState::FAILED; - return false; + auto& admission_gate = service::globalStopAllAdmissionGate(); + std::uint64_t admission_generation = 0U; + { + auto admission = admission_gate.lockAdmission(); + if (!admission.accepting()) { + return false; + } + admission_generation = admission.generation(); } - std::string error; - if (!service_->start(&error)) { - last_error_ = std::move(error); - state_ = TaskState::FAILED; - return false; + + { + std::lock_guard lock(mutex_); + if (state_ == TaskState::RUNNING) { + auto admission = admission_gate.lockAdmission(); + return admission.accepting() && + admission.generation() == admission_generation; + } + if ((state_ != TaskState::IDLE && state_ != TaskState::STOPPED) || + !service_) { + last_error_ = "QUIC edge task is not initialized"; + state_ = TaskState::FAILED; + return false; + } + std::string error; + if (!service_->start(&error)) { + last_error_ = std::move(error); + state_ = TaskState::FAILED; + return false; + } + last_error_.clear(); + state_ = TaskState::RUNNING; + CMVR_LOG(INFO) << "[QuicEdgeTask] Started, id=" << id_; } - last_error_.clear(); - state_ = TaskState::RUNNING; - CMVR_LOG(INFO) << "[QuicEdgeTask] Started, id=" << id_; - return true; + + bool admission_current = false; + { + auto admission = admission_gate.lockAdmission(); + admission_current = admission.accepting() && + admission.generation() == admission_generation; + } + if (admission_current) { + return true; + } + + stop(); + return false; } bool QuicEdgeTask::step(const double dt) @@ -110,12 +140,23 @@ bool QuicEdgeTask::step(const double dt) void QuicEdgeTask::stop() { std::lock_guard lock(mutex_); - // Keep ownership stable for the complete stop. init() may replace the - // unique service instance and therefore must not race a raw pointer here. + // stop() is the task lifecycle terminator used by TaskManager shutdown. + // Unlike StopAll's stopActivity(), it intentionally tears down transport. if (service_) service_->stop(); if (state_ != TaskState::FAILED) state_ = TaskState::STOPPED; } +bool QuicEdgeTask::stopActivity() +{ + std::lock_guard lock(mutex_); + if (!service_ || state_ == TaskState::FAILED) { + return false; + } + // System StopAll must preserve the QUIC presence channel. Only old media + // subscriptions are fenced; registration and heartbeat stay online. + return service_->interruptMediaActivities(); +} + TaskState QuicEdgeTask::mappedState() const { if (!service_ || state_ != TaskState::RUNNING) return state_; diff --git a/cmvr-es/task/quic_edge_task/tests/quic_edge_task_test.cpp b/cmvr-es/task/quic_edge_task/tests/quic_edge_task_test.cpp index 3157e4b6..e0c1b0b4 100644 --- a/cmvr-es/task/quic_edge_task/tests/quic_edge_task_test.cpp +++ b/cmvr-es/task/quic_edge_task/tests/quic_edge_task_test.cpp @@ -1,9 +1,44 @@ #include +#include "service/stop_all/include/stop_all_admission_gate.h" #include "task/quic_edge_task/include/quic_edge_task.h" +namespace { + +cmvr::config::QuicEdgeConfig validConfig() +{ + cmvr::config::QuicEdgeConfig config; + config.set_id("quic-admission-test"); + config.set_server_host("127.0.0.1"); + config.set_server_port(4433U); + config.set_alpn("cmvr-quic-edge/1"); + config.set_node_id("test-node"); + config.set_robot_id("test-robot"); + config.set_software_version("test-version"); + config.set_grpc_endpoint_host("127.0.0.1"); + config.set_grpc_endpoint_port(50052U); + config.set_heartbeat_interval_ms(250U); + config.set_control_response_timeout_ms(100U); + config.mutable_tls()->set_allow_insecure(true); + config.mutable_reconnect()->set_initial_delay_ms(5U); + config.mutable_reconnect()->set_maximum_delay_ms(20U); + config.mutable_reconnect()->set_multiplier(2.0); + config.mutable_reconnect()->set_connect_timeout_ms(20U); + config.set_maximum_datagram_bytes(1200U); + config.set_maximum_control_frame_bytes(4096U); + config.set_maximum_frame_bytes(32U * 1024U); + config.set_datagram_send_queue_depth(32U); + config.set_media_poll_interval_ms(1U); + return config; +} + +} // namespace + int main() { + auto& admission = cmvr::service::globalStopAllAdmissionGate(); + admission.clearForTesting(); + cmvr::config::QuicEdgeConfig config; config.set_id("quic-invalid-config-test"); @@ -17,6 +52,52 @@ int main() std::cerr << "failed QUIC task did not preserve its failure state\n"; return 1; } + if (task.stopActivity()) { + std::cerr << "failed QUIC task reported a confirmed activity stop\n"; + return 1; + } + + cmvr::task::QuicEdgeTask admission_task(validConfig()); + if (!admission_task.init()) { + std::cerr << "valid QUIC admission task did not initialize\n"; + return 1; + } + auto ticket = admission.beginStopAll(); + if (admission_task.start() || admission_task.isBusy()) { + std::cerr << "QUIC public start bypassed closed StopAll admission\n"; + return 1; + } + if (!admission.finishStopAll(ticket, true) || + !admission_task.start() || !admission_task.isBusy()) { + std::cerr << "QUIC public start was not restored after StopAll\n"; + return 1; + } + if (!admission_task.stopActivity()) { + std::cerr << "restarted QUIC activity did not stop cleanly\n"; + return 1; + } + if (!admission_task.isBusy() || + admission_task.state() != cmvr::task::TaskState::RUNNING) { + std::cerr << "QUIC activity stop terminated the service lifecycle\n"; + return 1; + } + + ticket = admission.beginStopAll(); + if (admission.finishStopAll(ticket, false) || admission_task.start()) { + std::cerr << "failed StopAll did not keep QUIC start fail-closed\n"; + return 1; + } + ticket = admission.beginStopAll(); + if (!admission.finishStopAll(ticket, true) || + !admission_task.start() || !admission_task.stopActivity()) { + std::cerr << "successful StopAll did not restore QUIC restart\n"; + return 1; + } + if (!admission_task.isBusy()) { + std::cerr << "QUIC service did not remain available after StopAll\n"; + return 1; + } + admission.clearForTesting(); std::cout << "quic_edge_task_test: PASS\n"; return 0; } diff --git a/cmvr-es/task/task.h b/cmvr-es/task/task.h index a6786f4e..571fad9b 100644 --- a/cmvr-es/task/task.h +++ b/cmvr-es/task/task.h @@ -19,17 +19,30 @@ enum class TaskRunMode { BLOCKING_SERVICE }; +enum class TaskShutdownPhase { + COMMAND_INGRESS = 0, + DEPENDENT_ACTIVITY +}; + class Task { public: virtual ~Task() = default; virtual const std::string& id() const = 0; virtual TaskRunMode runMode() const { return TaskRunMode::PERIODIC_STEP; } + virtual TaskShutdownPhase shutdownPhase() const + { + return TaskShutdownPhase::DEPENDENT_ACTIVITY; + } virtual bool init() = 0; virtual bool start() { return true; } virtual bool step(double dt) = 0; virtual void stop() = 0; + // Cancels current command-driven activity while preserving task lifecycle. + // Tasks with no separately stoppable activity remain a successful no-op. + virtual bool stopActivity() { return true; } + virtual TaskState state() const = 0; virtual bool isBusy() const = 0; virtual bool isFinished() const = 0; 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 6e284d7e..76ae242b 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 @@ -4,7 +4,9 @@ #define CMVR_ES_TOUCH_SCREEN_TASK_H #include +#include #include +#include #include #include #include @@ -17,12 +19,16 @@ #include "devices/camera/abstract_camera.h" #include "devices/dexhand/abstract_dexhand.h" #include "devices/arm/robot_arm.h" +#include "manager/control_authority/include/control_authority_manager.h" #include "task/task.h" #include "algorithms/perception/apriltag/include/apriltag_perception.h" #include "algorithms/perception/apriltag/include/tag_relative_target_3d.h" namespace cmvr::task { +class TouchScreenTaskStopActivityTestPeer; +class TouchScreenTaskAdmissionTestPeer; + class TouchScreenTask : public Task { public: enum class Phase { @@ -70,11 +76,22 @@ public: const std::string& id() const override { return id_; } + bool touchIfCurrent( + int u, + int v, + const std::function& still_admitted); bool touch(int u, int v); bool startFromPixel(int u, int v); bool step(double dt) override; void stop() override; + // Stops only the current touch operation. The task remains initialized + // and can accept another touch after StopAll admission reopens. Returns + // true only after no old step can submit another arm command. When this + // task still owns control it also confirms the arm stop; when a safety + // barrier already displaced the task, that barrier owns the physical stop. + bool stopActivity() override; + Phase phase() const; Status lastStatus() const; static const char* phaseToString(Phase phase); @@ -99,12 +116,23 @@ public: const IbvsController& ibvs() const { return ibvs_; } private: + friend class TouchScreenTaskStopActivityTestPeer; + friend class TouchScreenTaskAdmissionTestPeer; + using Clock = std::chrono::steady_clock; static bool validateConfig(const cmvr::config::TouchScreenTaskConfig& config); bool isBusyUnlocked() const; + bool beginActivityIfCurrent(std::uint64_t activity_generation); + bool acquireActivityControlUnlocked(); + control::ControlLeaseToken activityControlToken() const; + bool activityControlCurrent() const; + std::function activityCancellationRequested() const; + control::ControlDispatchGuard tryBeginActivityDispatch() const; + void releaseActivityControlUnlocked() noexcept; + void finishActivityUnlocked(Phase phase, Status status) noexcept; bool startFromPixelUnlocked(int u, int v); - void stopUnlocked(); + void resetActivityUnlocked(); bool applyConfig(); bool validateControlJointNames() const; bool stepAligning(double dt); @@ -132,8 +160,11 @@ private: private: mutable std::mutex mutex_; + mutable std::mutex activity_arm_mutex_; + mutable std::mutex activity_control_mutex_; std::string id_; std::shared_ptr arm_{nullptr}; + std::shared_ptr activity_arm_{nullptr}; std::shared_ptr dexhand_{nullptr}; std::shared_ptr camera_{nullptr}; @@ -148,6 +179,10 @@ private: Status last_status_{Status::NOT_INITIALIZED}; bool initialized_{false}; + std::atomic stop_requested_{false}; + std::atomic activity_active_{false}; + std::atomic activity_generation_{1U}; + control::ControlLeaseToken activity_control_token_; 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 new file mode 100644 index 00000000..2a78ed1c --- /dev/null +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_admission_test.cpp @@ -0,0 +1,463 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "manager/device_manager/include/device_manager.h" +#include "service/grpc/include/camera_operational_activity_registry.h" +#include "service/stop_all/include/stop_all_admission_gate.h" +#include "task/touch_screen_task/include/touch_screen_task.h" + +namespace cmvr::task { + +class TouchScreenTaskAdmissionTestPeer final { +public: + static void prepare( + TouchScreenTask& task, + const std::shared_ptr& arm, + const std::shared_ptr& camera) + { + std::lock_guard lock(task.mutex_); + task.arm_ = arm; + { + std::lock_guard arm_lock(task.activity_arm_mutex_); + task.activity_arm_ = arm; + } + task.camera_ = camera; + task.initialized_ = true; + task.phase_ = TouchScreenTask::Phase::IDLE; + task.last_status_ = TouchScreenTask::Status::IDLE; + } + + static void prepareInit( + TouchScreenTask& task, + const std::string& arm_id, + const std::string& dexhand_id, + const std::string& camera_id) + { + std::lock_guard lock(task.mutex_); + task.config_valid_ = true; + auto* devices = task.config_.mutable_devices(); + devices->set_arm_id(arm_id); + devices->set_dexhand_id(dexhand_id); + devices->set_camera_id(camera_id); + task.initialized_ = false; + task.phase_ = TouchScreenTask::Phase::IDLE; + task.last_status_ = TouchScreenTask::Status::NOT_INITIALIZED; + } +}; + +} // namespace cmvr::task + +namespace { + +class AdmissionRobotArm final : public cmvr::device::RobotArm { +public: + AdmissionRobotArm() { id_ = "touch-admission-arm"; } + + std::string typeName() const override { return "AdmissionRobotArm"; } + cmvr::device::RobotModel getRobotModel() const override { return {}; } + std::size_t getDof() const override { return 0U; } + cmvr::device::ArmState getRobotState() const override { return {}; } + cmvr::device::JointGroupState getJointState() const override { return {}; } + cmvr::device::CartesianPose getTcpPose( + cmvr::device::FrameType = cmvr::device::FrameType::Base) const override + { + return {}; + } + cmvr::device::RobotMode getRobotMode() const override + { + return cmvr::device::RobotMode::Idle; + } + cmvr::device::SafetyMode getSafetyMode() const override + { + return cmvr::device::SafetyMode::Normal; + } + cmvr::device::ControlMode getControlMode() const override + { + return cmvr::device::ControlMode::None; + } + cmvr::device::Result torqueOn() override { return success(); } + cmvr::device::Result torqueOff() override { return success(); } + cmvr::device::Result calibrateZeroQ(const std::string&) override + { + return success(); + } + cmvr::device::Result emergencyStop() override { return success(); } + cmvr::device::Result protectiveStop() override { return success(); } + cmvr::device::Result setSpeedScaling(double) override { return success(); } + double getSpeedScaling() const override { return 1.0; } + bool isProtectiveStopped() const override { return false; } + bool isEmergencyStopped() const override { return false; } + bool isFault() const override { return false; } + cmvr::device::Result moveJ( + const cmvr::device::JointPositionCommand&, + const cmvr::device::MotionOptions&) override + { + return success(); + } + cmvr::device::Result speedJ( + const cmvr::device::JointVelocityCommand&, double, double) override + { + return success(); + } + cmvr::device::Result stopJ(double) override { return success(); } + cmvr::device::Result moveL( + const cmvr::device::CartesianPose&, + const cmvr::device::MotionOptions&, + cmvr::device::FrameType = cmvr::device::FrameType::Base) override + { + return success(); + } + cmvr::device::Result speedL( + const cmvr::device::CartesianVelocity&, + double, + double, + cmvr::device::FrameType = cmvr::device::FrameType::Base) override + { + return success(); + } + cmvr::device::Result stopL(std::optional = std::nullopt) override + { + return success(); + } + cmvr::device::Result stopMotion() override { return success(); } + cmvr::device::Result startServoMode( + const cmvr::device::ServoOptions&) override + { + return success(); + } + cmvr::device::Result servoJ( + const cmvr::device::JointPositionCommand&) override + { + return success(); + } + cmvr::device::Result servoL( + const cmvr::device::CartesianPose&, + cmvr::device::FrameType = cmvr::device::FrameType::Base) override + { + return success(); + } + cmvr::device::Result servoSpeedJ( + const cmvr::device::JointVelocityCommand&) override + { + return success(); + } + cmvr::device::Result servoSpeedL( + const cmvr::device::CartesianVelocity&, + cmvr::device::FrameType = cmvr::device::FrameType::Base) override + { + return success(); + } + cmvr::device::Result stopServoMode() override { return success(); } + cmvr::device::Result connect(const std::string&, int) override + { + return success(); + } + cmvr::device::Result disconnect() override { return success(); } + bool isConnected() const override { return true; } + cmvr::device::Result powerOn() override { return success(); } + cmvr::device::Result powerOff() override { return success(); } + cmvr::device::Result brakeRelease() override { return success(); } + cmvr::device::Result shutdown() override { return success(); } + cmvr::device::Result clearFault() override { return success(); } + cmvr::device::Result unlockProtectiveStop() override { return success(); } + cmvr::device::Result loadProgram(const std::string&) override + { + return success(); + } + cmvr::device::Result playProgram() override { return success(); } + cmvr::device::Result pauseProgram() override { return success(); } + cmvr::device::Result stopProgram() override { return success(); } + std::vector ik( + const std::string&, + const std::string&, + const cmvr::device::CartesianPose&) override + { + return {}; + } + std::shared_ptr kinematicsSolver() const override + { + return nullptr; + } + cmvr::device::CartesianPose fk( + const std::string&, const std::string&) override + { + return {}; + } + cmvr::device::CartesianPose fk(bool = true) override { return {}; } + cmvr::device::CartesianVelocity getSpeedLCommandTwistBase() const override + { + return {}; + } + bool busy() const override { return false; } + +private: + static cmvr::device::Result success() + { + return cmvr::device::Result::success(); + } +}; + +class AdmissionCamera final : public cmvr::device::AbstractCamera { +public: + explicit AdmissionCamera(std::string id) + { + id_ = std::move(id); + state_.is_initialized = true; + } + + std::string typeName() const override { return "AdmissionCamera"; } + + void getState(cmvr::device::CameraState& state) override + { + std::lock_guard lock(mutex_); + state = state_; + } + + bool start() override + { + ++lifecycle_start_calls_; + return true; + } + + bool stop() override + { + ++lifecycle_stop_calls_; + std::lock_guard lock(mutex_); + operational_active_ = false; + state_.is_opened = false; + return true; + } + + bool startOperationalActivity() override + { + ++operational_start_calls_; + std::unique_lock lock(mutex_); + start_entered_ = true; + condition_.notify_all(); + condition_.wait(lock, [this] { return !block_start_; }); + operational_active_ = true; + state_.is_opened = true; + return true; + } + + bool stopOperationalActivity() override + { + ++operational_stop_calls_; + std::lock_guard lock(mutex_); + operational_active_ = false; + state_.is_opened = false; + return true; + } + + void blockStart() + { + std::lock_guard lock(mutex_); + block_start_ = true; + start_entered_ = false; + } + + void waitForStartEntered() + { + std::unique_lock lock(mutex_); + condition_.wait(lock, [this] { return start_entered_; }); + } + + void releaseStart() + { + { + std::lock_guard lock(mutex_); + block_start_ = false; + } + condition_.notify_all(); + } + + bool operationalActive() const + { + std::lock_guard lock(mutex_); + return operational_active_; + } + + int lifecycleStartCalls() const { return lifecycle_start_calls_; } + int lifecycleStopCalls() const { return lifecycle_stop_calls_; } + int operationalStartCalls() const { return operational_start_calls_; } + int operationalStopCalls() const { return operational_stop_calls_; } + +private: + mutable std::mutex mutex_; + std::condition_variable condition_; + bool block_start_{false}; + bool start_entered_{false}; + bool operational_active_{false}; + std::atomic lifecycle_start_calls_{0}; + std::atomic lifecycle_stop_calls_{0}; + std::atomic operational_start_calls_{0}; + std::atomic operational_stop_calls_{0}; +}; + +class TouchScreenAdmissionTest : public ::testing::Test { +protected: + void SetUp() override + { + cmvr::service::globalCameraOperationalActivityRegistry() + .clearForTesting(); + cmvr::device::DeviceManager::destroyInstance(); + cmvr::service::globalStopAllAdmissionGate().clearForTesting(); + cmvr::control::ControlAuthorityManager::instance().clear(); + } + + void TearDown() override + { + cmvr::service::globalCameraOperationalActivityRegistry() + .clearForTesting(); + cmvr::device::DeviceManager::destroyInstance(); + cmvr::service::globalStopAllAdmissionGate().clearForTesting(); + cmvr::control::ControlAuthorityManager::instance().clear(); + } +}; + +TEST_F(TouchScreenAdmissionTest, + DirectTouchAndStartFromPixelHonorFailClosedAdmissionAndRecovery) +{ + auto arm = std::make_shared(); + auto camera = std::make_shared( + "touch-admission-camera"); + cmvr::task::TouchScreenTask task(cmvr::config::TouchScreenTaskConfig{}); + cmvr::task::TouchScreenTaskAdmissionTestPeer::prepare( + task, arm, camera); + auto& gate = cmvr::service::globalStopAllAdmissionGate(); + + auto ticket = gate.beginStopAll(); + EXPECT_FALSE(task.touch(10, 20)); + EXPECT_FALSE(task.startFromPixel(10, 20)); + EXPECT_FALSE(task.isBusy()); + + EXPECT_TRUE(gate.finishStopAll(ticket, true)); + ASSERT_TRUE(task.touch(10, 20)); + EXPECT_TRUE(task.isBusy()); + EXPECT_TRUE(task.stopActivity()); + + ticket = gate.beginStopAll(); + EXPECT_FALSE(gate.finishStopAll(ticket, false)); + EXPECT_FALSE(task.touch(10, 20)); + EXPECT_FALSE(task.startFromPixel(10, 20)); + + ticket = gate.beginStopAll(); + EXPECT_TRUE(gate.finishStopAll(ticket, true)); + EXPECT_TRUE(task.startFromPixel(30, 40)); + EXPECT_TRUE(task.stopActivity()); +} + +TEST_F(TouchScreenAdmissionTest, + StopAllStopsCameraActivityAndNextTouchRestartsIt) +{ + auto arm = std::make_shared(); + auto camera = std::make_shared( + "touch-restart-camera"); + cmvr::task::TouchScreenTask task(cmvr::config::TouchScreenTaskConfig{}); + cmvr::task::TouchScreenTaskAdmissionTestPeer::prepare( + task, arm, camera); + auto& gate = cmvr::service::globalStopAllAdmissionGate(); + auto& registry = + cmvr::service::globalCameraOperationalActivityRegistry(); + + ASSERT_TRUE(task.touch(10, 20)); + EXPECT_TRUE(camera->operationalActive()); + EXPECT_EQ(camera->operationalStartCalls(), 1); + + const auto ticket = gate.beginStopAll(); + ASSERT_TRUE(ticket.valid()); + EXPECT_TRUE(task.stopActivity()); + EXPECT_TRUE(registry.stopAllActivities()); + EXPECT_FALSE(camera->operationalActive()); + EXPECT_EQ(camera->operationalStopCalls(), 1); + EXPECT_EQ(camera->lifecycleStopCalls(), 0); + ASSERT_TRUE(gate.finishStopAll(ticket, true)); + + ASSERT_TRUE(task.touch(30, 40)); + EXPECT_TRUE(camera->operationalActive()); + EXPECT_EQ(camera->operationalStartCalls(), 2); + EXPECT_EQ(camera->lifecycleStartCalls(), 0); + EXPECT_TRUE(task.stopActivity()); +} + +TEST_F(TouchScreenAdmissionTest, + TouchRollsBackCameraStartThatCrossesStopAllGeneration) +{ + auto arm = std::make_shared(); + auto camera = std::make_shared( + "touch-racing-camera"); + cmvr::task::TouchScreenTask task(cmvr::config::TouchScreenTaskConfig{}); + cmvr::task::TouchScreenTaskAdmissionTestPeer::prepare( + task, arm, camera); + camera->blockStart(); + + bool touch_result = true; + std::thread touch_thread([&] { + touch_result = task.touch(10, 20); + }); + camera->waitForStartEntered(); + + auto& gate = cmvr::service::globalStopAllAdmissionGate(); + const auto ticket = gate.beginStopAll(); + EXPECT_TRUE(ticket.valid()); + camera->releaseStart(); + touch_thread.join(); + + EXPECT_FALSE(touch_result); + EXPECT_FALSE(task.isBusy()); + EXPECT_FALSE(camera->operationalActive()); + EXPECT_EQ(camera->operationalStartCalls(), 1); + EXPECT_EQ(camera->operationalStopCalls(), 1); + EXPECT_EQ(camera->lifecycleStopCalls(), 0); + EXPECT_TRUE(gate.finishStopAll(ticket, true)); +} + +TEST_F(TouchScreenAdmissionTest, + InitRollsBackCameraStartThatCrossesStopAllGeneration) +{ + cmvr::config::DeviceManagerConfig manager_config; + auto& manager = + cmvr::device::DeviceManager::getInstance(manager_config); + auto camera = std::make_shared( + "touch-init-racing-camera"); + manager.registerDevice(camera); + + cmvr::task::TouchScreenTask task(cmvr::config::TouchScreenTaskConfig{}); + cmvr::task::TouchScreenTaskAdmissionTestPeer::prepareInit( + task, + "missing-arm", + "missing-dexhand", + camera->id()); + camera->blockStart(); + + bool init_result = true; + std::thread init_thread([&] { + init_result = task.init(); + }); + camera->waitForStartEntered(); + + auto& gate = cmvr::service::globalStopAllAdmissionGate(); + const auto ticket = gate.beginStopAll(); + EXPECT_TRUE(ticket.valid()); + camera->releaseStart(); + init_thread.join(); + + EXPECT_FALSE(init_result); + EXPECT_EQ(task.state(), cmvr::task::TaskState::UNINITIALIZED); + EXPECT_FALSE(camera->operationalActive()); + EXPECT_EQ(camera->operationalStartCalls(), 1); + EXPECT_EQ(camera->operationalStopCalls(), 1); + EXPECT_EQ(camera->lifecycleStopCalls(), 0); + EXPECT_TRUE(gate.finishStopAll(ticket, true)); +} + +} // namespace 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 7640cc33..d3a739a4 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 @@ -12,6 +12,8 @@ #include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h" #include "cmvr/config/touch_screen_algorithm_config.pb.h" #include "manager/device_manager/include/device_manager.h" +#include "service/grpc/include/camera_operational_activity_registry.h" +#include "service/stop_all/include/stop_all_admission_gate.h" #include namespace cmvr::task { @@ -277,6 +279,17 @@ TouchScreenTask::TouchScreenTask(const cmvr::config::TouchScreenTaskConfig& cfg) } bool TouchScreenTask::init() { + auto& admission_gate = service::globalStopAllAdmissionGate(); + std::uint64_t admission_generation = 0U; + { + auto admission = admission_gate.lockAdmission(); + if (!admission.accepting()) { + last_status_ = Status::NOT_INITIALIZED; + return false; + } + admission_generation = admission.generation(); + } + if (!config_valid_) { last_status_ = Status::INVALID_CONFIG; return false; @@ -294,12 +307,73 @@ bool TouchScreenTask::init() { auto arm = dm.getDevice(devices.arm_id()); auto dexhand = dm.getDevice(devices.dexhand_id()); auto camera = dm.getDevice(devices.camera_id()); - if (!camera || !camera->start()) { + if (!camera) { CMVR_LOG(ERROR) << "[TouchScreenTask] Failed to start camera: " << devices.camera_id(); last_status_ = Status::NOT_INITIALIZED; return false; } - return init(arm, dexhand, camera); + + auto& camera_registry = + service::globalCameraOperationalActivityRegistry(); + service::CameraOperationalActivityRegistry::ActivityToken camera_token; + service::CameraOperationalActivityRegistry::DispatchResult camera_start; + try { + camera_start = camera_registry.start( + devices.camera_id(), camera, &camera_token); + } catch (...) { + last_status_ = Status::NOT_INITIALIZED; + return false; + } + if (camera_start != service::CameraOperationalActivityRegistry:: + DispatchResult::Success) { + CMVR_LOG(ERROR) << "[TouchScreenTask] Failed to start camera: " + << devices.camera_id(); + last_status_ = Status::NOT_INITIALIZED; + return false; + } + + const auto admission_current = [&] { + auto admission = admission_gate.lockAdmission(); + return admission.accepting() && + admission.generation() == admission_generation; + }; + const auto rollback_camera = [&] { + if (camera_registry.stopIfCurrent(camera_token)) { + return; + } + const auto ticket = admission_gate.beginStopAll(); + (void)admission_gate.finishStopAll(ticket, false); + }; + const auto mark_interrupted = [this] { + std::lock_guard lock(mutex_); + initialized_ = false; + last_status_ = Status::NOT_INITIALIZED; + }; + + if (!admission_current()) { + rollback_camera(); + mark_interrupted(); + return false; + } + + bool initialized = false; + try { + initialized = init(arm, dexhand, camera); + } catch (...) { + rollback_camera(); + throw; + } + if (!initialized) { + rollback_camera(); + return false; + } + if (admission_current()) { + return true; + } + + rollback_camera(); + mark_interrupted(); + return false; } bool TouchScreenTask::init(const std::shared_ptr& arm, @@ -307,6 +381,10 @@ bool TouchScreenTask::init(const std::shared_ptr& arm, const std::shared_ptr& camera) { std::lock_guard lock(mutex_); arm_ = arm; + { + std::lock_guard arm_lock(activity_arm_mutex_); + activity_arm_ = arm; + } dexhand_ = dexhand; camera_ = camera; @@ -383,16 +461,213 @@ bool TouchScreenTask::init(const std::shared_ptr& arm, } bool TouchScreenTask::touch(const int u, const int v) { - std::lock_guard lock(mutex_); - if (isBusyUnlocked()) { + return touchIfCurrent(u, v, [] { return true; }); +} + +bool TouchScreenTask::touchIfCurrent( + const int u, + const int v, + const std::function& still_admitted) +{ + auto& admission_gate = service::globalStopAllAdmissionGate(); + std::uint64_t admission_generation = 0U; + { + auto admission = admission_gate.lockAdmission(); + if (!admission.accepting()) { + return false; + } + admission_generation = admission.generation(); + } + if (stop_requested_.load(std::memory_order_acquire)) { return false; } - return startFromPixelUnlocked(u, v); + const auto activity_generation = + activity_generation_.load(std::memory_order_acquire); + std::lock_guard lock(mutex_); + if (!still_admitted || !still_admitted()) { + return false; + } + if (!initialized_ || !camera_ || camera_->id().empty()) { + last_status_ = Status::NOT_INITIALIZED; + return false; + } + + auto& camera_registry = + service::globalCameraOperationalActivityRegistry(); + service::CameraOperationalActivityRegistry::ActivityToken camera_token; + service::CameraOperationalActivityRegistry::DispatchResult camera_start; + try { + camera_start = camera_registry.start( + camera_->id(), camera_, &camera_token); + } catch (...) { + last_status_ = Status::NOT_INITIALIZED; + return false; + } + if (camera_start != service::CameraOperationalActivityRegistry:: + DispatchResult::Success) { + last_status_ = Status::NOT_INITIALIZED; + return false; + } + + const auto rollback_camera = [&] { + if (camera_registry.stopIfCurrent(camera_token)) { + return; + } + const auto ticket = admission_gate.beginStopAll(); + (void)admission_gate.finishStopAll(ticket, false); + }; + + bool admission_current = false; + bool admitted = false; + { + // The arm lease and activity marker are the publication point. Holding + // admission here makes that point linearizable with beginStopAll(). + auto admission = admission_gate.lockAdmission(); + admission_current = admission.accepting() && + admission.generation() == admission_generation; + if (admission_current) { + admitted = beginActivityIfCurrent(activity_generation); + } + } + if (!admission_current || !admitted) { + rollback_camera(); + return false; + } + + const bool started = startFromPixelUnlocked(u, v); + if (!started) { + activity_active_.store(false, std::memory_order_release); + releaseActivityControlUnlocked(); + rollback_camera(); + return false; + } + + // startFromPixelUnlocked() may perform an interruptible initialization + // move. Do not retain the global gate across device work; reject and roll + // back if StopAll changed the generation while that work was in flight. + admission_current = false; + { + auto admission = admission_gate.lockAdmission(); + admission_current = admission.accepting() && + admission.generation() == admission_generation; + } + if (admission_current) { + return true; + } + activity_active_.store(false, std::memory_order_release); + resetActivityUnlocked(); + releaseActivityControlUnlocked(); + rollback_camera(); + return false; +} + +bool TouchScreenTask::beginActivityIfCurrent( + const std::uint64_t activity_generation) +{ + if (stop_requested_.load(std::memory_order_acquire) || + activity_generation_.load(std::memory_order_acquire) != + activity_generation) { + return false; + } + if (isBusyUnlocked()) { + last_status_ = Status::TASK_BUSY; + return false; + } + if (!acquireActivityControlUnlocked()) { + last_status_ = Status::TASK_BUSY; + return false; + } + if (stop_requested_.load(std::memory_order_acquire) || + activity_generation_.load(std::memory_order_acquire) != + activity_generation) { + releaseActivityControlUnlocked(); + return false; + } + activity_active_.store(true, std::memory_order_release); + return true; +} + +bool TouchScreenTask::acquireActivityControlUnlocked() +{ + if (!arm_ || arm_->id().empty() || activityControlToken().valid()) { + return false; + } + static std::atomic sequence{0U}; + const auto acquired = control::ControlAuthorityManager::instance() + .tryAcquire( + arm_->id(), + "touch-screen:" + id_ + ":" + + std::to_string( + sequence.fetch_add(1U, std::memory_order_relaxed) + 1U), + std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>( + std::chrono::hours(24))); + if (!acquired.acquired) { + return false; + } + { + std::lock_guard lock(activity_control_mutex_); + activity_control_token_ = acquired.token; + } + return true; +} + +control::ControlLeaseToken TouchScreenTask::activityControlToken() const +{ + std::lock_guard lock(activity_control_mutex_); + return activity_control_token_; +} + +bool TouchScreenTask::activityControlCurrent() const +{ + const auto token = activityControlToken(); + return token.valid() && + control::ControlAuthorityManager::instance().validate( + token); +} + +std::function TouchScreenTask::activityCancellationRequested() const +{ + const auto token = activityControlToken(); + return [this, token] { + return stop_requested_.load(std::memory_order_acquire) || + !control::ControlAuthorityManager::instance().validate(token); + }; +} + +control::ControlDispatchGuard +TouchScreenTask::tryBeginActivityDispatch() const +{ + const auto token = activityControlToken(); + return control::ControlAuthorityManager::instance().tryBeginDispatch( + token); +} + +void TouchScreenTask::releaseActivityControlUnlocked() noexcept +{ + control::ControlLeaseToken token; + { + std::lock_guard lock(activity_control_mutex_); + token = std::move(activity_control_token_); + activity_control_token_ = {}; + } + control::ControlAuthorityManager::instance().release(token); +} + +void TouchScreenTask::finishActivityUnlocked( + const Phase phase, + const Status status) noexcept +{ + phase_ = phase; + last_status_ = status; + touch_command_started_ = false; + retract_command_started_ = false; + activity_active_.store(false, std::memory_order_release); + releaseActivityControlUnlocked(); } bool TouchScreenTask::startFromPixel(const int u, const int v) { - std::lock_guard lock(mutex_); - return startFromPixelUnlocked(u, v); + return touchIfCurrent(u, v, [] { return true; }); } bool TouchScreenTask::startFromPixelUnlocked(int u, int v) { @@ -405,7 +680,7 @@ bool TouchScreenTask::startFromPixelUnlocked(int u, int v) { return false; } - stopUnlocked(); + resetActivityUnlocked(); if (!moveToInitPositionBeforeStartIfEnabled()) { return false; } @@ -442,12 +717,22 @@ bool TouchScreenTask::startFromPixelUnlocked(int u, int v) { bool TouchScreenTask::step(const double dt) { std::lock_guard lock(mutex_); + if (stop_requested_.load(std::memory_order_acquire)) { + return true; + } + if (activity_active_.load(std::memory_order_acquire) && + !activityControlCurrent()) { + activity_active_.store(false, std::memory_order_release); + resetActivityUnlocked(); + releaseActivityControlUnlocked(); + return true; + } if (!initialized_) { last_status_ = Status::NOT_INITIALIZED; return false; } if (!std::isfinite(dt) || dt <= 0.0) { - last_status_ = Status::INVALID_CONFIG; + enterFailed(Status::INVALID_CONFIG); return false; } @@ -494,26 +779,101 @@ bool TouchScreenTask::step(const double dt) { case Phase::FAILED: return false; } - last_status_ = Status::INVALID_CONFIG; + enterFailed(Status::INVALID_CONFIG); return false; } void TouchScreenTask::stop() { - std::lock_guard lock(mutex_); - stopUnlocked(); + (void)stopActivity(); } -void TouchScreenTask::stopUnlocked() { - if (arm_) { +bool TouchScreenTask::stopActivity() { + activity_generation_.fetch_add(1U, std::memory_order_acq_rel); + stop_requested_.store(true, std::memory_order_release); + + std::shared_ptr arm; + { + std::lock_guard lock(activity_arm_mutex_); + arm = activity_arm_; + } + + static std::atomic stop_sequence{0U}; + auto& authority = control::ControlAuthorityManager::instance(); + const auto expected_token = activityControlToken(); + control::ControlAcquireResult stop_barrier; + bool barrier_error = false; + if (arm && expected_token.valid()) { try { - arm_->stopL(); + stop_barrier = authority.preemptAcquireIfCurrent( + expected_token, + "touch-screen-stop:" + id_ + ":" + + std::to_string( + stop_sequence.fetch_add( + 1U, std::memory_order_relaxed) + 1U), + std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>( + std::chrono::hours(24))); + } catch (...) { + barrier_error = true; + (void)authority.quarantineIfCurrent(expected_token); + } + } + + // Only the caller which atomically converted this task's exact lease may + // touch the driver. If StopAll already owns the safety barrier, its arm + // stop runs independently while this task only drains its old step. + if (stop_barrier.acquired) { + try { + (void)arm->stopMotion(); } catch (...) { } } - sendZeroJointVelocity(); + bool was_active = false; + { + // A step holds this mutex through all of its arm submissions. Taking + // it here proves that the old step has exited before state is reset. + std::lock_guard lock(mutex_); + was_active = activity_active_.exchange( + false, std::memory_order_acq_rel); + if (was_active || activityControlToken().valid() || + isBusyUnlocked()) { + resetActivityUnlocked(); + } + releaseActivityControlUnlocked(); + } + + bool stopped = !barrier_error; + if (stop_barrier.acquired) { + const bool handler_released = authority.waitForPreemptedRelease( + stop_barrier.token, + control::ControlAuthorityManager::Duration::zero()); + if (handler_released) { + // A backend may have allowed the cancellation request to return + // without fully quiescing. Confirm once more after the old task + // step and every guarded dispatch have drained. + try { + stopped = arm->stopMotion().ok(); + } catch (...) { + stopped = false; + } + } else { + stopped = false; + } + + if (stopped) { + authority.release(stop_barrier.token); + } else { + (void)authority.retireSafetyHolder(stop_barrier.token); + } + } + + stop_requested_.store(false, std::memory_order_release); + return stopped; +} + +void TouchScreenTask::resetActivityUnlocked() { ibvs_.resetTwistCommandState(); - holdCurrentControlledPosition(); phase_ = Phase::IDLE; phase_after_retract_ = Phase::DONE; @@ -1337,12 +1697,13 @@ bool TouchScreenTask::stepRetracting() { holdCurrentControlledPosition(); if ((phase_after_retract_ == Phase::DONE || phase_after_retract_ == Phase::FAILED) && !moveToInitPositionIfEnabled()) { - phase_ = Phase::FAILED; - last_status_ = Status::ROBOT_COMMAND_FAILED; + finishActivityUnlocked( + Phase::FAILED, Status::ROBOT_COMMAND_FAILED); return false; } - phase_ = phase_after_retract_; - last_status_ = final_status_after_retract_; + const auto completed_phase = phase_after_retract_; + const auto completed_status = final_status_after_retract_; + finishActivityUnlocked(completed_phase, completed_status); return phase_ != Phase::FAILED; } @@ -1377,6 +1738,10 @@ 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); @@ -1470,6 +1835,10 @@ 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; @@ -1500,6 +1869,7 @@ bool TouchScreenTask::moveToInitPositionBeforeStartIfEnabled() { device::MotionOptions options; 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()) { last_status_ = Status::ROBOT_COMMAND_FAILED; @@ -1521,6 +1891,7 @@ bool TouchScreenTask::moveToInitPositionIfEnabled() const { device::MotionOptions options; 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; @@ -1565,6 +1936,11 @@ 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(), @@ -1596,6 +1972,7 @@ bool TouchScreenTask::startTouchPhase() { options.jerk = move_l.jerk(); 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()) { last_status_ = Status::ROBOT_COMMAND_FAILED; @@ -1607,12 +1984,9 @@ bool TouchScreenTask::startTouchPhase() { return false; } - phase_ = Phase::DONE; - touch_command_started_ = false; - retract_command_started_ = false; + finishActivityUnlocked(Phase::DONE, Status::DONE); retract_start_position_valid_ = false; retract_start_position_base_.setZero(); - last_status_ = Status::DONE; return true; } @@ -1644,6 +2018,10 @@ 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, @@ -1673,10 +2051,10 @@ void TouchScreenTask::enterFailed(const Status status) { } hardStopIbvsMotion(); holdCurrentControlledPosition(); - phase_ = Phase::FAILED; - touch_command_started_ = false; - retract_command_started_ = false; - last_status_ = moveToInitPositionIfEnabled() ? status : Status::ROBOT_COMMAND_FAILED; + const auto final_status = moveToInitPositionIfEnabled() + ? status + : Status::ROBOT_COMMAND_FAILED; + finishActivityUnlocked(Phase::FAILED, final_status); } bool TouchScreenTask::updateTouchPressure() { diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp index 55a45b27..4ca05918 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -33,6 +34,33 @@ #include "manager/task_manager/include/task_manager.h" #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" +namespace cmvr::task { + +class TouchScreenTaskStopActivityTestPeer final { +public: + static void setActive( + TouchScreenTask& task, + const std::shared_ptr& arm, + const control::ControlLeaseToken& token) + { + { + std::lock_guard lock(task.activity_arm_mutex_); + task.activity_arm_ = arm; + } + { + std::lock_guard lock(task.activity_control_mutex_); + task.activity_control_token_ = token; + } + { + std::lock_guard lock(task.mutex_); + task.phase_ = TouchScreenTask::Phase::ALIGNING; + task.activity_active_.store(true, std::memory_order_release); + } + } +}; + +} // namespace cmvr::task + namespace { constexpr int kRealTargetU = 1280 / 2.0; @@ -44,6 +72,168 @@ constexpr std::array kJointNames = { "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R" }; +class StopCountingRobotArm final : public cmvr::device::RobotArm { +public: + explicit StopCountingRobotArm(std::string id) + { + id_ = std::move(id); + } + + std::string typeName() const override { return "StopCountingRobotArm"; } + cmvr::device::RobotModel getRobotModel() const override { return {}; } + std::size_t getDof() const override { return 0U; } + cmvr::device::ArmState getRobotState() const override { return {}; } + cmvr::device::JointGroupState getJointState() const override { return {}; } + cmvr::device::CartesianPose getTcpPose( + cmvr::device::FrameType = cmvr::device::FrameType::Base) const override + { + return {}; + } + cmvr::device::RobotMode getRobotMode() const override + { + return cmvr::device::RobotMode::Idle; + } + cmvr::device::SafetyMode getSafetyMode() const override + { + return cmvr::device::SafetyMode::Normal; + } + cmvr::device::ControlMode getControlMode() const override + { + return cmvr::device::ControlMode::None; + } + cmvr::device::Result torqueOn() override { return success(); } + cmvr::device::Result torqueOff() override { return success(); } + cmvr::device::Result calibrateZeroQ(const std::string&) override + { + return success(); + } + cmvr::device::Result emergencyStop() override { return success(); } + cmvr::device::Result protectiveStop() override { return success(); } + cmvr::device::Result setSpeedScaling(double) override { return success(); } + double getSpeedScaling() const override { return 1.0; } + bool isProtectiveStopped() const override { return false; } + bool isEmergencyStopped() const override { return false; } + bool isFault() const override { return false; } + cmvr::device::Result moveJ( + const cmvr::device::JointPositionCommand&, + const cmvr::device::MotionOptions&) override + { + return success(); + } + cmvr::device::Result speedJ( + const cmvr::device::JointVelocityCommand&, double, double) override + { + return success(); + } + cmvr::device::Result stopJ(double) override { return success(); } + cmvr::device::Result moveL( + const cmvr::device::CartesianPose&, + const cmvr::device::MotionOptions&, + cmvr::device::FrameType = cmvr::device::FrameType::Base) override + { + return success(); + } + cmvr::device::Result speedL( + const cmvr::device::CartesianVelocity&, + double, + double, + cmvr::device::FrameType = cmvr::device::FrameType::Base) override + { + return success(); + } + cmvr::device::Result stopL(std::optional = std::nullopt) override + { + return success(); + } + cmvr::device::Result stopMotion() override + { + stop_motion_calls_.fetch_add(1, std::memory_order_relaxed); + return success(); + } + cmvr::device::Result startServoMode( + const cmvr::device::ServoOptions&) override + { + return success(); + } + cmvr::device::Result servoJ( + const cmvr::device::JointPositionCommand&) override + { + return success(); + } + cmvr::device::Result servoL( + const cmvr::device::CartesianPose&, + cmvr::device::FrameType = cmvr::device::FrameType::Base) override + { + return success(); + } + cmvr::device::Result servoSpeedJ( + const cmvr::device::JointVelocityCommand&) override + { + return success(); + } + cmvr::device::Result servoSpeedL( + const cmvr::device::CartesianVelocity&, + cmvr::device::FrameType = cmvr::device::FrameType::Base) override + { + return success(); + } + cmvr::device::Result stopServoMode() override { return success(); } + cmvr::device::Result connect(const std::string&, int) override + { + return success(); + } + cmvr::device::Result disconnect() override { return success(); } + bool isConnected() const override { return true; } + cmvr::device::Result powerOn() override { return success(); } + cmvr::device::Result powerOff() override { return success(); } + cmvr::device::Result brakeRelease() override { return success(); } + cmvr::device::Result shutdown() override { return success(); } + cmvr::device::Result clearFault() override { return success(); } + cmvr::device::Result unlockProtectiveStop() override { return success(); } + cmvr::device::Result loadProgram(const std::string&) override + { + return success(); + } + cmvr::device::Result playProgram() override { return success(); } + cmvr::device::Result pauseProgram() override { return success(); } + cmvr::device::Result stopProgram() override { return success(); } + std::vector ik( + const std::string&, + const std::string&, + const cmvr::device::CartesianPose&) override + { + return {}; + } + std::shared_ptr kinematicsSolver() const override + { + return nullptr; + } + cmvr::device::CartesianPose fk( + const std::string&, const std::string&) override + { + return {}; + } + cmvr::device::CartesianPose fk(bool = true) override { return {}; } + cmvr::device::CartesianVelocity getSpeedLCommandTwistBase() const override + { + return {}; + } + bool busy() const override { return false; } + + int stopMotionCalls() const + { + return stop_motion_calls_.load(std::memory_order_relaxed); + } + +private: + static cmvr::device::Result success() + { + return cmvr::device::Result::success(); + } + + std::atomic stop_motion_calls_{0}; +}; + std::filesystem::path findProjectRoot() { const std::filesystem::path marker = "model/xiaoyan_description/dual_arm.xml"; @@ -422,6 +612,60 @@ void run_touch_once(int u, int v) { } // namespace +TEST(TouchScreenTaskTest, StandaloneStopOwnsAndConfirmsPhysicalArmStop) +{ + auto& authority = cmvr::control::ControlAuthorityManager::instance(); + authority.clear(); + const auto arm = std::make_shared( + "touch-standalone-stop-arm"); + const auto lease = authority.tryAcquire( + arm->id(), "touch-task-owner", std::chrono::hours(1)); + ASSERT_TRUE(lease.acquired) << lease.detail; + + cmvr::task::TouchScreenTask task( + cmvr::config::TouchScreenTaskConfig{}); + cmvr::task::TouchScreenTaskStopActivityTestPeer::setActive( + task, arm, lease.token); + + EXPECT_TRUE(task.stopActivity()); + EXPECT_EQ(arm->stopMotionCalls(), 2); + EXPECT_EQ(task.phase(), cmvr::task::TouchScreenTask::Phase::IDLE); + EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::STOPPED); + EXPECT_FALSE(authority.isLeased(arm->id())); + authority.clear(); +} + +TEST(TouchScreenTaskTest, StopSkipsArmWhenExternalSafetyBarrierAlreadyPreempted) +{ + auto& authority = cmvr::control::ControlAuthorityManager::instance(); + authority.clear(); + const auto arm = std::make_shared( + "touch-external-stop-arm"); + const auto lease = authority.tryAcquire( + arm->id(), "touch-task-owner", std::chrono::hours(1)); + ASSERT_TRUE(lease.acquired) << lease.detail; + + cmvr::task::TouchScreenTask task( + cmvr::config::TouchScreenTaskConfig{}); + cmvr::task::TouchScreenTaskStopActivityTestPeer::setActive( + task, arm, lease.token); + const auto external_barrier = authority.preemptAcquire( + arm->id(), "system-stop-all", std::chrono::hours(1)); + ASSERT_TRUE(external_barrier.acquired) << external_barrier.detail; + + EXPECT_TRUE(task.stopActivity()); + EXPECT_EQ(arm->stopMotionCalls(), 0); + EXPECT_EQ(task.phase(), cmvr::task::TouchScreenTask::Phase::IDLE); + EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::STOPPED); + EXPECT_TRUE(authority.validate(external_barrier.token)); + EXPECT_TRUE(authority.waitForPreemptedRelease( + external_barrier.token, + cmvr::control::ControlAuthorityManager::Duration::zero())); + authority.release(external_barrier.token); + EXPECT_FALSE(authority.isLeased(arm->id())); + authority.clear(); +} + TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) { const auto project_root = findProjectRoot(); ASSERT_FALSE(project_root.empty()); diff --git a/cmvr-es/task/ume_teleop_task/CMakeLists.txt b/cmvr-es/task/ume_teleop_task/CMakeLists.txt index d624679f..1b3b6362 100644 --- a/cmvr-es/task/ume_teleop_task/CMakeLists.txt +++ b/cmvr-es/task/ume_teleop_task/CMakeLists.txt @@ -12,6 +12,7 @@ target_link_libraries(ume_teleop_task cmvr_es::proto PRIVATE cmvr_es::logging + cmvr_es::stop_all_admission_gate Threads::Threads ) @@ -26,6 +27,7 @@ if(BUILD_TESTING) target_link_libraries(ume_teleop_task_test PRIVATE cmvr_es::ume_teleop_task + cmvr_es::stop_all_admission_gate Threads::Threads ) add_test(NAME ume_teleop_task_test COMMAND ume_teleop_task_test) diff --git a/cmvr-es/task/ume_teleop_task/include/ume_teleop_task.h b/cmvr-es/task/ume_teleop_task/include/ume_teleop_task.h index 0c3da0ae..9886e6ff 100644 --- a/cmvr-es/task/ume_teleop_task/include/ume_teleop_task.h +++ b/cmvr-es/task/ume_teleop_task/include/ume_teleop_task.h @@ -30,6 +30,7 @@ public: bool start() override; bool step(double dt) override; void stop() override; + bool stopActivity() override; TaskState state() const override; bool isBusy() const override; @@ -64,6 +65,7 @@ private: std::string id_; std::shared_ptr client_; + mutable std::mutex lifecycle_mutex_; mutable std::mutex mutex_; std::condition_variable stop_condition_; std::thread worker_; diff --git a/cmvr-es/task/ume_teleop_task/src/ume_teleop_task.cpp b/cmvr-es/task/ume_teleop_task/src/ume_teleop_task.cpp index 314102af..4caec47d 100644 --- a/cmvr-es/task/ume_teleop_task/src/ume_teleop_task.cpp +++ b/cmvr-es/task/ume_teleop_task/src/ume_teleop_task.cpp @@ -11,6 +11,7 @@ #include "cmvr/config/task_manager_config/task_manager_config.pb.h" #include "common/base/logging/logger.h" +#include "service/stop_all/include/stop_all_admission_gate.h" #include "common/config/config_files.h" #include "task/task_factory.h" @@ -69,6 +70,7 @@ UmeTeleopTask::~UmeTeleopTask() bool UmeTeleopTask::init() { + std::lock_guard lifecycle_lock(lifecycle_mutex_); std::lock_guard lock(mutex_); if (state_ == TaskState::IDLE) { return true; @@ -122,9 +124,33 @@ bool UmeTeleopTask::init() bool UmeTeleopTask::start() { + auto& admission_gate = service::globalStopAllAdmissionGate(); + std::uint64_t admission_generation = 0U; + { + auto admission = admission_gate.lockAdmission(); + if (!admission.accepting()) { + return false; + } + admission_generation = admission.generation(); + } + + std::unique_lock lifecycle_lock(lifecycle_mutex_); + // A start request may have waited behind stopActivity(). Recheck the exact + // generation before creating workers, but do not retain the global guard + // while starting or cleaning up task threads. + { + auto admission = admission_gate.lockAdmission(); + if (!admission.accepting() || + admission.generation() != admission_generation) { + return false; + } + } std::unique_lock lock(mutex_); if (state_ == TaskState::RUNNING) { - return true; + lock.unlock(); + auto current = admission_gate.lockAdmission(); + return current.accepting() && + current.generation() == admission_generation; } if ((state_ != TaskState::IDLE && state_ != TaskState::STOPPED) || !client_ || worker_.joinable() || sender_.joinable()) { @@ -194,7 +220,23 @@ bool UmeTeleopTask::start() } state_ = TaskState::RUNNING; - return true; + lock.unlock(); + + bool admission_current = false; + { + auto admission = admission_gate.lockAdmission(); + admission_current = admission.accepting() && + admission.generation() == admission_generation; + } + if (admission_current) { + return true; + } + + // Let stopActivity take the lifecycle lock and roll back every worker + // created by this stale start request. + lifecycle_lock.unlock(); + (void)stopActivity(); + return false; } bool UmeTeleopTask::step(const double dt) @@ -205,6 +247,12 @@ bool UmeTeleopTask::step(const double dt) void UmeTeleopTask::stop() { + (void)stopActivity(); +} + +bool UmeTeleopTask::stopActivity() +{ + std::lock_guard lifecycle_lock(lifecycle_mutex_); std::shared_ptr client; std::thread worker; std::thread sender; @@ -247,10 +295,16 @@ void UmeTeleopTask::stop() worker.join(); } - std::lock_guard lock(mutex_); - if (state_ != TaskState::FAILED) { - state_ = TaskState::STOPPED; + bool stopped = !client || !client->isSessionActive(); + { + std::lock_guard lock(mutex_); + stopped = stopped && !worker_.joinable() && !sender_.joinable() && + !receiver_session_active_; + if (state_ != TaskState::FAILED) { + state_ = TaskState::STOPPED; + } } + return stopped; } TaskState UmeTeleopTask::state() const diff --git a/cmvr-es/task/ume_teleop_task/tests/ume_teleop_task_test.cpp b/cmvr-es/task/ume_teleop_task/tests/ume_teleop_task_test.cpp index 60ff3e1c..a075ffcc 100644 --- a/cmvr-es/task/ume_teleop_task/tests/ume_teleop_task_test.cpp +++ b/cmvr-es/task/ume_teleop_task/tests/ume_teleop_task_test.cpp @@ -14,6 +14,7 @@ #include "cmvr/api/arm_teleop_v1.grpc.pb.h" #include "service/arm_teleop_client/include/grpc_arm_teleop_client.h" +#include "service/stop_all/include/stop_all_admission_gate.h" #include "task/ume_teleop_task/include/ume_teleop_task.h" namespace { @@ -42,6 +43,9 @@ public: { std::lock_guard lock(mutex_); open_received_ = true; + ++open_count_; + reader_finished_ = false; + last_sequence_ = 0; last_activity_ = std::chrono::steady_clock::now(); } condition_.notify_all(); @@ -90,6 +94,7 @@ public: } } else if (incoming.has_stop()) { stop_received_ = true; + ++stop_count_; terminal = true; } if (terminal) { @@ -138,6 +143,7 @@ public: { std::lock_guard lock(mutex_); handler_finished_ = true; + ++handler_finish_count_; } condition_.notify_all(); return expired @@ -148,16 +154,31 @@ public: } bool waitForOpen(const std::chrono::milliseconds timeout) + { + return waitForOpenCount(1, timeout); + } + + bool waitForOpenCount( + const std::size_t count, + const std::chrono::milliseconds timeout) { std::unique_lock lock(mutex_); - return condition_.wait_for(lock, timeout, [this] { return open_received_; }); + return condition_.wait_for( + lock, timeout, [&] { return open_count_ >= count; }); } bool waitForHandlerFinish(const std::chrono::milliseconds timeout) + { + return waitForHandlerFinishCount(1, timeout); + } + + bool waitForHandlerFinishCount( + const std::size_t count, + const std::chrono::milliseconds timeout) { std::unique_lock lock(mutex_); return condition_.wait_for( - lock, timeout, [this] { return handler_finished_; }); + lock, timeout, [&] { return handler_finish_count_ >= count; }); } bool waitForHeartbeatCount( @@ -184,6 +205,18 @@ public: return setpoints_.size(); } + std::size_t heartbeatCount() const + { + std::lock_guard lock(mutex_); + return heartbeat_count_; + } + + std::size_t stopCount() const + { + std::lock_guard lock(mutex_); + return stop_count_; + } + api::JointSetpoint lastSetpoint() const { std::lock_guard lock(mutex_); @@ -220,6 +253,9 @@ private: bool watchdog_expired_{false}; bool sequence_valid_{true}; bool stop_received_{false}; + std::size_t open_count_{0}; + std::size_t handler_finish_count_{0}; + std::size_t stop_count_{0}; std::uint64_t activity_version_{0}; std::uint64_t last_sequence_{0}; std::size_t heartbeat_count_{0}; @@ -261,6 +297,9 @@ int fail(const std::string& detail) int main() { + auto& admission = cmvr::service::globalStopAllAdmissionGate(); + admission.clearForTesting(); + { cmvr::config::UmeTeleopConfig invalid; invalid.set_id("invalid"); @@ -308,21 +347,19 @@ int main() } WatchdogArmTeleopService service(120ms); - const std::string socket_path = - "/tmp/cmvr_ume_teleop_task_test_" + - std::to_string(static_cast(::getpid())) + ".sock"; - std::remove(socket_path.c_str()); - const std::string endpoint = "unix:" + socket_path; - grpc::ServerBuilder builder; + int selected_port = 0; builder.AddListeningPort( - endpoint, - grpc::InsecureServerCredentials()); + "127.0.0.1:0", + grpc::InsecureServerCredentials(), + &selected_port); builder.RegisterService(&service); std::unique_ptr server = builder.BuildAndStart(); - if (!server) { + if (!server || selected_port == 0) { return fail("failed to start in-process gRPC server"); } + const std::string endpoint = + "127.0.0.1:" + std::to_string(selected_port); auto channel = grpc::CreateChannel( endpoint, @@ -335,9 +372,19 @@ int main() server->Shutdown(); return fail("task is not a BLOCKING_SERVICE"); } - if (!task.init() || !task.start()) { + if (!task.init()) { server->Shutdown(); - return fail("valid task did not initialize and start"); + return fail("valid task did not initialize"); + } + + auto admission_ticket = admission.beginStopAll(); + if (task.start() || task.isBusy()) { + server->Shutdown(); + return fail("public start bypassed closed StopAll admission"); + } + if (!admission.finishStopAll(admission_ticket, true) || !task.start()) { + server->Shutdown(); + return fail("explicit start was not restored after successful StopAll"); } if (!service.waitForOpen(2s)) { task.stop(); @@ -398,24 +445,78 @@ int main() } const auto stop_begin = std::chrono::steady_clock::now(); - task.stop(); + const bool activity_stopped = task.stopActivity(); const auto stop_elapsed = std::chrono::steady_clock::now() - stop_begin; - if (stop_elapsed > 2s) { + if (!activity_stopped || stop_elapsed > 2s) { server->Shutdown(); - return fail("Task stop did not TryCancel and join promptly"); + return fail("stopActivity did not stop and join promptly"); } if (task.state() != cmvr::task::TaskState::STOPPED || task.isBusy() || !task.isFinished()) { server->Shutdown(); - return fail("task did not reach STOPPED after joining its worker"); + return fail("task did not reach STOPPED after stopping its activity"); } if (!service.waitForHandlerFinish(2s)) { server->Shutdown(); - return fail("server handler did not observe Task cancellation"); + return fail("server handler did not observe activity cancellation"); } if (!service.stopReceived()) { server->Shutdown(); - return fail("Task did not send StopSession before TryCancel"); + return fail("stopActivity did not send StopSession before TryCancel"); + } + api::JointSetpoint stopped_setpoint; + stopped_setpoint.add_position_rad(5.0); + stopped_setpoint.set_valid_for_us(100000); + if (task.submitSetpoint(stopped_setpoint)) { + server->Shutdown(); + return fail("stopped activity still accepted a setpoint"); + } + const auto heartbeats_after_stop = service.heartbeatCount(); + const auto setpoints_after_stop = service.setpointCount(); + std::this_thread::sleep_for(200ms); + if (service.heartbeatCount() != heartbeats_after_stop || + service.setpointCount() != setpoints_after_stop) { + server->Shutdown(); + return fail("stopped activity continued sending heartbeat or setpoint frames"); + } + + if (!task.start() || + !service.waitForOpenCount(2, 2s) || + !service.waitForHeartbeatCount(heartbeats_after_stop + 1, 2s) || + !task.isBusy()) { + task.stop(); + server->Shutdown(); + return fail("explicit start did not create a new teleoperation activity"); + } + api::JointSetpoint restarted_setpoint; + restarted_setpoint.add_position_rad(6.0); + restarted_setpoint.set_valid_for_us(100000); + if (!task.submitSetpoint(restarted_setpoint) || + !service.waitForSetpointCount(setpoints_after_stop + 1, 2s)) { + task.stop(); + server->Shutdown(); + return fail("restarted activity did not send a setpoint"); + } + if (!task.stopActivity() || + !service.waitForHandlerFinishCount(2, 2s) || + service.stopCount() != 2 || + task.state() != cmvr::task::TaskState::STOPPED) { + server->Shutdown(); + return fail("restarted activity did not stop cleanly"); + } + + admission_ticket = admission.beginStopAll(); + if (admission.finishStopAll(admission_ticket, false) || task.start()) { + server->Shutdown(); + return fail("failed StopAll did not keep public start fail-closed"); + } + admission_ticket = admission.beginStopAll(); + if (!admission.finishStopAll(admission_ticket, true) || + !task.start() || !service.waitForOpenCount(3, 2s) || + !task.stopActivity() || + !service.waitForHandlerFinishCount(3, 2s)) { + server->Shutdown(); + return fail("successful StopAll did not restore a new explicit start"); } if (service.watchdogExpired() || !service.sequenceValid()) { server->Shutdown(); @@ -423,7 +524,7 @@ int main() } server->Shutdown(); - std::remove(socket_path.c_str()); + admission.clearForTesting(); std::cout << "ume_teleop_task_test: PASS\n"; return 0; } From de764de60777fa9c9d2facb13d14b098793db26d Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 14 Aug 2026 08:38:00 +0800 Subject: [PATCH 2/8] feat(grpc): persist RPC failures in edge logs --- cmvr-es/common/base/logging/CMakeLists.txt | 23 + cmvr-es/common/base/logging/logger.cpp | 12 +- cmvr-es/common/base/logging/logger.h | 1 + .../common/base/logging/tests/logger_test.cpp | 93 ++ cmvr-es/config/logger/logger.pb.txt | 6 +- cmvr-es/service/CMakeLists.txt | 26 + .../include/grpc_error_logging_interceptor.h | 37 + .../service/grpc/src/grpc_camera_service.cpp | 1 - .../service/grpc/src/grpc_dexhand_service.cpp | 1 - .../src/grpc_error_logging_interceptor.cpp | 504 ++++++++++ .../service/grpc/src/grpc_head_service.cpp | 4 - .../grpc/src/grpc_microphone_service.cpp | 1 - .../service/grpc/src/grpc_speaker_service.cpp | 1 - .../grpc_error_logging_interceptor_test.cpp | 864 ++++++++++++++++++ .../grpc_server_task/src/grpc_server_task.cpp | 8 + 15 files changed, 1570 insertions(+), 12 deletions(-) create mode 100644 cmvr-es/common/base/logging/tests/logger_test.cpp create mode 100644 cmvr-es/service/grpc/include/grpc_error_logging_interceptor.h create mode 100644 cmvr-es/service/grpc/src/grpc_error_logging_interceptor.cpp create mode 100644 cmvr-es/service/grpc/tests/grpc_error_logging_interceptor_test.cpp diff --git a/cmvr-es/common/base/logging/CMakeLists.txt b/cmvr-es/common/base/logging/CMakeLists.txt index c6c31e5c..5322a2e5 100644 --- a/cmvr-es/common/base/logging/CMakeLists.txt +++ b/cmvr-es/common/base/logging/CMakeLists.txt @@ -13,3 +13,26 @@ target_link_libraries(logging PUBLIC add_library(cmvr_es::logging ALIAS logging) install(TARGETS logging ARCHIVE DESTINATION lib) + +if(BUILD_TESTING) + add_executable(logger_test + tests/logger_test.cpp + ) + target_link_libraries(logger_test PRIVATE + cmvr_es::logging + gtest + gtest_main + pthread + ) + add_test(NAME logger_test COMMAND logger_test) + set(_logger_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}") + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _logger_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(logger_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_logger_test_environment}" + ) +endif() diff --git a/cmvr-es/common/base/logging/logger.cpp b/cmvr-es/common/base/logging/logger.cpp index 0a6355ee..594b95c1 100644 --- a/cmvr-es/common/base/logging/logger.cpp +++ b/cmvr-es/common/base/logging/logger.cpp @@ -167,6 +167,15 @@ void Logger::shutdown() initialized_ = false; } +void Logger::flush() +{ + std::lock_guard lock(mutex_); + if (log_file_.is_open()) { + log_file_.flush(); + last_flush_ = std::chrono::steady_clock::now(); + } +} + bool Logger::enabled(const Level level) const { std::lock_guard lock(mutex_); @@ -200,7 +209,8 @@ void Logger::write(const Level level, rotateIfNeeded_(); log_file_ << line << '\n'; const auto now = std::chrono::steady_clock::now(); - if (level == Level::ERROR || level == Level::FATAL || now - last_flush_ >= flush_interval_) { + if (level == Level::ERROR || level == Level::FATAL || + now - last_flush_ >= flush_interval_) { log_file_.flush(); last_flush_ = now; } diff --git a/cmvr-es/common/base/logging/logger.h b/cmvr-es/common/base/logging/logger.h index a7ee3a74..d6bd17c5 100644 --- a/cmvr-es/common/base/logging/logger.h +++ b/cmvr-es/common/base/logging/logger.h @@ -43,6 +43,7 @@ public: const std::string& application_name, const std::filesystem::path& executable_directory); void shutdown(); + void flush(); bool enabled(Level level) const; void write(Level level, const char* source_file, int source_line, const std::string& message); diff --git a/cmvr-es/common/base/logging/tests/logger_test.cpp b/cmvr-es/common/base/logging/tests/logger_test.cpp new file mode 100644 index 00000000..cc0aef19 --- /dev/null +++ b/cmvr-es/common/base/logging/tests/logger_test.cpp @@ -0,0 +1,93 @@ +#include "common/base/logging/logger.h" + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace cmvr::logging { +namespace { + +class LoggerTest : public testing::Test { +protected: + void SetUp() override + { + std::array pattern{}; + const std::string value = "/tmp/cmvr-logger-test-XXXXXX"; + std::copy(value.begin(), value.end(), pattern.begin()); + if (char* created = ::mkdtemp(pattern.data())) { + directory_ = created; + } + ASSERT_FALSE(directory_.empty()); + } + + void TearDown() override + { + shutdownLogging(); + std::error_code error; + std::filesystem::remove_all(directory_, error); + } + + config::LoggerConfig warningFileConfig() const + { + config::LoggerConfig config; + config.set_minimum_level(config::LOG_LEVEL_DEBUG); + config.set_directory(directory_.string()); + config.set_flush_interval_seconds(3600); + auto* route = config.add_routes(); + route->set_level(config::LOG_LEVEL_WARNING); + route->set_terminal(false); + route->set_file(true); + return config; + } + + std::string fileContents() const + { + std::ifstream input(directory_ / "logger_test.log"); + return {std::istreambuf_iterator(input), + std::istreambuf_iterator()}; + } + + std::filesystem::path directory_; +}; + +TEST_F(LoggerTest, ExplicitFlushMakesWarningVisibleInFile) +{ + ASSERT_TRUE(initLogging( + warningFileConfig(), "logger_test", directory_)); + + Logger::instance().write( + Level::WARNING, __FILE__, __LINE__, "warning sentinel"); + Logger::instance().flush(); + + EXPECT_NE(fileContents().find("warning sentinel"), std::string::npos); +} + +TEST_F(LoggerTest, ErrorIsVisibleInFileImmediately) +{ + auto config = warningFileConfig(); + config.mutable_routes(0)->set_level(config::LOG_LEVEL_ERROR); + ASSERT_TRUE(initLogging(config, "logger_test", directory_)); + + Logger::instance().write( + Level::ERROR, __FILE__, __LINE__, "error sentinel"); + + EXPECT_NE(fileContents().find("error sentinel"), std::string::npos); +} + +TEST_F(LoggerTest, FlushIsSafeOutsideInitializedLifetime) +{ + Logger::instance().flush(); + ASSERT_TRUE(initLogging( + warningFileConfig(), "logger_test", directory_)); + shutdownLogging(); + Logger::instance().flush(); +} + +} // namespace +} // namespace cmvr::logging diff --git a/cmvr-es/config/logger/logger.pb.txt b/cmvr-es/config/logger/logger.pb.txt index 85f8aac0..54182979 100644 --- a/cmvr-es/config/logger/logger.pb.txt +++ b/cmvr-es/config/logger/logger.pb.txt @@ -13,17 +13,17 @@ logger { routes { level: LOG_LEVEL_WARNING terminal: true - file: false + file: true } routes { level: LOG_LEVEL_ERROR terminal: true - file: false + file: true } routes { level: LOG_LEVEL_FATAL terminal: true - file: false + file: true } directory: "../log" diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index ef742c48..1244e7cb 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -6,6 +6,7 @@ add_library(service grpc/src/media_activity_coordinator.cpp grpc/src/motor_activity_coordinator.cpp grpc/src/grpc_camera_service.cpp + grpc/src/grpc_error_logging_interceptor.cpp grpc/src/grpc_system_service.cpp grpc/src/grpc_speaker_service.cpp grpc/src/grpc_microphone_service.cpp @@ -216,6 +217,31 @@ if(BUILD_TESTING) ENVIRONMENT "${_grpc_system_test_environment}" ) + add_executable(grpc_error_logging_interceptor_test + grpc/tests/grpc_error_logging_interceptor_test.cpp + grpc/src/grpc_error_logging_interceptor.cpp + ) + target_include_directories(grpc_error_logging_interceptor_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(grpc_error_logging_interceptor_test + PRIVATE + cmvr_es::logging + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_error_logging_interceptor_test + COMMAND grpc_error_logging_interceptor_test + ) + set_tests_properties(grpc_error_logging_interceptor_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/grpc/include/grpc_error_logging_interceptor.h b/cmvr-es/service/grpc/include/grpc_error_logging_interceptor.h new file mode 100644 index 00000000..f780aa5b --- /dev/null +++ b/cmvr-es/service/grpc/include/grpc_error_logging_interceptor.h @@ -0,0 +1,37 @@ +#pragma once + +#include +#include +#include + +#include + +namespace cmvr::service { + +enum class GrpcFailureKind { + APPLICATION, + GRPC_STATUS, + MALFORMED_RESPONSE, +}; + +enum class GrpcFailureSeverity { + INFO, + WARNING, + ERROR, +}; + +struct GrpcFailureRecord { + GrpcFailureKind kind{GrpcFailureKind::APPLICATION}; + std::string method; + std::string peer; + grpc::StatusCode status_code{grpc::StatusCode::OK}; + GrpcFailureSeverity severity{GrpcFailureSeverity::ERROR}; + std::string detail; +}; + +using GrpcFailureSink = std::function; + +std::unique_ptr +makeGrpcErrorLoggingInterceptorFactory(GrpcFailureSink sink = {}); + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/src/grpc_camera_service.cpp index f0a4dc64..1f7712b3 100644 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_camera_service.cpp @@ -21,7 +21,6 @@ using namespace cmvr::device; namespace { template grpc::Status failResponse(ResponseT* response, const std::string& message) { - CMVR_LOG(ERROR) << "[gRPCCameraServiceImpl] " << message; response->mutable_header()->set_success(false); response->mutable_header()->set_error_message(message); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); diff --git a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp b/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp index 6149aa6f..d0fcecc1 100644 --- a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp @@ -120,7 +120,6 @@ bool applyFreedomValues(const FreedomCollection& freedoms, template grpc::Status failResponse(ResponseT* response, const std::string& message) { - CMVR_LOG(ERROR) << "[gRPCDexHandServiceImpl] " << message; response->mutable_header()->set_success(false); response->mutable_header()->set_error_message(message); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); diff --git a/cmvr-es/service/grpc/src/grpc_error_logging_interceptor.cpp b/cmvr-es/service/grpc/src/grpc_error_logging_interceptor.cpp new file mode 100644 index 00000000..ecbe3da9 --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_error_logging_interceptor.cpp @@ -0,0 +1,504 @@ +#include "service/grpc/include/grpc_error_logging_interceptor.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "cmvr/api/common.pb.h" +#include "common/base/logging/logger.h" + +namespace cmvr::service { + +namespace { + +using Hook = grpc::experimental::InterceptionHookPoints; +using CodedInputStream = google::protobuf::io::CodedInputStream; +using WireFormatLite = google::protobuf::internal::WireFormatLite; + +constexpr std::string_view kFeedbackType = + "cmvr.api.CommandHeader.Feedback"; +constexpr std::size_t kMaxLogDetailBytes = 1024; +constexpr int kMaxFeedbackBytes = 64 * 1024; + +enum class ResponseShape { + NONE, + DIRECT_FEEDBACK, + ENVELOPE, +}; + +struct ApplicationFeedback { + bool present{false}; + bool parsed{false}; + bool success{false}; + std::string error_message; +}; + +ApplicationFeedback feedbackFromProto( + const api::CommandHeader_Feedback& message) +{ + return ApplicationFeedback{ + true, true, message.success(), message.error_message()}; +} + +std::string boundedDetail(const std::string& detail) +{ + if (detail.size() <= kMaxLogDetailBytes) { + return detail; + } + + constexpr std::string_view suffix = "... [truncated]"; + std::size_t length = kMaxLogDetailBytes - suffix.size(); + while (length > 0 && + (static_cast(detail[length]) & 0xc0U) == 0x80U) { + --length; + } + return detail.substr(0, length) + std::string(suffix); +} + +const char* failureKindName(const GrpcFailureKind kind) +{ + switch (kind) { + case GrpcFailureKind::APPLICATION: + return "application"; + case GrpcFailureKind::GRPC_STATUS: + return "grpc_status"; + case GrpcFailureKind::MALFORMED_RESPONSE: + return "malformed_response"; + } + return "unknown"; +} + +const char* statusCodeName(const grpc::StatusCode code) +{ + switch (code) { + case grpc::StatusCode::OK: return "OK"; + case grpc::StatusCode::CANCELLED: return "CANCELLED"; + case grpc::StatusCode::UNKNOWN: return "UNKNOWN"; + case grpc::StatusCode::INVALID_ARGUMENT: return "INVALID_ARGUMENT"; + case grpc::StatusCode::DEADLINE_EXCEEDED: return "DEADLINE_EXCEEDED"; + case grpc::StatusCode::NOT_FOUND: return "NOT_FOUND"; + case grpc::StatusCode::ALREADY_EXISTS: return "ALREADY_EXISTS"; + case grpc::StatusCode::PERMISSION_DENIED: return "PERMISSION_DENIED"; + case grpc::StatusCode::RESOURCE_EXHAUSTED: return "RESOURCE_EXHAUSTED"; + case grpc::StatusCode::FAILED_PRECONDITION: return "FAILED_PRECONDITION"; + case grpc::StatusCode::ABORTED: return "ABORTED"; + case grpc::StatusCode::OUT_OF_RANGE: return "OUT_OF_RANGE"; + case grpc::StatusCode::UNIMPLEMENTED: return "UNIMPLEMENTED"; + case grpc::StatusCode::INTERNAL: return "INTERNAL"; + case grpc::StatusCode::UNAVAILABLE: return "UNAVAILABLE"; + case grpc::StatusCode::DATA_LOSS: return "DATA_LOSS"; + case grpc::StatusCode::UNAUTHENTICATED: return "UNAUTHENTICATED"; + case grpc::StatusCode::DO_NOT_USE: break; + } + return "UNKNOWN_CODE"; +} + +void logFailure(const GrpcFailureRecord& record) +{ + std::ostringstream message; + message << "[gRPC] request failed, method=" << record.method + << ", peer=" << (record.peer.empty() ? "unknown" : record.peer) + << ", kind=" << failureKindName(record.kind) + << ", code=" << static_cast(record.status_code) + << '(' << statusCodeName(record.status_code) << ')' + << ", detail=" << boundedDetail(record.detail); + switch (record.severity) { + case GrpcFailureSeverity::INFO: + CMVR_LOG(INFO) << message.str(); + break; + case GrpcFailureSeverity::WARNING: + CMVR_LOG(WARNING) << message.str(); + logging::Logger::instance().flush(); + break; + case GrpcFailureSeverity::ERROR: + CMVR_LOG(ERROR) << message.str(); + break; + } +} + +GrpcFailureSeverity severityForStatus(const grpc::StatusCode code) +{ + switch (code) { + case grpc::StatusCode::CANCELLED: + case grpc::StatusCode::INVALID_ARGUMENT: + case grpc::StatusCode::DEADLINE_EXCEEDED: + case grpc::StatusCode::NOT_FOUND: + case grpc::StatusCode::ALREADY_EXISTS: + case grpc::StatusCode::PERMISSION_DENIED: + case grpc::StatusCode::RESOURCE_EXHAUSTED: + case grpc::StatusCode::FAILED_PRECONDITION: + case grpc::StatusCode::ABORTED: + case grpc::StatusCode::OUT_OF_RANGE: + case grpc::StatusCode::UNIMPLEMENTED: + case grpc::StatusCode::UNAVAILABLE: + case grpc::StatusCode::UNAUTHENTICATED: + return GrpcFailureSeverity::WARNING; + case grpc::StatusCode::OK: + case grpc::StatusCode::UNKNOWN: + case grpc::StatusCode::INTERNAL: + case grpc::StatusCode::DATA_LOSS: + case grpc::StatusCode::DO_NOT_USE: + return GrpcFailureSeverity::ERROR; + } + return GrpcFailureSeverity::ERROR; +} + +bool splitMethodName(const std::string_view full_method, + std::string_view& service_name, + std::string_view& method_name) +{ + if (full_method.empty()) { + return false; + } + const std::size_t service_begin = full_method.front() == '/' ? 1 : 0; + const std::size_t separator = full_method.find('/', service_begin); + if (separator == std::string_view::npos || + separator == service_begin || separator + 1 >= full_method.size()) { + return false; + } + service_name = full_method.substr(service_begin, separator - service_begin); + method_name = full_method.substr(separator + 1); + return true; +} + +ResponseShape responseShapeForMethod(const std::string_view full_method) +{ + std::string_view service_name; + std::string_view method_name; + if (!splitMethodName(full_method, service_name, method_name)) { + return ResponseShape::NONE; + } + + const auto* pool = google::protobuf::DescriptorPool::generated_pool(); + const auto* service = pool->FindServiceByName(std::string(service_name)); + if (service == nullptr) { + return ResponseShape::NONE; + } + const auto* method = service->FindMethodByName(std::string(method_name)); + if (method == nullptr || method->output_type() == nullptr) { + return ResponseShape::NONE; + } + + const auto* output = method->output_type(); + if (output->full_name() == kFeedbackType) { + return ResponseShape::DIRECT_FEEDBACK; + } + + const auto* header = output->FindFieldByNumber(1); + if (header == nullptr || header->name() != "header" || + header->is_repeated() || + header->cpp_type() != google::protobuf::FieldDescriptor::CPPTYPE_MESSAGE || + header->message_type() == nullptr || + header->message_type()->full_name() != kFeedbackType) { + return ResponseShape::NONE; + } + return ResponseShape::ENVELOPE; +} + +bool mergeFeedback(CodedInputStream& input, + api::CommandHeader_Feedback& feedback) +{ + return feedback.MergePartialFromCodedStream(&input) && + input.ConsumedEntireMessage(); +} + +bool mergeBoundedFeedback(CodedInputStream& input, + api::CommandHeader_Feedback& feedback) +{ + std::uint32_t length = 0; + if (!input.ReadVarint32(&length) || + length > static_cast(kMaxFeedbackBytes) || + length > static_cast(std::numeric_limits::max())) { + return false; + } + + std::string payload; + if (!input.ReadString(&payload, static_cast(length))) { + return false; + } + return feedback.MergeFromString(payload); +} + +ApplicationFeedback parseDirectFeedback(grpc::ByteBuffer& buffer) +{ + ApplicationFeedback feedback; + grpc::ProtoBufferReader reader(&buffer); + if (!reader.status().ok()) { + return feedback; + } + CodedInputStream input(&reader); + input.SetTotalBytesLimit(kMaxFeedbackBytes); + api::CommandHeader_Feedback message; + if (!mergeFeedback(input, message)) { + feedback.present = true; + return feedback; + } + return feedbackFromProto(message); +} + +ApplicationFeedback parseEnvelopeFeedback(grpc::ByteBuffer& buffer) +{ + ApplicationFeedback feedback; + grpc::ProtoBufferReader reader(&buffer); + if (!reader.status().ok()) { + return feedback; + } + CodedInputStream input(&reader); + const std::uint32_t tag = input.ReadTag(); + if (tag == 0 || WireFormatLite::GetTagFieldNumber(tag) != 1) { + feedback.parsed = true; + return feedback; + } + + feedback.present = true; + if (WireFormatLite::GetTagWireType(tag) != + WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { + return feedback; + } + + api::CommandHeader_Feedback message; + if (!mergeBoundedFeedback(input, message)) { + return feedback; + } + return feedbackFromProto(message); +} + +class GrpcErrorLoggingInterceptor final + : public grpc::experimental::Interceptor { +public: + GrpcErrorLoggingInterceptor(std::string method, + grpc::ServerContextBase* context, + const ResponseShape response_shape, + const bool server_streaming, + const bool client_streaming, + GrpcFailureSink sink) + : method_(std::move(method)), + context_(context), + peer_(context == nullptr ? std::string{} : context->peer()), + response_shape_(response_shape), + server_streaming_(server_streaming), + client_streaming_(client_streaming), + sink_(std::move(sink)) + { + } + + void Intercept(grpc::experimental::InterceptorBatchMethods* methods) override + { + if (methods->QueryInterceptionHookPoint(Hook::PRE_SEND_CANCEL)) { + // gRPC forbids delaying this hook. Only publish the signal here; + // the final status hook performs any logging. + server_cancel_requested_.store(true, std::memory_order_release); + return; + } + if (methods->QueryInterceptionHookPoint(Hook::PRE_SEND_MESSAGE)) { + inspectResponse(methods->GetSerializedSendMessage()); + } + if (methods->QueryInterceptionHookPoint(Hook::PRE_SEND_STATUS)) { + inspectStatus(methods->GetSendStatus()); + } + methods->Proceed(); + } + +private: + void emit(GrpcFailureRecord record) + { + record.method = method_; + record.peer = peer_; + record.detail = boundedDetail(record.detail); + if (sink_) { + std::lock_guard lock(sink_mutex_); + sink_(record); + } else { + logFailure(record); + } + } + + void inspectResponse(grpc::ByteBuffer* buffer) + { + if (response_shape_ == ResponseShape::NONE || + buffer == nullptr || !buffer->Valid()) { + return; + } + + ApplicationFeedback feedback = + response_shape_ == ResponseShape::DIRECT_FEEDBACK + ? parseDirectFeedback(*buffer) + : parseEnvelopeFeedback(*buffer); + std::optional failure; + if (!feedback.present) { + failure = GrpcFailureRecord{ + GrpcFailureKind::MALFORMED_RESPONSE, + {}, {}, grpc::StatusCode::INTERNAL, + GrpcFailureSeverity::ERROR, + "response is missing CommandHeader.Feedback"}; + } else if (!feedback.parsed) { + failure = GrpcFailureRecord{ + GrpcFailureKind::MALFORMED_RESPONSE, + {}, {}, grpc::StatusCode::INTERNAL, + GrpcFailureSeverity::ERROR, + "response contains an invalid CommandHeader.Feedback"}; + } else if (!feedback.success) { + failure = GrpcFailureRecord{ + GrpcFailureKind::APPLICATION, + {}, {}, grpc::StatusCode::OK, + GrpcFailureSeverity::ERROR, + feedback.error_message.empty() + ? "operation failed without an error message" + : feedback.error_message}; + } + + if (!failure.has_value()) { + return; + } + if (server_streaming_) { + emit(std::move(*failure)); + return; + } + pending_failure_ = std::move(*failure); + } + + void inspectStatus(const grpc::Status& status) + { + const grpc::StatusCode status_code = status.error_code(); + std::string status_detail = status.error_message(); + if (!status.ok() && status_detail.empty()) { + status_detail = "RPC completed with a non-OK status"; + } + + if (!status.ok()) { + // A non-OK unary/client-streaming status suppresses the response + // body, so only the status is visible to the platform. + pending_failure_.reset(); + if (status_code == grpc::StatusCode::CANCELLED) { + emitCancellationOnce(status_code, std::move(status_detail)); + return; + } + emit({GrpcFailureKind::GRPC_STATUS, + {}, {}, status_code, + severityForStatus(status_code), + status_detail}); + return; + } + + const bool deadline_expired = + context_ != nullptr && + context_->deadline() <= std::chrono::system_clock::now(); + // IsCancelled() waits for the client close on the synchronous API. + // Unary requests have already sent that close, but querying it here + // could block an early client-streaming/bidi response indefinitely. + if (deadline_expired || + (context_ != nullptr && !client_streaming_ && + context_->IsCancelled())) { + pending_failure_.reset(); + const grpc::StatusCode code = deadline_expired + ? grpc::StatusCode::DEADLINE_EXCEEDED + : grpc::StatusCode::CANCELLED; + std::string detail; + if (deadline_expired) { + detail = "RPC deadline expired before completion"; + } else if (server_cancel_requested_.load( + std::memory_order_acquire)) { + detail = + "RPC cancellation was requested by the edge server before completion"; + } else { + detail = + "RPC was cancelled by the client or transport before completion"; + } + emitCancellationOnce(code, std::move(detail)); + return; + } + + if (server_cancel_requested_.load(std::memory_order_acquire)) { + pending_failure_.reset(); + emitCancellationOnce( + grpc::StatusCode::CANCELLED, + "RPC cancellation was requested by the edge server before completion"); + return; + } + + if (pending_failure_.has_value()) { + emit(std::move(*pending_failure_)); + pending_failure_.reset(); + } + } + + void emitCancellationOnce(const grpc::StatusCode code, std::string detail) + { + if (cancellation_recorded_.exchange(true, std::memory_order_acq_rel)) { + return; + } + emit({GrpcFailureKind::GRPC_STATUS, + {}, {}, code, severityForStatus(code), std::move(detail)}); + } + + std::string method_; + grpc::ServerContextBase* context_{nullptr}; + std::string peer_; + ResponseShape response_shape_{ResponseShape::NONE}; + bool server_streaming_{false}; + bool client_streaming_{false}; + GrpcFailureSink sink_; + std::optional pending_failure_; + std::mutex sink_mutex_; + std::atomic server_cancel_requested_{false}; + std::atomic cancellation_recorded_{false}; +}; + +class GrpcErrorLoggingInterceptorFactory final + : public grpc::experimental::ServerInterceptorFactoryInterface { +public: + explicit GrpcErrorLoggingInterceptorFactory(GrpcFailureSink sink) + : sink_(std::move(sink)) + { + } + + grpc::experimental::Interceptor* CreateServerInterceptor( + grpc::experimental::ServerRpcInfo* info) override + { + const std::string method = + info != nullptr && info->method() != nullptr + ? info->method() + : ""; + const bool server_streaming = + info != nullptr && + (info->type() == grpc::experimental::ServerRpcInfo::Type::SERVER_STREAMING || + info->type() == grpc::experimental::ServerRpcInfo::Type::BIDI_STREAMING); + const bool client_streaming = + info != nullptr && + (info->type() == grpc::experimental::ServerRpcInfo::Type::CLIENT_STREAMING || + info->type() == grpc::experimental::ServerRpcInfo::Type::BIDI_STREAMING); + return new GrpcErrorLoggingInterceptor( + method, + info == nullptr ? nullptr : info->server_context(), + responseShapeForMethod(method), server_streaming, + client_streaming, sink_); + } + +private: + GrpcFailureSink sink_; +}; + +} // namespace + +std::unique_ptr +makeGrpcErrorLoggingInterceptorFactory(GrpcFailureSink sink) +{ + return std::make_unique( + std::move(sink)); +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/src/grpc_head_service.cpp b/cmvr-es/service/grpc/src/grpc_head_service.cpp index 9ba03019..8e6934e7 100644 --- a/cmvr-es/service/grpc/src/grpc_head_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_head_service.cpp @@ -20,7 +20,6 @@ using namespace cmvr::api; namespace { template grpc::Status failResponse(ResponseT* response, const std::string& message) { - CMVR_LOG(ERROR) << "[gRPCMBioHeadServiceImpl] " << message; response->mutable_header()->set_success(false); response->mutable_header()->set_error_message(message); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -150,7 +149,6 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression( if (first_message) { dev_id = request_msg.header().device_id(); if (dev_id.empty()) { - CMVR_LOG(ERROR) << "[gRPCMBioHeadServiceImpl] Device ID is empty in first message"; feedback_msg.mutable_header()->set_success(false); feedback_msg.mutable_header()->set_error_message("Device ID is empty in first message"); setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp()); @@ -160,7 +158,6 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression( robot = dmgr_.getDevice(dev_id); if (!robot) { const std::string message = "Biohead device not found: " + dev_id; - CMVR_LOG(ERROR) << "[gRPCMBioHeadServiceImpl] " << message; feedback_msg.mutable_header()->set_success(false); feedback_msg.mutable_header()->set_error_message(message); setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp()); @@ -259,7 +256,6 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression( CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (StreamExpression): finished, id=" << dev_id; return grpc::Status::OK; } catch (const std::exception& e) { - CMVR_LOG(ERROR) << "StreamExpression error: " << e.what(); feedback_msg.mutable_header()->set_success(false); feedback_msg.mutable_header()->set_error_message(e.what()); setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp()); diff --git a/cmvr-es/service/grpc/src/grpc_microphone_service.cpp b/cmvr-es/service/grpc/src/grpc_microphone_service.cpp index c44a0f57..e2cb0272 100644 --- a/cmvr-es/service/grpc/src/grpc_microphone_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_microphone_service.cpp @@ -18,7 +18,6 @@ using namespace cmvr::service; namespace { template grpc::Status failResponse(ResponseT* response, const std::string& message) { - CMVR_LOG(ERROR) << "[gRPCMicroPhoneServiceImpl] " << message; response->mutable_header()->set_success(false); response->mutable_header()->set_error_message(message); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); diff --git a/cmvr-es/service/grpc/src/grpc_speaker_service.cpp b/cmvr-es/service/grpc/src/grpc_speaker_service.cpp index 4eaf03dd..f80a257f 100644 --- a/cmvr-es/service/grpc/src/grpc_speaker_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_speaker_service.cpp @@ -14,7 +14,6 @@ using namespace cmvr::service; namespace { template grpc::Status failResponse(ResponseT* response, const std::string& message) { - CMVR_LOG(ERROR) << "[gRPCSpeakerServiceImpl] " << message; response->mutable_header()->set_success(false); response->mutable_header()->set_error_message(message); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); diff --git a/cmvr-es/service/grpc/tests/grpc_error_logging_interceptor_test.cpp b/cmvr-es/service/grpc/tests/grpc_error_logging_interceptor_test.cpp new file mode 100644 index 00000000..02b7adca --- /dev/null +++ b/cmvr-es/service/grpc/tests/grpc_error_logging_interceptor_test.cpp @@ -0,0 +1,864 @@ +#include "service/grpc/include/grpc_error_logging_interceptor.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "cmvr/api/agv_service.grpc.pb.h" +#include "cmvr/api/camera_service.grpc.pb.h" +#include "cmvr/api/test_service.grpc.pb.h" +#include "common/base/logging/logger.h" + +namespace cmvr::service { +namespace { + +std::atomic g_socket_sequence{0}; + +void setRpcDeadline(grpc::ClientContext& context) +{ + context.set_deadline( + std::chrono::system_clock::now() + std::chrono::seconds(3)); +} + +grpc::ByteBuffer byteBuffer(const std::string& payload) +{ + const grpc::Slice slice(payload); + return grpc::ByteBuffer(&slice, 1); +} + +std::string byteBufferString(const grpc::ByteBuffer& buffer) +{ + std::vector slices; + EXPECT_TRUE(buffer.Dump(&slices).ok()); + std::string payload; + for (const auto& slice : slices) { + payload.append( + reinterpret_cast(slice.begin()), slice.size()); + } + return payload; +} + +grpc::Status rawUnaryCall(const std::shared_ptr& channel, + const std::string& method, + const std::string& request_payload, + grpc::ByteBuffer& response) +{ + grpc::ClientContext context; + setRpcDeadline(context); + const auto request = byteBuffer(request_payload); + const grpc::internal::RpcMethod rpc_method( + method.c_str(), grpc::internal::RpcMethod::NORMAL_RPC); + return grpc::internal::BlockingUnaryCall( + channel.get(), rpc_method, &context, request, &response); +} + +class ErrorLoggingTestService final : public api::TestService::Service { +public: + grpc::Status Call(grpc::ServerContext* context, + const api::TestReqeust* request, + api::TestResponse* response) override + { + if (request->data() == "grpc-error") { + return grpc::Status( + grpc::StatusCode::INTERNAL, "transport sentinel"); + } + if (request->data() == "cancelled") { + return grpc::Status( + grpc::StatusCode::CANCELLED, "client closed stream"); + } + if (request->data() == "server-cancel") { + context->TryCancel(); + return grpc::Status::OK; + } + if (request->data() == "client-cancel") { + { + std::lock_guard lock(cancel_mutex_); + cancel_started_ = true; + } + cancel_started_condition_.notify_all(); + const auto timeout = + std::chrono::steady_clock::now() + std::chrono::seconds(3); + while (!context->IsCancelled() && + std::chrono::steady_clock::now() < timeout) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + return grpc::Status::OK; + } + response->set_data("ok"); + return grpc::Status::OK; + } + + bool waitForCancelableCall() + { + std::unique_lock lock(cancel_mutex_); + return cancel_started_condition_.wait_for( + lock, std::chrono::seconds(3), + [this] { return cancel_started_; }); + } + +private: + std::mutex cancel_mutex_; + std::condition_variable cancel_started_condition_; + bool cancel_started_{false}; +}; + +class ErrorLoggingAgvService final : public api::AgvService::Service { +public: + grpc::Status emergencyStop( + grpc::ServerContext*, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override + { + if (request->device_id() == "missing") { + response->set_success(false); + response->set_error_message("missing sentinel"); + return grpc::Status( + grpc::StatusCode::NOT_FOUND, "missing sentinel"); + } + response->set_success(false); + response->set_error_message("direct sentinel"); + return grpc::Status::OK; + } + + grpc::Status streamMap( + grpc::ServerContext*, + const api::AgvMapStreamCommand_Request* request, + grpc::ServerWriter* writer) override + { + const auto write_failure = [writer](const std::string& detail) { + api::AgvMapStreamCommand_Feedback response; + response.mutable_header()->set_success(false); + response.mutable_header()->set_error_message(detail); + return writer->Write(response); + }; + + if (request->resume_token() == "multiple") { + write_failure("first stream sentinel"); + write_failure("second stream sentinel"); + return grpc::Status::OK; + } + if (request->resume_token() == "hold") { + write_failure("immediate stream sentinel"); + std::unique_lock lock(stream_mutex_); + stream_released_.wait_for( + lock, std::chrono::seconds(3), + [this] { return release_stream_; }); + return grpc::Status::OK; + } + if (request->resume_token() == "different") { + write_failure("application stream sentinel"); + return grpc::Status( + grpc::StatusCode::INTERNAL, "status stream sentinel"); + } + if (request->resume_token() == "long-different") { + const std::string common_prefix(1100, 'x'); + write_failure(common_prefix + " application"); + return grpc::Status( + grpc::StatusCode::INTERNAL, + common_prefix + " status"); + } + + write_failure("stream sentinel"); + return grpc::Status( + grpc::StatusCode::INTERNAL, "stream sentinel"); + } + + void releaseHeldStream() + { + { + std::lock_guard lock(stream_mutex_); + release_stream_ = true; + } + stream_released_.notify_all(); + } + +private: + std::mutex stream_mutex_; + std::condition_variable stream_released_; + bool release_stream_{false}; +}; + +class ErrorLoggingCameraService final : public api::CameraService::Service { +public: + grpc::Status StartCamera( + grpc::ServerContext*, + const api::StartCameraCommand_Request*, + api::StartCameraCommand_Feedback* response) override + { + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message("nested sentinel"); + return grpc::Status::OK; + } + + grpc::Status GetRGBImage( + grpc::ServerContext*, + const api::GetRGBImageCommand_Request*, + api::GetRGBImageCommand_Feedback* response) override + { + response->mutable_header()->set_success(true); + response->mutable_color_frame()->set_data( + std::string(2 * 1024 * 1024, 'x')); + return grpc::Status::OK; + } +}; + +class RawCameraService final + : public api::CameraService::WithRawCallbackMethod_StartCamera< + api::CameraService::Service> { +public: + explicit RawCameraService(std::string response) + : response_(std::move(response)) + { + } + + grpc::ServerUnaryReactor* StartCamera( + grpc::CallbackServerContext* context, + const grpc::ByteBuffer*, + grpc::ByteBuffer* response) override + { + *response = byteBuffer(response_); + auto* reactor = context->DefaultReactor(); + reactor->Finish(grpc::Status::OK); + return reactor; + } + +private: + std::string response_; +}; + +struct RawCallResult { + grpc::Status status; + std::vector records; + std::string response; +}; + +class ScopedServer final { +public: + ScopedServer(std::unique_ptr server, + std::string socket_path = {}) + : server_(std::move(server)), socket_path_(std::move(socket_path)) + { + } + + ~ScopedServer() + { + if (server_) { + server_->Shutdown(); + server_->Wait(); + } + if (!socket_path_.empty()) { + std::remove(socket_path_.c_str()); + } + } + + grpc::Server* get() const { return server_.get(); } + +private: + std::unique_ptr server_; + std::string socket_path_; +}; + +class ScopedSocket final { +public: + explicit ScopedSocket(std::string path) : path_(std::move(path)) + { + std::remove(path_.c_str()); + } + + ~ScopedSocket() { std::remove(path_.c_str()); } + +private: + std::string path_; +}; + +RawCallResult callRawStartCamera(const std::string& response_payload) +{ + std::mutex records_mutex; + std::vector records; + RawCameraService service(response_payload); + grpc::ServerBuilder builder; + const std::string address = + "unix:/tmp/cmvr_grpc_error_logging_raw_" + + std::to_string(static_cast(::getpid())) + "_" + + std::to_string(g_socket_sequence.fetch_add(1U)) + ".sock"; + const std::string socket_path = address.substr(5); + ScopedSocket socket(socket_path); + builder.AddListeningPort(address, grpc::InsecureServerCredentials()); + builder.RegisterService(&service); + std::vector> factories; + factories.emplace_back(makeGrpcErrorLoggingInterceptorFactory( + [&records, &records_mutex](const GrpcFailureRecord& record) { + std::lock_guard lock(records_mutex); + records.push_back(record); + })); + builder.experimental().SetInterceptorCreators(std::move(factories)); + ScopedServer server(builder.BuildAndStart()); + EXPECT_NE(server.get(), nullptr); + if (server.get() == nullptr) { + return {}; + } + + const auto channel = grpc::CreateChannel( + address, grpc::InsecureChannelCredentials()); + api::StartCameraCommand_Request request; + grpc::ByteBuffer response; + const auto status = rawUnaryCall( + channel, "/cmvr.api.CameraService/StartCamera", + request.SerializeAsString(), response); + std::lock_guard lock(records_mutex); + return {status, records, byteBufferString(response)}; +} + +class GrpcErrorLoggingInterceptorTest : public testing::Test { +protected: + void SetUp() override + { + grpc::ServerBuilder builder; + socket_path_ = + "/tmp/cmvr_grpc_error_logging_interceptor_test_" + + std::to_string(static_cast(::getpid())) + "_" + + std::to_string(g_socket_sequence.fetch_add(1U)) + ".sock"; + std::remove(socket_path_.c_str()); + const std::string address = "unix:" + socket_path_; + builder.AddListeningPort( + address, grpc::InsecureServerCredentials()); + builder.RegisterService(&test_service_); + builder.RegisterService(&agv_service_); + builder.RegisterService(&camera_service_); + + std::vector> factories; + factories.emplace_back(makeGrpcErrorLoggingInterceptorFactory( + [this](const GrpcFailureRecord& record) { + { + std::lock_guard lock(records_mutex_); + records_.push_back(record); + } + records_changed_.notify_all(); + })); + builder.experimental().SetInterceptorCreators(std::move(factories)); + server_ = builder.BuildAndStart(); + ASSERT_NE(server_, nullptr); + + const auto channel = grpc::CreateChannel( + address, + grpc::InsecureChannelCredentials()); + test_stub_ = api::TestService::NewStub(channel); + agv_stub_ = api::AgvService::NewStub(channel); + camera_stub_ = api::CameraService::NewStub(channel); + } + + void TearDown() override + { + if (server_) { + server_->Shutdown(); + server_->Wait(); + } + test_stub_.reset(); + agv_stub_.reset(); + camera_stub_.reset(); + if (!socket_path_.empty()) { + std::remove(socket_path_.c_str()); + } + } + + std::vector records() + { + std::lock_guard lock(records_mutex_); + return records_; + } + + std::vector waitForRecords( + const std::size_t count, + const std::chrono::milliseconds timeout = std::chrono::seconds(3)) + { + std::unique_lock lock(records_mutex_); + records_changed_.wait_for( + lock, timeout, [this, count] { return records_.size() >= count; }); + return records_; + } + + ErrorLoggingTestService test_service_; + ErrorLoggingAgvService agv_service_; + ErrorLoggingCameraService camera_service_; + std::unique_ptr server_; + std::unique_ptr test_stub_; + std::unique_ptr agv_stub_; + std::unique_ptr camera_stub_; + std::string socket_path_; + std::mutex records_mutex_; + std::condition_variable records_changed_; + std::vector records_; +}; + +TEST_F(GrpcErrorLoggingInterceptorTest, RecordsNonOkGrpcStatus) +{ + grpc::ClientContext context; + setRpcDeadline(context); + api::TestReqeust request; + api::TestResponse response; + request.set_data("grpc-error"); + + const grpc::Status status = test_stub_->Call(&context, request, &response); + ASSERT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); + + const auto captured = waitForRecords(1U); + ASSERT_EQ(captured.size(), 1U); + EXPECT_EQ(captured.front().kind, GrpcFailureKind::GRPC_STATUS); + EXPECT_EQ(captured.front().method, "/cmvr.api.TestService/Call"); + EXPECT_EQ(captured.front().status_code, grpc::StatusCode::INTERNAL); + EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::ERROR); + EXPECT_EQ(captured.front().detail, "transport sentinel"); +} + +TEST_F(GrpcErrorLoggingInterceptorTest, RecordsDirectFeedbackFailure) +{ + grpc::ClientContext context; + setRpcDeadline(context); + api::CommandHeader_Request request; + api::CommandHeader_Feedback response; + + ASSERT_TRUE(agv_stub_->emergencyStop(&context, request, &response).ok()); + ASSERT_FALSE(response.success()); + + const auto captured = waitForRecords(1U); + ASSERT_EQ(captured.size(), 1U); + EXPECT_EQ(captured.front().kind, GrpcFailureKind::APPLICATION); + EXPECT_EQ(captured.front().method, "/cmvr.api.AgvService/emergencyStop"); + EXPECT_EQ(captured.front().status_code, grpc::StatusCode::OK); + EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::ERROR); + EXPECT_EQ(captured.front().detail, "direct sentinel"); +} + +TEST_F(GrpcErrorLoggingInterceptorTest, RecordsCancellationAsWarning) +{ + grpc::ClientContext context; + setRpcDeadline(context); + api::TestReqeust request; + api::TestResponse response; + request.set_data("cancelled"); + + const grpc::Status status = test_stub_->Call(&context, request, &response); + ASSERT_EQ(status.error_code(), grpc::StatusCode::CANCELLED); + + const auto captured = waitForRecords(1U); + ASSERT_EQ(captured.size(), 1U); + EXPECT_EQ(captured.front().kind, GrpcFailureKind::GRPC_STATUS); + EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::WARNING); + EXPECT_EQ(captured.front().detail, "client closed stream"); +} + +TEST(GrpcErrorLoggingInterceptorFileTest, WarningStatusIsFlushedToFile) +{ + std::array directory_pattern{}; + const std::string pattern = "/tmp/cmvr-grpc-log-test-XXXXXX"; + std::copy(pattern.begin(), pattern.end(), directory_pattern.begin()); + const char* created = ::mkdtemp(directory_pattern.data()); + ASSERT_NE(created, nullptr); + const std::filesystem::path directory(created); + + struct Cleanup { + ~Cleanup() + { + logging::shutdownLogging(); + if (!socket_path.empty()) { + std::remove(socket_path.c_str()); + } + std::error_code error; + std::filesystem::remove_all(directory, error); + } + + std::filesystem::path directory; + std::string socket_path; + } cleanup{directory, {}}; + + config::LoggerConfig config; + config.set_minimum_level(config::LOG_LEVEL_WARNING); + config.set_directory(directory.string()); + config.set_flush_interval_seconds(3600); + auto* warning_route = config.add_routes(); + warning_route->set_level(config::LOG_LEVEL_WARNING); + warning_route->set_file(true); + ASSERT_TRUE(logging::initLogging(config, "grpc_log_test", directory)); + + ErrorLoggingTestService service; + grpc::ServerBuilder builder; + const std::string socket_path = + "/tmp/cmvr_grpc_error_logging_file_test_" + + std::to_string(static_cast(::getpid())) + "_" + + std::to_string(g_socket_sequence.fetch_add(1U)) + ".sock"; + cleanup.socket_path = socket_path; + ScopedSocket socket(socket_path); + const std::string address = "unix:" + socket_path; + builder.AddListeningPort(address, grpc::InsecureServerCredentials()); + builder.RegisterService(&service); + std::vector> factories; + factories.emplace_back(makeGrpcErrorLoggingInterceptorFactory()); + builder.experimental().SetInterceptorCreators(std::move(factories)); + ScopedServer server(builder.BuildAndStart()); + ASSERT_NE(server.get(), nullptr); + + const auto channel = grpc::CreateChannel( + address, grpc::InsecureChannelCredentials()); + auto stub = api::TestService::NewStub(channel); + grpc::ClientContext context; + setRpcDeadline(context); + api::TestReqeust request; + api::TestResponse response; + request.set_data("cancelled"); + const grpc::Status status = stub->Call(&context, request, &response); + ASSERT_EQ(status.error_code(), grpc::StatusCode::CANCELLED); + + std::ifstream input(directory / "grpc_log_test.log"); + const std::string contents{ + std::istreambuf_iterator(input), + std::istreambuf_iterator()}; + EXPECT_NE(contents.find("/cmvr.api.TestService/Call"), std::string::npos); + EXPECT_NE(contents.find("code=1(CANCELLED)"), std::string::npos); + EXPECT_NE(contents.find("client closed stream"), std::string::npos); + +} + +TEST_F(GrpcErrorLoggingInterceptorTest, RecordsServerCancellationReturnedAsOk) +{ + grpc::ClientContext context; + setRpcDeadline(context); + api::TestReqeust request; + api::TestResponse response; + request.set_data("server-cancel"); + + const grpc::Status status = test_stub_->Call(&context, request, &response); + ASSERT_EQ(status.error_code(), grpc::StatusCode::CANCELLED); + + // TryCancel is best effort: the final status may be intercepted before + // the cancellation hook. Either way, the interceptor must not recurse or + // emit duplicate records. + const auto captured = waitForRecords(1U, std::chrono::milliseconds(50)); + ASSERT_LE(captured.size(), 1U); + if (!captured.empty()) { + EXPECT_EQ(captured.front().kind, GrpcFailureKind::GRPC_STATUS); + EXPECT_EQ(captured.front().status_code, grpc::StatusCode::CANCELLED); + EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::WARNING); + EXPECT_EQ( + captured.front().detail, + "RPC cancellation was requested by the edge server before completion"); + } +} + +TEST_F(GrpcErrorLoggingInterceptorTest, RecordsClientCancellation) +{ + grpc::ClientContext context; + setRpcDeadline(context); + api::TestReqeust request; + api::TestResponse response; + request.set_data("client-cancel"); + + grpc::Status status; + std::thread call([&] { + status = test_stub_->Call(&context, request, &response); + }); + const bool call_started = test_service_.waitForCancelableCall(); + context.TryCancel(); + call.join(); + + ASSERT_TRUE(call_started); + ASSERT_EQ(status.error_code(), grpc::StatusCode::CANCELLED); + const auto captured = waitForRecords(1U); + ASSERT_EQ(captured.size(), 1U); + EXPECT_EQ(captured.front().kind, GrpcFailureKind::GRPC_STATUS); + EXPECT_EQ(captured.front().status_code, grpc::StatusCode::CANCELLED); + EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::WARNING); + EXPECT_EQ( + captured.front().detail, + "RPC was cancelled by the client or transport before completion"); +} + +TEST_F(GrpcErrorLoggingInterceptorTest, + RecordsStatusWhenNonOkSuppressesUnaryResponse) +{ + grpc::ClientContext context; + setRpcDeadline(context); + api::CommandHeader_Request request; + api::CommandHeader_Feedback response; + request.set_device_id("missing"); + + const grpc::Status status = + agv_stub_->emergencyStop(&context, request, &response); + ASSERT_EQ(status.error_code(), grpc::StatusCode::NOT_FOUND); + + const auto captured = waitForRecords(1U); + ASSERT_EQ(captured.size(), 1U); + EXPECT_EQ(captured.front().kind, GrpcFailureKind::GRPC_STATUS); + EXPECT_EQ(captured.front().status_code, grpc::StatusCode::NOT_FOUND); + EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::WARNING); + EXPECT_EQ(captured.front().detail, "missing sentinel"); +} + +TEST_F(GrpcErrorLoggingInterceptorTest, RecordsNestedFeedbackFailure) +{ + grpc::ClientContext context; + setRpcDeadline(context); + api::StartCameraCommand_Request request; + api::StartCameraCommand_Feedback response; + + ASSERT_TRUE(camera_stub_->StartCamera(&context, request, &response).ok()); + ASSERT_FALSE(response.header().success()); + + const auto captured = waitForRecords(1U); + ASSERT_EQ(captured.size(), 1U); + EXPECT_EQ(captured.front().kind, GrpcFailureKind::APPLICATION); + EXPECT_EQ(captured.front().method, "/cmvr.api.CameraService/StartCamera"); + EXPECT_EQ(captured.front().detail, "nested sentinel"); +} + +TEST_F(GrpcErrorLoggingInterceptorTest, + RecordsStreamingResponseAndFinalNonOkStatus) +{ + grpc::ClientContext context; + setRpcDeadline(context); + api::AgvMapStreamCommand_Request request; + auto reader = agv_stub_->streamMap(&context, request); + api::AgvMapStreamCommand_Feedback response; + + ASSERT_TRUE(reader->Read(&response)); + ASSERT_FALSE(response.header().success()); + const grpc::Status status = reader->Finish(); + ASSERT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); + + const auto captured = waitForRecords(2U); + ASSERT_EQ(captured.size(), 2U); + EXPECT_EQ(captured[0].kind, GrpcFailureKind::APPLICATION); + EXPECT_EQ(captured[0].method, "/cmvr.api.AgvService/streamMap"); + EXPECT_EQ(captured[0].status_code, grpc::StatusCode::OK); + EXPECT_EQ(captured[0].severity, GrpcFailureSeverity::ERROR); + EXPECT_EQ(captured[0].detail, "stream sentinel"); + EXPECT_EQ(captured[1].kind, GrpcFailureKind::GRPC_STATUS); + EXPECT_EQ(captured[1].status_code, grpc::StatusCode::INTERNAL); + EXPECT_EQ(captured[1].severity, GrpcFailureSeverity::ERROR); + EXPECT_EQ(captured[1].detail, "stream sentinel"); +} + +TEST_F(GrpcErrorLoggingInterceptorTest, + RecordsStreamingFailureBeforeTheStreamFinishes) +{ + grpc::ClientContext context; + setRpcDeadline(context); + api::AgvMapStreamCommand_Request request; + request.set_resume_token("hold"); + auto reader = agv_stub_->streamMap(&context, request); + api::AgvMapStreamCommand_Feedback response; + + const bool read = reader->Read(&response); + const auto captured_before_finish = waitForRecords(1U); + agv_service_.releaseHeldStream(); + const grpc::Status status = reader->Finish(); + + ASSERT_TRUE(read); + ASSERT_TRUE(status.ok()); + ASSERT_EQ(captured_before_finish.size(), 1U); + EXPECT_EQ( + captured_before_finish.front().detail, + "immediate stream sentinel"); +} + +TEST_F(GrpcErrorLoggingInterceptorTest, + RecordsEveryStreamingFailureResponse) +{ + grpc::ClientContext context; + setRpcDeadline(context); + api::AgvMapStreamCommand_Request request; + request.set_resume_token("multiple"); + auto reader = agv_stub_->streamMap(&context, request); + api::AgvMapStreamCommand_Feedback response; + + ASSERT_TRUE(reader->Read(&response)); + ASSERT_TRUE(reader->Read(&response)); + ASSERT_TRUE(reader->Finish().ok()); + + const auto captured = waitForRecords(2U); + ASSERT_EQ(captured.size(), 2U); + EXPECT_EQ(captured[0].detail, "first stream sentinel"); + EXPECT_EQ(captured[1].detail, "second stream sentinel"); +} + +TEST_F(GrpcErrorLoggingInterceptorTest, + PreservesDifferentResponseAndStatusErrors) +{ + grpc::ClientContext context; + setRpcDeadline(context); + api::AgvMapStreamCommand_Request request; + request.set_resume_token("different"); + auto reader = agv_stub_->streamMap(&context, request); + api::AgvMapStreamCommand_Feedback response; + + ASSERT_TRUE(reader->Read(&response)); + const grpc::Status status = reader->Finish(); + ASSERT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); + + const auto captured = waitForRecords(2U); + ASSERT_EQ(captured.size(), 2U); + EXPECT_EQ(captured[0].kind, GrpcFailureKind::APPLICATION); + EXPECT_EQ(captured[0].detail, "application stream sentinel"); + EXPECT_EQ(captured[1].kind, GrpcFailureKind::GRPC_STATUS); + EXPECT_EQ(captured[1].status_code, grpc::StatusCode::INTERNAL); + EXPECT_EQ(captured[1].detail, "status stream sentinel"); +} + +TEST_F(GrpcErrorLoggingInterceptorTest, + DoesNotDeduplicateLongErrorsByTruncatedPrefix) +{ + grpc::ClientContext context; + setRpcDeadline(context); + api::AgvMapStreamCommand_Request request; + request.set_resume_token("long-different"); + auto reader = agv_stub_->streamMap(&context, request); + api::AgvMapStreamCommand_Feedback response; + + ASSERT_TRUE(reader->Read(&response)); + const grpc::Status status = reader->Finish(); + ASSERT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); + + const auto captured = waitForRecords(2U); + ASSERT_EQ(captured.size(), 2U); + EXPECT_EQ(captured[0].kind, GrpcFailureKind::APPLICATION); + EXPECT_EQ(captured[1].kind, GrpcFailureKind::GRPC_STATUS); +} + +TEST(GrpcErrorLoggingInterceptorWireTest, RequiresHeaderAsFirstField) +{ + api::CommandHeader_Feedback header; + header.set_success(false); + header.set_error_message("out-of-order sentinel"); + std::string payload; + payload.push_back(static_cast(0x12)); + payload.push_back(static_cast(0x01)); + payload.push_back('x'); + payload.push_back(static_cast(0x0a)); + payload.push_back(static_cast(header.ByteSizeLong())); + payload.append(header.SerializeAsString()); + + const auto result = callRawStartCamera(payload); + + ASSERT_TRUE(result.status.ok()); + ASSERT_EQ(result.response, payload); + ASSERT_EQ(result.records.size(), 1U); + EXPECT_EQ( + result.records.front().kind, + GrpcFailureKind::MALFORMED_RESPONSE); +} + +TEST(GrpcErrorLoggingInterceptorWireTest, DoesNotScanPayloadAfterHeader) +{ + api::CommandHeader_Feedback header; + header.set_success(false); + header.set_error_message("leading header sentinel"); + std::string payload; + payload.push_back(static_cast(0x0a)); + payload.push_back(static_cast(header.ByteSizeLong())); + payload.append(header.SerializeAsString()); + payload.push_back(static_cast(0x12)); + payload.push_back(static_cast(0xff)); + payload.append(2U * 1024U * 1024U, 'x'); + + const auto result = callRawStartCamera(payload); + + ASSERT_TRUE(result.status.ok()); + ASSERT_EQ(result.records.size(), 1U); + EXPECT_EQ(result.records.front().kind, GrpcFailureKind::APPLICATION); + EXPECT_EQ(result.records.front().detail, "leading header sentinel"); +} + +TEST(GrpcErrorLoggingInterceptorWireTest, TreatsExplicitEmptyHeaderAsFailure) +{ + const std::string payload{ + static_cast(0x0a), static_cast(0x00)}; + + const auto result = callRawStartCamera(payload); + + ASSERT_TRUE(result.status.ok()); + ASSERT_EQ(result.records.size(), 1U); + EXPECT_EQ(result.records.front().kind, GrpcFailureKind::APPLICATION); + EXPECT_EQ( + result.records.front().detail, + "operation failed without an error message"); +} + +TEST(GrpcErrorLoggingInterceptorWireTest, RejectsInvalidHeaderWireType) +{ + const std::string payload{ + static_cast(0x08), static_cast(0x01)}; + + const auto result = callRawStartCamera(payload); + + ASSERT_TRUE(result.status.ok()); + ASSERT_EQ(result.records.size(), 1U); + EXPECT_EQ( + result.records.front().kind, + GrpcFailureKind::MALFORMED_RESPONSE); +} + +TEST(GrpcErrorLoggingInterceptorWireTest, RejectsInvalidUtf8ErrorMessage) +{ + const std::string payload{ + static_cast(0x0a), static_cast(0x04), + static_cast(0x12), static_cast(0x02), + static_cast(0xc3), static_cast(0x28)}; + + const auto result = callRawStartCamera(payload); + + ASSERT_TRUE(result.status.ok()); + ASSERT_EQ(result.records.size(), 1U); + EXPECT_EQ( + result.records.front().kind, + GrpcFailureKind::MALFORMED_RESPONSE); +} + +TEST_F(GrpcErrorLoggingInterceptorTest, DoesNotRecordSuccessfulLargeResponse) +{ + grpc::ClientContext context; + setRpcDeadline(context); + api::GetRGBImageCommand_Request request; + api::GetRGBImageCommand_Feedback response; + + ASSERT_TRUE(camera_stub_->GetRGBImage(&context, request, &response).ok()); + ASSERT_TRUE(response.header().success()); + ASSERT_EQ(response.color_frame().data().size(), 2U * 1024U * 1024U); + EXPECT_TRUE(records().empty()); +} + +TEST_F(GrpcErrorLoggingInterceptorTest, IgnoresSuccessfulResponseWithoutHeader) +{ + grpc::ClientContext context; + setRpcDeadline(context); + api::TestReqeust request; + api::TestResponse response; + request.set_data("ok"); + + ASSERT_TRUE(test_stub_->Call(&context, request, &response).ok()); + EXPECT_EQ(response.data(), "ok"); + EXPECT_TRUE(records().empty()); +} + +} // namespace +} // namespace cmvr::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 34325bd9..c7e46a9f 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 @@ -16,6 +16,7 @@ #include "service/grpc/include/grpc_robot_arm_teleop_backend.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" #include "service/grpc/include/grpc_head_service.h" #include "service/grpc/include/grpc_hlc_service.h" #include "service/grpc/include/grpc_microphone_service.h" @@ -120,6 +121,13 @@ bool GrpcServerTask::start() grpc::ServerBuilder builder; builder.AddListeningPort(local_address, grpc::InsecureServerCredentials()); + std::vector> + interceptor_factories; + interceptor_factories.emplace_back( + service::makeGrpcErrorLoggingInterceptorFactory()); + builder.experimental().SetInterceptorCreators( + std::move(interceptor_factories)); builder.RegisterService(camera_service_.get()); builder.RegisterService(system_service_.get()); builder.RegisterService(speaker_service_.get()); From 55912abad015544d9cd596944ae3e34b78cb7a2a Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 14 Aug 2026 11:58:17 +0800 Subject: [PATCH 3/8] fix(stop-all): cancel blocked arm startup safely --- cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp | 314 ++++++++++++++++-- cmvr-es/devices/arm/aubo_arm/aubo_arm.h | 3 + .../devices/arm/aubo_arm/aubo_motion_state.h | 103 +++++- .../arm/aubo_arm/aubo_torque_on_result.h | 50 +++ .../tests/aubo_arm_motion_result_test.cpp | 75 +++++ .../aubo_arm/tests/aubo_motion_state_test.cpp | 59 +++- cmvr-es/devices/arm/robot_arm.h | 23 ++ .../grpc/include/grpc_system_service.h | 14 +- cmvr-es/service/grpc/src/grpc_arm_service.cpp | 37 ++- .../service/grpc/src/grpc_system_service.cpp | 192 ++++++++++- .../grpc/tests/grpc_arm_service_test.cpp | 277 ++++++++++++++- .../grpc/tests/grpc_system_service_test.cpp | 184 ++++++++++ .../include/stop_operation_dispatcher.h | 10 +- .../src/stop_operation_dispatcher.cpp | 32 +- .../tests/stop_operation_dispatcher_test.cpp | 82 +++++ 15 files changed, 1377 insertions(+), 78 deletions(-) create mode 100644 cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index 2842c9af..63bbd9e6 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -3,6 +3,7 @@ #include "devices/arm/aubo_arm/aubo_motion_result.h" #include "devices/arm/aubo_arm/aubo_motion_state.h" #include "devices/arm/aubo_arm/aubo_safety_state.h" +#include "devices/arm/aubo_arm/aubo_torque_on_result.h" #include #include @@ -547,9 +548,30 @@ bool enforceControllerTermination( { std::unique_lock termination_lock(monitor->termination_mutex); monitor->motion_state->cancelActiveForSafety(); - const auto stop_request = monitor->motion_state->beginStop(); + auto stop_request = monitor->motion_state->beginStop(); if (!stop_request.started()) { - return monitor->cancellation_confirmed.load(); + constexpr auto kExistingStopTimeout = std::chrono::seconds(6); + const auto existing_result = + monitor->motion_state->waitForStopCompletion( + stop_request, + std::chrono::duration_cast( + kExistingStopTimeout)); + if (existing_result == aubo_internal::StopWaitStatus::Timeout) { + monitor->cancellation_confirmed.store(false); + return false; + } + + // A regular stopMotion confirms direct-motion idle, while safety + // termination additionally verifies runtime, servo and controller + // queues. Re-enter the stop state and perform that stronger check. A + // failed existing stop is retried here as well, while MotionState + // remains fail-closed between the two attempts. + monitor->motion_state->cancelActiveForSafety(); + stop_request = monitor->motion_state->beginStop(); + if (!stop_request.started()) { + monitor->cancellation_confirmed.store(false); + return false; + } } const auto fail = [&monitor]() { @@ -1010,18 +1032,38 @@ RobotInterfacePtr getPrimaryRobotInterface(const std::shared_ptr& cancellation_requested) { const auto start_time = std::chrono::steady_clock::now(); while (std::chrono::steady_clock::now() - start_time < std::chrono::seconds(20)) { + if (cancellationRequested(cancellation_requested)) { + return RobotModeWaitResult::Cancelled; + } const auto current_mode = robot_interface->getRobotState()->getRobotModeType(); if (current_mode == target_mode) { - return true; + return RobotModeWaitResult::Reached; } std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - return false; + return cancellationRequested(cancellation_requested) + ? RobotModeWaitResult::Cancelled + : RobotModeWaitResult::Timeout; +} + +bool waitForRobotMode(const RobotInterfacePtr& robot_interface, + const RobotModeType& target_mode) +{ + return waitForRobotMode(robot_interface, target_mode, {}) == + RobotModeWaitResult::Reached; } template @@ -1487,6 +1529,12 @@ bool AuboArm::busy() const } Result AuboArm::torqueOn() +{ + return torqueOn({}); +} + +Result AuboArm::torqueOn( + const std::function& cancellation_requested) { std::shared_ptr rpc_client; std::shared_ptr monitor; @@ -1500,22 +1548,82 @@ Result AuboArm::torqueOn() monitor = sdk_->safety_monitor; } + bool cancellation_latched = false; + bool controller_mutated = false; + const std::function cancellation_check = + [&cancellation_requested, &cancellation_latched]() { + if (!cancellation_latched) { + cancellation_latched = cancellationRequested( + cancellation_requested); + } + return cancellation_latched; + }; + const auto cancelled_before_startup = []() { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] torqueOn cancelled before controller startup"); + }; + const auto cancellation_result = [&]() -> std::optional { + if (!cancellation_check()) { + return std::nullopt; + } + if (!controller_mutated) { + return cancelled_before_startup(); + } + + try { + std::unique_lock cleanup_rpc_lock( + monitor->command_rpc_mutex); + cancelForSafetyTransition(monitor); + if (!enforceControllerTermination(rpc_client, monitor)) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn cancelled: control ownership was revoked, but controller safety termination could not be confirmed"); + } + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] torqueOn cancelled: control ownership was revoked; controller safety termination confirmed"); + } catch (const std::exception& e) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn cancelled: controller safety termination raised an exception: " + + std::string(e.what())); + } catch (...) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn cancelled: controller safety termination raised an unknown exception"); + } + }; + try { if (!monitor) { return Result::failure( ArmErrorCode::RobotNotReady, "[AuboArm] torqueOn failed: hardware safety monitor is unavailable"); } + + if (cancellation_check()) { + return cancelled_before_startup(); + } std::unique_lock command_rpc_lock( monitor->command_rpc_mutex); + if (cancellation_check()) { + return cancelled_before_startup(); + } Result interface_result; auto robot_interface = getPrimaryRobotInterface( rpc_client, "torqueOn", interface_result); if (!interface_result.ok()) { return interface_result; } + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } refreshSafetySample(rpc_client, monitor, robot_interface); + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } auto safety_snapshot = monitor->safety_state->snapshot(); const std::uint64_t entry_safety_epoch = safety_snapshot.epoch; if (monitor->emergency_stop_source.load() != 0) { @@ -1554,21 +1662,37 @@ Result AuboArm::torqueOn() std::string(safetyConditionName(condition))); } - const int restart_ret = - robot_interface->getRobotManage()->restartInterfaceBoard(); - if (restart_ret != arcs::common_interface::AUBO_OK) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] torqueOn recovery failed: restartInterfaceBoard ret=" + - std::to_string(restart_ret)); + controller_mutated = true; + if (const auto mutation_result = + aubo_internal::runTorqueOnControllerMutation( + arcs::common_interface::AUBO_OK, + [&robot_interface] { + return robot_interface->getRobotManage() + ->restartInterfaceBoard(); + }, + [](const int return_code) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn recovery failed: " + "restartInterfaceBoard ret=" + + std::to_string(return_code)); + }, + cancellation_result)) { + return *mutation_result; } const auto safety_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(10); do { + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } std::this_thread::sleep_for( std::chrono::milliseconds(100)); + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } refreshSafetySample( rpc_client, monitor, robot_interface); safety_snapshot = monitor->safety_state->snapshot(); @@ -1580,6 +1704,10 @@ Result AuboArm::torqueOn() safety_deadline); } + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } + safety_snapshot = monitor->safety_state->snapshot(); if (safety_snapshot.epoch != entry_safety_epoch) { return Result::failure( @@ -1595,6 +1723,9 @@ Result AuboArm::torqueOn() } if (recovering) { + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } const auto token = monitor->safety_state->beginRecovery( entry_safety_epoch); if (!token.has_value()) { @@ -1607,30 +1738,71 @@ Result AuboArm::torqueOn() monitor->safety_state, recovery_token); } + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } + double mass = 0.0; std::vector cog(3, 0.0); std::vector aom(3, 0.0); std::vector inertia(6, 0.0); - robot_interface->getRobotConfig()->setPayload(mass, cog, aom, inertia); + controller_mutated = true; + if (const auto mutation_result = + aubo_internal::runTorqueOnControllerMutation( + arcs::common_interface::AUBO_OK, + [&robot_interface, mass, &cog, &aom, &inertia] { + return robot_interface->getRobotConfig()->setPayload( + mass, cog, aom, inertia); + }, + [](const int return_code) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn failed: setPayload ret=" + + std::to_string(return_code)); + }, + cancellation_result)) { + return *mutation_result; + } auto current_mode = robot_interface->getRobotState()->getRobotModeType(); if (current_mode != RobotModeType::Running && current_mode != RobotModeType::Idle) { - const int poweron_ret = - robot_interface->getRobotManage()->poweron(); - if (poweron_ret != arcs::common_interface::AUBO_OK) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] torqueOn failed: poweron ret=" + - std::to_string(poweron_ret)); + if (const auto cancelled = cancellation_result()) { + return *cancelled; } - if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Idle)) { + controller_mutated = true; + if (const auto mutation_result = + aubo_internal::runTorqueOnControllerMutation( + arcs::common_interface::AUBO_OK, + [&robot_interface] { + return robot_interface->getRobotManage()->poweron(); + }, + [](const int return_code) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn failed: poweron ret=" + + std::to_string(return_code)); + }, + cancellation_result)) { + return *mutation_result; + } + const auto idle_wait = waitForRobotMode( + robot_interface, + arcs::common_interface::RobotModeType::Idle, + cancellation_check); + if (idle_wait != RobotModeWaitResult::Reached) { + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Idle"); } current_mode = RobotModeType::Idle; } + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } refreshSafetySample(rpc_client, monitor, robot_interface); auto before_brake_release = monitor->safety_state->snapshot(); @@ -1651,6 +1823,10 @@ Result AuboArm::torqueOn() } if (recovering) { + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } + controller_mutated = true; cancelForSafetyTransition(monitor); const bool cleanup_ok = current_mode == RobotModeType::Running ? enforceControllerTermination(rpc_client, monitor) @@ -1660,23 +1836,50 @@ Result AuboArm::torqueOn() ArmErrorCode::CommandFailed, "[AuboArm] torqueOn recovery failed: old controller queue could not be acknowledged and cleared before brake release"); } + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } } if (current_mode != RobotModeType::Running) { - const int startup_ret = - robot_interface->getRobotManage()->startup(); - if (startup_ret != arcs::common_interface::AUBO_OK) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] torqueOn failed: startup ret=" + - std::to_string(startup_ret)); + if (const auto cancelled = cancellation_result()) { + return *cancelled; } - if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Running)) { + controller_mutated = true; + if (const auto mutation_result = + aubo_internal::runTorqueOnControllerMutation( + arcs::common_interface::AUBO_OK, + [&robot_interface] { + return robot_interface->getRobotManage()->startup(); + }, + [](const int return_code) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn failed: startup ret=" + + std::to_string(return_code)); + }, + cancellation_result)) { + return *mutation_result; + } + const auto running_wait = waitForRobotMode( + robot_interface, + arcs::common_interface::RobotModeType::Running, + cancellation_check); + if (running_wait != RobotModeWaitResult::Reached) { + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Running"); } } + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } refreshSafetySample(rpc_client, monitor, robot_interface); + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } const auto after_startup = monitor->safety_state->snapshot(); const bool post_recovery_token_current = !recovering || (after_startup.recovery_in_progress && @@ -1692,12 +1895,18 @@ Result AuboArm::torqueOn() "[AuboArm] torqueOn rejected: safety state changed during startup; the new event remains latched"); } if (recovering) { + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } cancelForSafetyTransition(monitor); if (!enforceControllerTermination(rpc_client, monitor)) { return Result::failure( ArmErrorCode::CommandFailed, "[AuboArm] torqueOn recovery failed: controller did not reach an empty, steady state after startup"); } + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } refreshSafetySample(rpc_client, monitor, robot_interface); const bool robot_running = monitor->robot_mode.load() == @@ -1711,11 +1920,31 @@ Result AuboArm::torqueOn() "[AuboArm] torqueOn recovery failed: safety state changed during recovery"); } } + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } emergency_stopped_.store(false); servo_mode_.store(false); + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } return Result::success(); } catch (const std::exception& e) { - return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOn failed: ") + e.what()); + auto primary_failure = Result::failure( + ArmErrorCode::CommandFailed, + std::string("[AuboArm] torqueOn failed: ") + e.what()); + return controller_mutated + ? aubo_internal::preservePrimaryTorqueOnFailure( + std::move(primary_failure), cancellation_result()) + : primary_failure; + } catch (...) { + auto primary_failure = Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn failed: unknown exception"); + return controller_mutated + ? aubo_internal::preservePrimaryTorqueOnFailure( + std::move(primary_failure), cancellation_result()) + : primary_failure; } } @@ -2293,9 +2522,28 @@ Result AuboArm::stopMotion_( const auto stop_request = motion_state->beginStop(forced_kind); if (!stop_request.started()) { + busy_.store(true); + constexpr auto kExistingStopTimeout = + std::chrono::seconds(6); + const auto existing_result = + motion_state->waitForStopCompletion( + stop_request, + std::chrono::duration_cast( + kExistingStopTimeout)); + busy_.store(motion_state->busy()); + if (existing_result == + aubo_internal::StopWaitStatus::Completed) { + return Result::success(); + } + if (existing_result == + aubo_internal::StopWaitStatus::Timeout) { + return Result::failure( + ArmErrorCode::Timeout, + "[AuboArm] stopMotion failed: timeout waiting for the existing stop operation to complete"); + } return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] stopMotion rejected: another stop operation is in progress"); + ArmErrorCode::CommandFailed, + "[AuboArm] stopMotion failed: the existing stop operation could not confirm controller idle"); } busy_.store(true); StopStateGuard stop_state_guard{ diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h index 4f22ddfe..8665aecc 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -36,6 +37,8 @@ public: bool supportsActionQueueMotion() const noexcept override { return true; } Result torqueOn() override; + Result torqueOn( + const std::function& cancellation_requested) override; Result torqueOff() override; Result calibrateZeroQ(const std::string& joint_name) override; Result emergencyStop() override; diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h b/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h index f3012c89..84c84367 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h @@ -5,6 +5,7 @@ #include #include #include +#include #include namespace cmvr::device::aubo_internal { @@ -54,11 +55,50 @@ enum class StopStartStatus { AlreadyStopping, }; +enum class StopWaitStatus { + Completed, + Failed, + Timeout, +}; + +class StopCompletion final { +public: + StopWaitStatus waitFor(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + if (!cv_.wait_for(lock, timeout, [this]() { return completed_; })) { + return StopWaitStatus::Timeout; + } + return succeeded_ + ? StopWaitStatus::Completed + : StopWaitStatus::Failed; + } + +private: + friend class MotionState; + + void finish(const bool succeeded) + { + { + std::lock_guard lock(mutex_); + succeeded_ = succeeded; + completed_ = true; + } + cv_.notify_all(); + } + + std::mutex mutex_; + std::condition_variable cv_; + bool completed_{false}; + bool succeeded_{false}; +}; + struct StopRequest { StopStartStatus status{StopStartStatus::AlreadyStopping}; MotionKind kind{MotionKind::None}; MotionToken active_token; bool tracked_motion{false}; + std::shared_ptr completion; bool started() const noexcept { @@ -152,10 +192,17 @@ public: { std::lock_guard lock(mutex_); if (stop_in_progress_) { - return {}; + return { + StopStartStatus::AlreadyStopping, + MotionKind::None, + {}, + false, + active_stop_completion_}; } + auto completion = std::make_shared(); stop_in_progress_ = true; + active_stop_completion_ = completion; const MotionToken active = owner_active_ ? active_token_ : MotionToken{}; @@ -186,7 +233,8 @@ public: StopStartStatus::Started, kind, active, - tracked_motion}; + tracked_motion, + completion}; } SafetyCancelResult cancelActiveForSafety() @@ -248,27 +296,51 @@ public: active_token_.generation == token.generation; } + StopWaitStatus waitForStopCompletion( + const StopRequest& request, + const std::chrono::milliseconds timeout) const + { + if (!request.completion) { + return StopWaitStatus::Failed; + } + return request.completion->waitFor(timeout); + } + bool completeStop() { - std::lock_guard lock(mutex_); - if (owner_active_) { - return false; + std::shared_ptr completion; + { + std::lock_guard lock(mutex_); + if (owner_active_) { + return false; + } + stop_in_progress_ = false; + blocked_ = false; + active_token_ = {}; + last_kind_ = MotionKind::None; + previous_kind_ = MotionKind::None; + completion = std::move(active_stop_completion_); + owner_finished_cv_.notify_all(); + } + if (completion) { + completion->finish(true); } - stop_in_progress_ = false; - blocked_ = false; - active_token_ = {}; - last_kind_ = MotionKind::None; - previous_kind_ = MotionKind::None; - owner_finished_cv_.notify_all(); return true; } void failStop() { - std::lock_guard lock(mutex_); - stop_in_progress_ = false; - blocked_ = true; - owner_finished_cv_.notify_all(); + std::shared_ptr completion; + { + std::lock_guard lock(mutex_); + stop_in_progress_ = false; + blocked_ = true; + completion = std::move(active_stop_completion_); + owner_finished_cv_.notify_all(); + } + if (completion) { + completion->finish(false); + } } bool busy() const @@ -286,6 +358,7 @@ private: MotionToken active_token_; MotionKind last_kind_{MotionKind::None}; MotionKind previous_kind_{MotionKind::None}; + std::shared_ptr active_stop_completion_; bool owner_active_{false}; bool stop_in_progress_{false}; bool blocked_{false}; diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h b/cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h new file mode 100644 index 00000000..4f2f1365 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h @@ -0,0 +1,50 @@ +#ifndef CMVR_ES_AUBO_TORQUE_ON_RESULT_H +#define CMVR_ES_AUBO_TORQUE_ON_RESULT_H + +#include +#include +#include + +#include "common/types/arm/arm_types.h" + +namespace cmvr::device::aubo_internal { + +inline Result preservePrimaryTorqueOnFailure( + Result primary_failure, + const std::optional& cancellation_outcome) +{ + if (!cancellation_outcome.has_value() || + cancellation_outcome->message.empty()) { + return primary_failure; + } + + if (!primary_failure.message.empty()) { + primary_failure.message += "; "; + } + primary_failure.message += + "cancellation handling: " + cancellation_outcome->message; + return primary_failure; +} + +template +std::optional runTorqueOnControllerMutation( + const int success_code, + Mutation&& mutation, + FailureResult&& failure_result, + CancellationOutcome&& cancellation_outcome) +{ + const int return_code = std::forward(mutation)(); + if (return_code != success_code) { + auto primary_failure = + std::forward(failure_result)(return_code); + return preservePrimaryTorqueOnFailure( + std::move(primary_failure), + std::forward(cancellation_outcome)()); + } + return std::forward(cancellation_outcome)(); +} + +} // namespace cmvr::device::aubo_internal + +#endif // CMVR_ES_AUBO_TORQUE_ON_RESULT_H diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp index fb1656e5..57bca235 100644 --- a/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp +++ b/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp @@ -1,6 +1,10 @@ #include "devices/arm/aubo_arm/aubo_motion_result.h" +#include "devices/arm/aubo_arm/aubo_torque_on_result.h" #include +#include +#include +#include namespace { @@ -19,7 +23,9 @@ int main() { using cmvr::device::aubo_internal::MotionCommandOutcome; using cmvr::device::aubo_internal::MotionWaitResult; + using cmvr::device::aubo_internal::preservePrimaryTorqueOnFailure; using cmvr::device::aubo_internal::resolveMotionCommand; + using cmvr::device::aubo_internal::runTorqueOnControllerMutation; constexpr int success_code = 0; constexpr int request_ignore_code = 13; @@ -79,5 +85,74 @@ int main() wait_cancelled) == MotionCommandOutcome::Cancelled); CHECK_TRUE(wait_calls == 1); + using cmvr::device::ArmErrorCode; + using cmvr::device::Result; + std::vector mutation_trace; + const auto failed_mutation = runTorqueOnControllerMutation( + success_code, + [&mutation_trace] { + mutation_trace.push_back("mutation"); + return 42; + }, + [&mutation_trace](const int return_code) { + mutation_trace.push_back("primary-failure"); + return Result::failure( + ArmErrorCode::CommandFailed, + "vendor failure ret=" + std::to_string(return_code)); + }, + [&mutation_trace]() -> std::optional { + mutation_trace.push_back("cancellation-cleanup"); + return Result::failure( + ArmErrorCode::CommandRejected, + "controller safety termination confirmed"); + }); + CHECK_TRUE(failed_mutation.has_value()); + CHECK_TRUE(failed_mutation->code == ArmErrorCode::CommandFailed); + CHECK_TRUE( + failed_mutation->message.find("vendor failure ret=42") != + std::string::npos); + CHECK_TRUE( + failed_mutation->message.find( + "controller safety termination confirmed") != + std::string::npos); + CHECK_TRUE(mutation_trace.size() == 3); + CHECK_TRUE(mutation_trace[0] == "mutation"); + CHECK_TRUE(mutation_trace[1] == "primary-failure"); + CHECK_TRUE(mutation_trace[2] == "cancellation-cleanup"); + + const auto cleanup_failed = preservePrimaryTorqueOnFailure( + Result::failure( + ArmErrorCode::CommandFailed, + "vendor exception"), + std::optional{Result::failure( + ArmErrorCode::CommandFailed, + "safety termination failed")}); + CHECK_TRUE(cleanup_failed.code == ArmErrorCode::CommandFailed); + CHECK_TRUE( + cleanup_failed.message.find("vendor exception") != + std::string::npos); + CHECK_TRUE( + cleanup_failed.message.find("safety termination failed") != + std::string::npos); + + int successful_cancellation_checks = 0; + const auto cancelled_after_success = runTorqueOnControllerMutation( + success_code, + [] { return 0; }, + [](const int) { + return Result::failure( + ArmErrorCode::CommandFailed, "must not be used"); + }, + [&successful_cancellation_checks]() -> std::optional { + ++successful_cancellation_checks; + return Result::failure( + ArmErrorCode::CommandRejected, + "cancelled after successful mutation"); + }); + CHECK_TRUE(cancelled_after_success.has_value()); + CHECK_TRUE( + cancelled_after_success->code == ArmErrorCode::CommandRejected); + CHECK_TRUE(successful_cancellation_checks == 1); + return 0; } diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp index bbeb9169..6f68937d 100644 --- a/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp +++ b/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp @@ -1,7 +1,9 @@ #include "devices/arm/aubo_arm/aubo_motion_state.h" +#include #include #include +#include namespace { @@ -36,8 +38,13 @@ int main() joint.token.generation); CHECK_TRUE(stop_joint.tracked_motion); CHECK_TRUE(state.cancelled(joint.token)); - CHECK_TRUE(state.beginStop().status == + const auto joined_stop_joint = state.beginStop(); + CHECK_TRUE(joined_stop_joint.status == StopStartStatus::AlreadyStopping); + CHECK_TRUE( + state.waitForStopCompletion( + joined_stop_joint, std::chrono::milliseconds(1)) == + StopWaitStatus::Timeout); CHECK_TRUE(state.begin(MotionKind::Linear).status == MotionStartStatus::Stopping); CHECK_TRUE(!state.waitForOwnerExit( @@ -48,6 +55,10 @@ int main() CHECK_TRUE(state.waitForOwnerExit( joint.token, std::chrono::milliseconds(1))); CHECK_TRUE(state.completeStop()); + CHECK_TRUE( + state.waitForStopCompletion( + joined_stop_joint, std::chrono::milliseconds(1)) == + StopWaitStatus::Completed); CHECK_TRUE(!state.busy()); const auto linear = state.begin(MotionKind::Linear); @@ -81,7 +92,14 @@ int main() const auto idle_stop = state.beginStop(); CHECK_TRUE(idle_stop.kind == MotionKind::None); CHECK_TRUE(!idle_stop.tracked_motion); + const auto joined_idle_stop = state.beginStop(); + CHECK_TRUE(joined_idle_stop.status == + StopStartStatus::AlreadyStopping); state.failStop(); + CHECK_TRUE( + state.waitForStopCompletion( + joined_idle_stop, std::chrono::milliseconds(1)) == + StopWaitStatus::Failed); const auto retry_idle_stop = state.beginStop(); CHECK_TRUE(retry_idle_stop.kind == MotionKind::None); CHECK_TRUE(!retry_idle_stop.tracked_motion); @@ -152,5 +170,44 @@ int main() CHECK_TRUE(retained_stop.tracked_motion); CHECK_TRUE(state.completeStop()); + // A waiter keeps the completion for the stop it joined even when another + // stop starts and finishes before the waiter is scheduled again. + const auto concurrent_first_stop = state.beginStop(); + CHECK_TRUE(concurrent_first_stop.started()); + const auto concurrent_first_join = state.beginStop(); + CHECK_TRUE(concurrent_first_join.status == + StopStartStatus::AlreadyStopping); + std::atomic waiter_entered{false}; + std::atomic first_wait_result{ + StopWaitStatus::Timeout}; + std::thread first_waiter([&]() { + waiter_entered.store(true, std::memory_order_release); + first_wait_result.store( + state.waitForStopCompletion( + concurrent_first_join, + std::chrono::milliseconds(500)), + std::memory_order_release); + }); + while (!waiter_entered.load(std::memory_order_acquire)) { + std::this_thread::yield(); + } + + const bool concurrent_first_completed = state.completeStop(); + const auto concurrent_second_stop = state.beginStop(); + const auto concurrent_second_join = state.beginStop(); + state.failStop(); + first_waiter.join(); + + CHECK_TRUE(concurrent_first_completed); + CHECK_TRUE(concurrent_second_stop.started()); + CHECK_TRUE(concurrent_second_join.status == + StopStartStatus::AlreadyStopping); + CHECK_TRUE(first_wait_result.load(std::memory_order_acquire) == + StopWaitStatus::Completed); + CHECK_TRUE( + state.waitForStopCompletion( + concurrent_second_join, std::chrono::milliseconds(1)) == + StopWaitStatus::Failed); + return 0; } diff --git a/cmvr-es/devices/arm/robot_arm.h b/cmvr-es/devices/arm/robot_arm.h index 4f63cf4d..fe8442e3 100644 --- a/cmvr-es/devices/arm/robot_arm.h +++ b/cmvr-es/devices/arm/robot_arm.h @@ -2,6 +2,7 @@ #define CMVR_ES_ROBOT_ARM_H #include +#include #include #include #include @@ -44,6 +45,28 @@ public: } virtual Result torqueOn() = 0; + // Long-running startup implementations may cooperatively observe loss of + // their control lease. Backends which have not adopted cancellation retain + // the legacy behavior, while still rejecting an already-cancelled request + // before entering the vendor API. + virtual Result torqueOn( + const std::function& cancellation_requested) + { + if (cancellation_requested) { + try { + if (cancellation_requested()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "torqueOn cancelled before execution"); + } + } catch (...) { + return Result::failure( + ArmErrorCode::CommandRejected, + "torqueOn cancellation check failed"); + } + } + return torqueOn(); + } virtual Result torqueOff() = 0; virtual Result calibrateZeroQ(const std::string& joint_name) = 0; diff --git a/cmvr-es/service/grpc/include/grpc_system_service.h b/cmvr-es/service/grpc/include/grpc_system_service.h index e7f714fe..6b126b5f 100644 --- a/cmvr-es/service/grpc/include/grpc_system_service.h +++ b/cmvr-es/service/grpc/include/grpc_system_service.h @@ -5,6 +5,7 @@ #ifndef GRPC_SYSTEM_SERVICE_H #define GRPC_SYSTEM_SERVICE_H +#include #include #include "cmvr/api/system_service.grpc.pb.h" @@ -19,7 +20,12 @@ namespace cmvr::service class gRPCSystemServiceImpl: public api::SystemService::Service { public: gRPCSystemServiceImpl(); + explicit gRPCSystemServiceImpl( + std::chrono::milliseconds stop_timeout); ~gRPCSystemServiceImpl() override; + // Exposed only to synchronize lifecycle concurrency tests. + static bool waitForStopDispatcherDestructionForTesting( + std::chrono::milliseconds timeout); // Called by GrpcServerTask before grpc::Server::Shutdown so accepted // ActionQueue handlers can reach a terminal result and do not hold the // synchronous server shutdown open indefinitely. @@ -32,10 +38,10 @@ namespace cmvr::service grpc::Status ExecuteActionQueue(grpc::ServerContext* context, const cmvr::api::ActionQueueCommand_Request* request, cmvr::api::ActionQueueCommand_Feedback* response) override; private: device::DeviceManager& dmgr_; - // Outlives ActionQueueExecutor and every StopAll RPC stack. A stop - // backend which ignores the shared deadline can therefore finish in - // its owned worker without accessing destroyed RPC-local state. - std::unique_ptr stop_dispatcher_; + const std::chrono::milliseconds stop_timeout_; + // 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_; }; } diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index 89a84745..1fd6ef0e 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -173,6 +173,19 @@ grpc::Status setControlLeaseConflict( response->mutable_header(), device_id, detail); } +grpc::Status setControlCancelled( + api::CommandHeader_Feedback* response, + const std::string& device_id, + const std::string& reason) +{ + std::string message = "RobotArm control was cancelled: " + device_id; + if (!reason.empty()) { + message += ", " + reason; + } + fillFeedback(response, false, message); + return grpc::Status(grpc::StatusCode::CANCELLED, message); +} + class ScopedUnaryControlLease final { public: ScopedUnaryControlLease( @@ -383,7 +396,7 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*, } } -grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*, +grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext* context, const api::CommandHeader_Request* request, api::CommandHeader_Feedback* response) { @@ -404,7 +417,27 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*, return setControlDispatchFailure( response, device_id, control_lease, "torqueOn"); } - const auto result = arm->torqueOn(); + const auto cancellation_requested = + control_lease.cancellationRequested(context); + const auto result = arm->torqueOn(cancellation_requested); + const bool control_current = control_lease.current(); + const bool cancellation_result = + result.ok() || + result.code == device::ArmErrorCode::CommandRejected; + if (cancellation_result && + !control_lease.admissionCurrent()) { + return setStopAllRejected(response, device_id); + } + const bool rpc_cancelled = context && context->IsCancelled(); + if (cancellation_result && + (rpc_cancelled || !control_current)) { + return setControlCancelled( + response, + device_id, + rpc_cancelled + ? "the RPC was cancelled" + : "control ownership was revoked"); + } fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { logRpcSuccess("torqueOn", device_id); diff --git a/cmvr-es/service/grpc/src/grpc_system_service.cpp b/cmvr-es/service/grpc/src/grpc_system_service.cpp index 2d7fb369..d39563fa 100644 --- a/cmvr-es/service/grpc/src/grpc_system_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_system_service.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -407,6 +408,92 @@ using StopDispatcher = cmvr::service::StopOperationDispatcher; using StopHandle = StopDispatcher::Handle; using StopOutcome = StopDispatcher::OperationResult; +struct ProcessStopDispatcherRegistry final { + std::mutex mutex; + std::condition_variable available; + std::weak_ptr dispatcher; + bool destroying{false}; +}; + +ProcessStopDispatcherRegistry& processStopDispatcherRegistry() +{ + static ProcessStopDispatcherRegistry registry; + return registry; +} + +void acquireProcessStopDispatcher( + std::shared_ptr& owner) +{ + auto& registry = processStopDispatcherRegistry(); + std::unique_lock lock(registry.mutex); + registry.available.wait( + lock, [®istry] { return !registry.destroying; }); + if (auto existing = registry.dispatcher.lock()) { + owner = std::move(existing); + return; + } + + owner = std::make_shared(); + registry.dispatcher = owner; +} + +void releaseProcessStopDispatcher( + std::shared_ptr& owner) noexcept +{ + if (!owner) { + return; + } + + auto& registry = processStopDispatcherRegistry(); + std::unique_lock lock(registry.mutex); + if (owner.use_count() > 1) { + owner.reset(); + return; + } + + // Do not admit a replacement while the last dispatcher is joining a + // worker which may still be inside a device driver. + registry.destroying = true; + registry.dispatcher.reset(); + registry.available.notify_all(); + auto last_owner = std::move(owner); + lock.unlock(); + last_owner.reset(); + lock.lock(); + registry.destroying = false; + lock.unlock(); + registry.available.notify_all(); +} + +class StopDeadlineDetail final { +public: + void publishInitial(std::string detail) + { + std::lock_guard lock(mutex_); + detail_ = std::move(detail); + } + + void publishUntil( + const StopDispatcher::Deadline deadline, + std::string detail) + { + std::lock_guard lock(mutex_); + if (StopDispatcher::Clock::now() < deadline) { + detail_ = std::move(detail); + } + } + + std::string snapshot() const + { + std::lock_guard lock(mutex_); + return detail_; + } + +private: + mutable std::mutex mutex_; + std::string detail_; +}; + struct StopHandleEntry final { std::string device_id; StopHandle handle; @@ -565,8 +652,13 @@ StopOutcome stopControlWithFence( const StopDispatcher::Deadline deadline, InitialStop&& initial_stop, FinalStop&& final_stop, - const std::string& description) + const std::string& description, + const std::shared_ptr& deadline_detail) { + deadline_detail->publishUntil( + deadline, + "initial " + description + + " stop did not complete before the StopAll deadline"); const auto initial = invokeStopOperation( std::forward(initial_stop), "initial " + description + " stop"); @@ -582,18 +674,37 @@ StopOutcome stopControlWithFence( " safety barrier was unavailable after the initial stop"}; } + const std::string handler_timeout_detail = + "timed out waiting for the preempted " + description + + " control handler to exit"; + deadline_detail->publishUntil(deadline, handler_timeout_detail); const bool handler_drained = cmvr::control::ControlAuthorityManager::instance() .waitForPreemptedRelease( barrier, remainingStopBudget(deadline)); + if (handler_drained) { + deadline_detail->publishUntil( + deadline, + "final " + description + + " stop did not complete before the StopAll deadline"); + } + // This final typed stop remains mandatory even when the handler wait used + // the entire RPC budget. The dispatcher owns this worker past the RPC + // deadline, closing the race where an old handler resumes after the first + // stop while the safety barrier remains fail-closed. const auto final = invokeStopOperation( std::forward(final_stop), "final " + description + " stop"); if (!handler_drained) { - return { - false, - "timed out waiting for the preempted " + description + - " control handler to exit"}; + std::string detail = handler_timeout_detail; + if (!final.success) { + detail += "; " + + (final.detail.empty() + ? "final " + description + + " stop was not confirmed" + : final.detail); + } + return {false, std::move(detail)}; } return final; } @@ -927,13 +1038,42 @@ cmvr::api::SystemDeviceHealth toApiDeviceHealth( } // namespace gRPCSystemServiceImpl::gRPCSystemServiceImpl() - : dmgr_(DeviceManager::getInstance()), - stop_dispatcher_(std::make_unique()), - action_queue_(std::make_unique(dmgr_)) + : gRPCSystemServiceImpl(std::chrono::seconds(15)) { } -gRPCSystemServiceImpl::~gRPCSystemServiceImpl() = default; +gRPCSystemServiceImpl::gRPCSystemServiceImpl( + const std::chrono::milliseconds stop_timeout) + : dmgr_(DeviceManager::getInstance()), + stop_timeout_( + stop_timeout > std::chrono::milliseconds::zero() + ? stop_timeout + : std::chrono::seconds(15)), + action_queue_(std::make_unique(dmgr_)) +{ + // Acquire only after ActionQueue construction succeeds. This ensures an + // exception cannot release the last dispatcher outside the registry. + acquireProcessStopDispatcher(stop_dispatcher_); +} + +gRPCSystemServiceImpl::~gRPCSystemServiceImpl() +{ + // The dispatcher intentionally outlives ActionQueue, then joins any + // deadline-overrunning stop workers before the last service disappears. + action_queue_.reset(); + releaseProcessStopDispatcher(stop_dispatcher_); +} + +bool gRPCSystemServiceImpl::waitForStopDispatcherDestructionForTesting( + const std::chrono::milliseconds timeout) +{ + auto& registry = processStopDispatcherRegistry(); + std::unique_lock lock(registry.mutex); + return registry.available.wait_for( + lock, + timeout, + [®istry] { return registry.destroying; }); +} void gRPCSystemServiceImpl::prepareForShutdown() { @@ -1079,9 +1219,8 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) { (void)request; - constexpr auto stop_timeout = std::chrono::seconds(15); const auto stop_deadline = - std::chrono::steady_clock::now() + stop_timeout; + std::chrono::steady_clock::now() + stop_timeout_; std::unique_lock stop_all_lock( processStopAllMutex(), std::defer_lock); @@ -1266,41 +1405,60 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, control_stops.reserve(control_targets.size()); for (const auto& target : control_targets) { StopHandle handle; + auto deadline_detail = std::make_shared(); if (target.arm) { const auto arm = target.arm; const auto barrier = target.barrier; + deadline_detail->publishInitial( + "initial RobotArm stop did not complete before the " + "StopAll deadline"); handle = stop_dispatcher_->submit( "control:" + stopResourceKey(target.id), - [arm, barrier, stop_deadline] { + [arm, barrier, stop_deadline, deadline_detail] { return stopControlWithFence( barrier, stop_deadline, [arm] { return stopArm(arm, false); }, [arm] { return stopArm(arm, true); }, - "RobotArm"); + "RobotArm", deadline_detail); + }, + [deadline_detail] { + return deadline_detail->snapshot(); }); } else if (target.agv) { const auto agv = target.agv; const auto barrier = target.barrier; + deadline_detail->publishInitial( + "initial AGV stop did not complete before the StopAll " + "deadline"); handle = stop_dispatcher_->submit( "control:" + stopResourceKey(target.id), - [agv, barrier, stop_deadline] { + [agv, barrier, stop_deadline, deadline_detail] { return stopControlWithFence( barrier, stop_deadline, [agv] { return stopAgv(agv, false); }, [agv] { return stopAgv(agv, true); }, - "AGV"); + "AGV", deadline_detail); + }, + [deadline_detail] { + return deadline_detail->snapshot(); }); } else { const auto hand = target.hand; const auto barrier = target.barrier; + deadline_detail->publishInitial( + "initial DexHand stop did not complete before the " + "StopAll deadline"); handle = stop_dispatcher_->submit( "control:" + stopResourceKey(target.id), - [hand, barrier, stop_deadline] { + [hand, barrier, stop_deadline, deadline_detail] { return stopControlWithFence( barrier, stop_deadline, [hand] { return stopDexHand(hand, false); }, [hand] { return stopDexHand(hand, true); }, - "DexHand"); + "DexHand", deadline_detail); + }, + [deadline_detail] { + return deadline_detail->snapshot(); }); } control_stops.push_back( 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 f5d24016..19108a25 100644 --- a/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp @@ -1,23 +1,28 @@ #include "service/grpc/include/grpc_arm_service.h" #include +#include #include +#include #include #include #include #include #include #include +#include #include #include #include #include #include +#include #include "cmvr/config/device_manager_config/device_manager_config.pb.h" #include "manager/control_authority/include/control_authority_manager.h" #include "manager/device_manager/include/device_manager.h" +#include "service/grpc/include/grpc_error_logging_interceptor.h" #include "service/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { @@ -63,7 +68,43 @@ public: return device::ControlMode::None; } - device::Result torqueOn() override { return device::Result::success(); } + device::Result torqueOn() override { return torqueOn({}); } + device::Result torqueOn( + const std::function& cancellation_requested) override + { + std::unique_lock lock(motion_mutex_); + ++torque_on_calls_; + last_torque_on_had_cancellation_ = + static_cast(cancellation_requested); + if (!block_next_torque_on_) { + return device::Result::success(); + } + + block_next_torque_on_ = false; + blocking_torque_on_started_ = true; + torque_on_started_cv_.notify_all(); + while (!release_blocking_torque_on_) { + lock.unlock(); + const bool cancelled = + cancellation_requested && cancellation_requested(); + lock.lock(); + if (cancelled) { + last_torque_on_cancellation_requested_ = true; + if (fail_next_torque_on_cancellation_) { + fail_next_torque_on_cancellation_ = false; + return device::Result::failure( + device::ArmErrorCode::CommandFailed, + "simulated torqueOn safety termination failure"); + } + return device::Result::failure( + device::ArmErrorCode::CommandRejected, + "simulated torqueOn cancellation"); + } + torque_on_release_cv_.wait_for( + lock, std::chrono::milliseconds(5)); + } + return device::Result::success(); + } device::Result torqueOff() override { std::lock_guard lock(motion_mutex_); @@ -159,6 +200,40 @@ public: blocking_motion_name_.clear(); } + void blockNextTorqueOn() + { + std::lock_guard lock(motion_mutex_); + block_next_torque_on_ = true; + blocking_torque_on_started_ = false; + release_blocking_torque_on_ = false; + last_torque_on_cancellation_requested_ = false; + } + + void failNextTorqueOnCancellation() + { + std::lock_guard lock(motion_mutex_); + fail_next_torque_on_cancellation_ = true; + } + + bool waitForBlockingTorqueOn( + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(motion_mutex_); + return torque_on_started_cv_.wait_for( + lock, + timeout, + [this]() { return blocking_torque_on_started_; }); + } + + void releaseBlockingTorqueOn() + { + { + std::lock_guard lock(motion_mutex_); + release_blocking_torque_on_ = true; + } + torque_on_release_cv_.notify_all(); + } + bool waitForBlockingMotion( const std::string& operation, const std::chrono::milliseconds timeout) @@ -244,6 +319,24 @@ public: return torque_off_calls_; } + int torqueOnCalls() const + { + std::lock_guard lock(motion_mutex_); + return torque_on_calls_; + } + + bool lastTorqueOnHadCancellation() const + { + std::lock_guard lock(motion_mutex_); + return last_torque_on_had_cancellation_; + } + + bool lastTorqueOnCancellationRequested() const + { + std::lock_guard lock(motion_mutex_); + return last_torque_on_cancellation_requested_; + } + bool lastMotionHadCancellation() const { std::lock_guard lock(motion_mutex_); @@ -388,6 +481,8 @@ private: std::condition_variable motion_release_cv_; std::condition_variable stop_started_cv_; std::condition_variable stop_release_cv_; + std::condition_variable torque_on_started_cv_; + std::condition_variable torque_on_release_cv_; bool block_next_motion_{false}; bool blocking_motion_started_{false}; bool release_blocking_motion_{false}; @@ -396,11 +491,18 @@ private: bool release_blocking_stop_{false}; bool fail_next_stop_{false}; bool throw_next_stop_{false}; + bool block_next_torque_on_{false}; + bool blocking_torque_on_started_{false}; + bool release_blocking_torque_on_{false}; + bool fail_next_torque_on_cancellation_{false}; std::string blocking_motion_name_; int move_j_calls_{0}; int move_l_calls_{0}; int stop_motion_calls_{0}; int torque_off_calls_{0}; + int torque_on_calls_{0}; + bool last_torque_on_had_cancellation_{false}; + bool last_torque_on_cancellation_requested_{false}; bool last_motion_had_cancellation_{false}; bool last_motion_cancellation_requested_{false}; }; @@ -520,6 +622,19 @@ protected: return service_->torqueOff(&context, &request, &response); } + MoveOutcome torqueOn(const std::string& device_id) + { + api::CommandHeader_Request request; + request.set_device_id(device_id); + api::CommandHeader_Feedback response; + grpc::ServerContext context; + auto status = service_->torqueOn(&context, &request, &response); + return { + std::move(status), + response.success(), + response.error_message()}; + } + std::shared_ptr left_arm_; std::shared_ptr aubo_arm_; std::shared_ptr non_arm_; @@ -718,6 +833,166 @@ TEST_F(GrpcArmServiceTest, MoveBindsLeaseRevocationCancellation) EXPECT_FALSE(aubo_arm_->lastMotionCancellationRequested()); } +TEST_F(GrpcArmServiceTest, + StopAllCancelsInFlightTorqueOnWithoutReportingSuccess) +{ + aubo_arm_->blockNextTorqueOn(); + auto blocked_torque_on = std::async( + std::launch::async, + [this]() { return torqueOn("aubo_arm"); }); + + const bool torque_on_started = aubo_arm_->waitForBlockingTorqueOn( + std::chrono::seconds(2)); + auto& admission = globalStopAllAdmissionGate(); + StopAllAdmissionGate::StopAllTicket stop_all_ticket; + if (torque_on_started) { + stop_all_ticket = admission.beginStopAll(); + } + + const bool cancelled_promptly = + blocked_torque_on.wait_for(std::chrono::seconds(2)) == + std::future_status::ready; + if (!cancelled_promptly) { + aubo_arm_->releaseBlockingTorqueOn(); + } + const auto outcome = blocked_torque_on.get(); + + ASSERT_TRUE(torque_on_started); + ASSERT_TRUE(stop_all_ticket.valid()); + EXPECT_TRUE(cancelled_promptly); + EXPECT_EQ(outcome.status.error_code(), grpc::StatusCode::UNAVAILABLE); + EXPECT_FALSE(outcome.response_success); + EXPECT_EQ(outcome.response_error, outcome.status.error_message()); + EXPECT_NE(outcome.response_error.find("StopAll"), std::string::npos); + EXPECT_EQ(aubo_arm_->torqueOnCalls(), 1); + EXPECT_TRUE(aubo_arm_->lastTorqueOnHadCancellation()); + EXPECT_TRUE(aubo_arm_->lastTorqueOnCancellationRequested()); + + EXPECT_TRUE(admission.finishStopAll(stop_all_ticket, true)); +} + +TEST_F(GrpcArmServiceTest, + StopAllPreservesTorqueOnSafetyTerminationFailure) +{ + aubo_arm_->blockNextTorqueOn(); + aubo_arm_->failNextTorqueOnCancellation(); + auto blocked_torque_on = std::async( + std::launch::async, + [this]() { return torqueOn("aubo_arm"); }); + + const bool torque_on_started = aubo_arm_->waitForBlockingTorqueOn( + std::chrono::seconds(2)); + auto& admission = globalStopAllAdmissionGate(); + StopAllAdmissionGate::StopAllTicket stop_all_ticket; + if (torque_on_started) { + stop_all_ticket = admission.beginStopAll(); + } + + const bool completed_promptly = + blocked_torque_on.wait_for(std::chrono::seconds(2)) == + std::future_status::ready; + if (!completed_promptly) { + aubo_arm_->releaseBlockingTorqueOn(); + } + const auto outcome = blocked_torque_on.get(); + + ASSERT_TRUE(torque_on_started); + ASSERT_TRUE(stop_all_ticket.valid()); + EXPECT_TRUE(completed_promptly); + EXPECT_EQ(outcome.status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_FALSE(outcome.response_success); + EXPECT_EQ( + outcome.response_error, + "simulated torqueOn safety termination failure"); + EXPECT_EQ(outcome.status.error_message(), outcome.response_error); + EXPECT_TRUE(aubo_arm_->lastTorqueOnCancellationRequested()); + + EXPECT_FALSE(admission.finishStopAll(stop_all_ticket, false)); +} + +TEST_F(GrpcArmServiceTest, + ClientCancellationPreservesTorqueOnSafetyTerminationFailure) +{ + std::mutex records_mutex; + std::condition_variable records_changed; + std::vector records; + + grpc::ServerBuilder builder; + const std::string socket_path = + "/tmp/cmvr_grpc_arm_service_test_" + + std::to_string(static_cast(::getpid())) + ".sock"; + std::remove(socket_path.c_str()); + const std::string address = "unix:" + socket_path; + builder.AddListeningPort( + address, + grpc::InsecureServerCredentials()); + builder.RegisterService(service_.get()); + std::vector> factories; + factories.emplace_back(makeGrpcErrorLoggingInterceptorFactory( + [&records, &records_mutex, &records_changed]( + const GrpcFailureRecord& record) { + { + std::lock_guard lock(records_mutex); + records.push_back(record); + } + records_changed.notify_all(); + })); + builder.experimental().SetInterceptorCreators(std::move(factories)); + auto server = builder.BuildAndStart(); + ASSERT_NE(server, nullptr); + + const auto channel = grpc::CreateChannel( + address, + grpc::InsecureChannelCredentials()); + auto stub = api::ArmService::NewStub(channel); + grpc::ClientContext client_context; + client_context.set_deadline( + std::chrono::system_clock::now() + std::chrono::seconds(3)); + api::CommandHeader_Request request; + request.set_device_id("aubo_arm"); + api::CommandHeader_Feedback response; + grpc::Status client_status; + + aubo_arm_->blockNextTorqueOn(); + aubo_arm_->failNextTorqueOnCancellation(); + std::thread client_call([&]() { + client_status = stub->torqueOn( + &client_context, request, &response); + }); + const bool torque_on_started = aubo_arm_->waitForBlockingTorqueOn( + std::chrono::seconds(2)); + client_context.TryCancel(); + client_call.join(); + + bool failure_recorded = false; + { + std::unique_lock lock(records_mutex); + failure_recorded = records_changed.wait_for( + lock, + std::chrono::seconds(2), + [&records]() { return !records.empty(); }); + } + if (!failure_recorded) { + aubo_arm_->releaseBlockingTorqueOn(); + } + server->Shutdown(); + server->Wait(); + std::remove(socket_path.c_str()); + + ASSERT_TRUE(torque_on_started); + EXPECT_EQ(client_status.error_code(), grpc::StatusCode::CANCELLED); + ASSERT_TRUE(failure_recorded); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records.front().kind, GrpcFailureKind::GRPC_STATUS); + EXPECT_EQ(records.front().status_code, grpc::StatusCode::INTERNAL); + EXPECT_EQ(records.front().severity, GrpcFailureSeverity::ERROR); + EXPECT_EQ( + records.front().detail, + "simulated torqueOn safety termination failure"); + EXPECT_TRUE(aubo_arm_->lastTorqueOnCancellationRequested()); +} + TEST_F(GrpcArmServiceTest, StopAllGateRejectsMutatingCommandsButAllowsReadsAndStops) { 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 2c43afca..ab7adbff 100644 --- a/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp @@ -3024,6 +3024,190 @@ TEST_F(GrpcSystemServiceTest, EXPECT_EQ(active_arm->motionCalls(), 1); } +TEST_F(GrpcSystemServiceTest, + StopAllSharesTimedOutArmStopAndDetailAcrossServiceInstances) +{ + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + auto trace = std::make_shared(); + auto arm = std::make_shared( + "preempted-handler-arm", trace); + manager.registerDevice(arm); + + auto& authority = control::ControlAuthorityManager::instance(); + const auto old_handler = authority.tryAcquire( + arm->id(), "blocked-control-handler", std::chrono::seconds(30)); + ASSERT_TRUE(old_handler.acquired) << old_handler.detail; + service_ = std::make_unique( + std::chrono::milliseconds(300)); + + api::StopAllCommand_Request request; + auto stop_all = std::async(std::launch::async, [this, &request] { + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll( + &context, &request, &response); + return std::make_pair(status, response); + }); + + const bool initial_stop_completed = arm->waitForStopMotionCalls( + 1, std::chrono::milliseconds(500)); + if (initial_stop_completed) { + arm->blockNextStopMotion(); + } + const auto completion = stop_all.wait_for(std::chrono::seconds(1)); + 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)); + } + } + } + + // Always release both test blocks before an assertion can abort the test; + // the dispatcher owns the final stop worker past the RPC deadline. + 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(); + } + } + 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); + EXPECT_EQ( + response.header().error_message().find( + "stop operation did not complete before the deadline"), + std::string::npos); + 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_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); +} + TEST_F(GrpcSystemServiceTest, StopAllDoesNotStopDeviceLifecyclesAndRevokesOnlyArmLease) { diff --git a/cmvr-es/service/stop_all/include/stop_operation_dispatcher.h b/cmvr-es/service/stop_all/include/stop_operation_dispatcher.h index 1dc0b8c4..60c25804 100644 --- a/cmvr-es/service/stop_all/include/stop_operation_dispatcher.h +++ b/cmvr-es/service/stop_all/include/stop_operation_dispatcher.h @@ -36,6 +36,7 @@ public: }; using Operation = std::function; + using TimeoutDetailProvider = std::function; class Handle final { public: @@ -62,8 +63,15 @@ public: // A running job is shared by every submission for the same resource key. // Once it has completed, the next submission joins/reaps the old worker - // and starts a new job. Empty keys or operations return an invalid handle. + // and starts a new job. A timeout provider belongs to the job that is + // actually created; submissions joining a running job retain that job's + // provider. Multiple handles may call the provider concurrently, so it + // must be thread-safe. Empty keys or operations return an invalid handle. Handle submit(std::string resource_key, Operation operation); + Handle submit( + std::string resource_key, + Operation operation, + TimeoutDetailProvider timeout_detail_provider); // Exposed only to verify dispatcher lifecycle behavior in tests. std::size_t jobCountForTesting() const; diff --git a/cmvr-es/service/stop_all/src/stop_operation_dispatcher.cpp b/cmvr-es/service/stop_all/src/stop_operation_dispatcher.cpp index 351d5c9f..f53b68c2 100644 --- a/cmvr-es/service/stop_all/src/stop_operation_dispatcher.cpp +++ b/cmvr-es/service/stop_all/src/stop_operation_dispatcher.cpp @@ -12,10 +12,16 @@ namespace cmvr::service { struct StopOperationDispatcher::JobState final { + explicit JobState(TimeoutDetailProvider provider) + : timeout_detail_provider(std::move(provider)) + { + } + std::mutex mutex; std::condition_variable condition; bool completed{false}; OperationResult outcome; + const TimeoutDetailProvider timeout_detail_provider; }; struct StopOperationDispatcher::Impl final { @@ -50,9 +56,18 @@ StopOperationDispatcher::Handle::waitUntil(const Deadline deadline) const if (!state_->condition.wait_until(lock, deadline, [this] { return state_->completed; })) { - return { - false, - false, + lock.unlock(); + if (state_->timeout_detail_provider) { + try { + auto detail = state_->timeout_detail_provider(); + if (!detail.empty()) { + return {false, false, std::move(detail)}; + } + } catch (...) { + // Diagnostics must not change timeout behavior. + } + } + return {false, false, "stop operation did not complete before the deadline"}; } @@ -85,6 +100,14 @@ StopOperationDispatcher::~StopOperationDispatcher() StopOperationDispatcher::Handle StopOperationDispatcher::submit( std::string resource_key, Operation operation) +{ + return submit(std::move(resource_key), std::move(operation), {}); +} + +StopOperationDispatcher::Handle StopOperationDispatcher::submit( + std::string resource_key, + Operation operation, + TimeoutDetailProvider timeout_detail_provider) { if (resource_key.empty() || !operation) { return {}; @@ -141,7 +164,8 @@ StopOperationDispatcher::Handle StopOperationDispatcher::submit( continue; } - auto state = std::make_shared(); + auto state = std::make_shared( + std::move(timeout_detail_provider)); const auto inserted = impl_->jobs.emplace( std::piecewise_construct, std::forward_as_tuple(std::move(resource_key)), diff --git a/cmvr-es/service/stop_all/tests/stop_operation_dispatcher_test.cpp b/cmvr-es/service/stop_all/tests/stop_operation_dispatcher_test.cpp index 4f469dd7..aabb337b 100644 --- a/cmvr-es/service/stop_all/tests/stop_operation_dispatcher_test.cpp +++ b/cmvr-es/service/stop_all/tests/stop_operation_dispatcher_test.cpp @@ -78,6 +78,88 @@ TEST(StopOperationDispatcherTest, TimeoutDoesNotCancelTheJob) EXPECT_EQ(completed.detail, "stopped"); } +TEST(StopOperationDispatcherTest, + RunningJobKeepsItsOriginalTimeoutDetailProvider) +{ + StopOperationDispatcher dispatcher; + std::promise release; + auto released = release.get_future().share(); + std::atomic first_operation_calls{0}; + std::atomic duplicate_operation_calls{0}; + std::atomic first_provider_calls{0}; + std::atomic duplicate_provider_calls{0}; + + const auto first = dispatcher.submit( + "arm:one", + [&] { + ++first_operation_calls; + released.wait(); + return StopOperationDispatcher::OperationResult{true, {}}; + }, + [&] { + ++first_provider_calls; + return std::string("first job is waiting for its handler"); + }); + const auto duplicate = dispatcher.submit( + "arm:one", + [&] { + ++duplicate_operation_calls; + return StopOperationDispatcher::OperationResult{true, {}}; + }, + [&] { + ++duplicate_provider_calls; + return std::string("duplicate submission detail"); + }); + + const auto first_timeout = first.waitUntil( + StopOperationDispatcher::Clock::now() + 20ms); + const auto duplicate_timeout = duplicate.waitUntil( + StopOperationDispatcher::Clock::now() + 20ms); + release.set_value(); + const auto completed = first.waitUntil( + StopOperationDispatcher::Clock::now() + 1s); + + EXPECT_FALSE(first_timeout.completed); + EXPECT_EQ( + first_timeout.detail, "first job is waiting for its handler"); + EXPECT_FALSE(duplicate_timeout.completed); + EXPECT_EQ( + duplicate_timeout.detail, "first job is waiting for its handler"); + EXPECT_TRUE(completed.completed); + EXPECT_TRUE(completed.result); + EXPECT_EQ(first_operation_calls.load(), 1); + EXPECT_EQ(duplicate_operation_calls.load(), 0); + EXPECT_EQ(first_provider_calls.load(), 2); + EXPECT_EQ(duplicate_provider_calls.load(), 0); +} + +TEST(StopOperationDispatcherTest, + ThrowingTimeoutDetailProviderFallsBackToGenericDetail) +{ + StopOperationDispatcher dispatcher; + std::promise release; + auto released = release.get_future().share(); + const auto handle = dispatcher.submit( + "arm:one", + [released] { + released.wait(); + return StopOperationDispatcher::OperationResult{true, {}}; + }, + []() -> std::string { + throw std::runtime_error("diagnostic provider failed"); + }); + + const auto timed_out = handle.waitUntil( + StopOperationDispatcher::Clock::now() + 20ms); + release.set_value(); + + EXPECT_FALSE(timed_out.completed); + EXPECT_FALSE(timed_out.result); + EXPECT_EQ( + timed_out.detail, + "stop operation did not complete before the deadline"); +} + TEST(StopOperationDispatcherTest, RunningSubmissionsForAKeyShareOneJob) { StopOperationDispatcher dispatcher; From 4f36cf495705aef7707ba5fd2776263aff91b295 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 14 Aug 2026 12:40:53 +0800 Subject: [PATCH 4/8] feat(quic): support multi-platform heartbeats --- cmvr-es/config/README.md | 38 ++- .../quic_edge_task/quic_edge_task.pb.txt | 15 + .../quic_edge_task/include/quic_edge_task.h | 17 +- .../quic_edge_task/src/quic_edge_task.cpp | 265 ++++++++++++---- .../tests/quic_edge_task_test.cpp | 285 ++++++++++++++++++ .../quic_edge_config/quic_edge_config.proto | 22 ++ protos/cmvr/quic_edge/v1/README.md | 12 +- 7 files changed, 599 insertions(+), 55 deletions(-) diff --git a/cmvr-es/config/README.md b/cmvr-es/config/README.md index e127e5c4..2375dbdc 100644 --- a/cmvr-es/config/README.md +++ b/cmvr-es/config/README.md @@ -73,9 +73,45 @@ cmvr_es.pb.txt - 新增 loader 对不认识的 enum 和未设置的 oneof 必须明确失败；当前个别历史路径仍有退化默认行为，不应复制； - 设备端口、坐标系、速度和单位写入注释； - `enable` 应由 manager 层控制，后端内部的 enable 字段不能替代 manager 开关； -- QUIC 需要 TaskManager 与 `QuicEdgeConfig.enable` 同时开启； +- QUIC 任务需要在 TaskManager 中显式开启； - QUIC 零媒体轨道是合法配置。 +### QUIC 多平台 + +`QuicEdgeTask` 可以同时连接多个平台。原有顶层 +`server_host`、`server_port`、`tls` 继续表示主平台；每个 `platforms` 条目会与主平台 +并行运行。也可以不配置顶层目标，只使用一个或多个 `platforms` 条目： + +```protobuf +platforms { + id: "operations" + server_host: "192.168.0.222" + server_port: 4433 + enable_media: false + tls { + ca_file: "certs/cmvr-quic-ca.crt" + server_name: "192.168.0.222" + } +} +platforms { + id: "analytics" + server_host: "192.168.0.223" + server_port: 4433 + enable_media: true + tls { + ca_file: "certs/cmvr-quic-ca.crt" + server_name: "192.168.0.223" + } +} +``` + +平台 ID 和 `host:port` 必须分别唯一；配置兼容主平台时，其平台 ID 使用任务 +`QuicEdgeConfig.id`，新增条目也不能与它重名。每个平台拥有独立的 QUIC 连接、 +注册会话、心跳序号、ACK 超时和重连退避，一个平台断线不会阻塞其他平台。 +`enable_media` 默认为 `false`，此时仍发送注册、心跳、网络接口和设备状态，但不会 +复制音视频；设为 `true` 才会把全局 `tracks` 转发到该平台。兼容的顶层主平台保持 +原有媒体行为。 + ### gRPC 相机实时流 [`tasks/grpc_server_task/grpc_server_task.pb.txt`](tasks/grpc_server_task/grpc_server_task.pb.txt) diff --git a/cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt b/cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt index d502fa57..a886cfbf 100644 --- a/cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt +++ b/cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt @@ -29,6 +29,21 @@ quic_edge { allow_insecure: false } + # Additional platforms run concurrently with the primary endpoint above. + # Each one has an independent connection, registration, heartbeat ACK state + # and reconnect loop. Media forwarding is opt-in for additional platforms. + # platforms { + # id: "operations_backup" + # server_host: "192.168.0.223" + # server_port: 4433 + # enable_media: false + # tls { + # ca_file: "certs/cmvr-quic-ca.crt" + # server_name: "192.168.0.223" + # allow_insecure: false + # } + # } + reconnect { initial_delay_ms: 500 maximum_delay_ms: 30000 diff --git a/cmvr-es/task/quic_edge_task/include/quic_edge_task.h b/cmvr-es/task/quic_edge_task/include/quic_edge_task.h index 8e10575d..5e46a54f 100644 --- a/cmvr-es/task/quic_edge_task/include/quic_edge_task.h +++ b/cmvr-es/task/quic_edge_task/include/quic_edge_task.h @@ -1,9 +1,11 @@ #ifndef CMVR_ES_QUIC_EDGE_TASK_H #define CMVR_ES_QUIC_EDGE_TASK_H +#include #include #include #include +#include #include "cmvr/config/quic_edge_config/quic_edge_config.pb.h" #include "service/quic_edge/include/quic_edge_service.h" @@ -13,7 +15,12 @@ namespace cmvr::task { class QuicEdgeTask final : public Task { public: + using ServiceFactory = std::function( + config::QuicEdgeConfig)>; + explicit QuicEdgeTask(const config::QuicEdgeConfig& config); + QuicEdgeTask(const config::QuicEdgeConfig& config, + ServiceFactory service_factory); ~QuicEdgeTask() override; const std::string& id() const override { return id_; } @@ -32,11 +39,19 @@ public: std::string detailStatusString() const override; private: + struct PlatformService { + std::string id; + bool media_enabled{false}; + config::QuicEdgeConfig config; + std::unique_ptr service; + }; + TaskState mappedState() const; config::QuicEdgeConfig config_; std::string id_; - std::unique_ptr service_; + ServiceFactory service_factory_; + std::vector services_; mutable std::mutex mutex_; TaskState state_{TaskState::UNINITIALIZED}; diff --git a/cmvr-es/task/quic_edge_task/src/quic_edge_task.cpp b/cmvr-es/task/quic_edge_task/src/quic_edge_task.cpp index 941615e7..a37bb855 100644 --- a/cmvr-es/task/quic_edge_task/src/quic_edge_task.cpp +++ b/cmvr-es/task/quic_edge_task/src/quic_edge_task.cpp @@ -1,7 +1,10 @@ #include "task/quic_edge_task/include/quic_edge_task.h" +#include #include +#include #include +#include #include "cmvr/config/task_manager_config/task_manager_config.pb.h" #include "common/base/logging/logger.h" @@ -13,6 +16,107 @@ namespace cmvr::task { namespace { +struct PlatformConfig { + std::string id; + bool media_enabled{false}; + config::QuicEdgeConfig config; +}; + +void resolveTlsFiles(config::QuicEdgeTlsConfig* tls) +{ + if (!tls) return; + if (!tls->ca_file().empty()) { + tls->set_ca_file(ConfigHelper::resolveConfigFile(tls->ca_file())); + } + if (!tls->certificate_file().empty()) { + tls->set_certificate_file( + ConfigHelper::resolveConfigFile(tls->certificate_file())); + } + if (!tls->private_key_file().empty()) { + tls->set_private_key_file( + ConfigHelper::resolveConfigFile(tls->private_key_file())); + } +} + +bool appendPlatformConfig( + const config::QuicEdgeConfig& root_config, + const std::string& platform_id, + const std::string& server_host, + const std::uint32_t server_port, + const config::QuicEdgeTlsConfig& tls, + const bool media_enabled, + std::unordered_set* platform_ids, + std::set>* endpoints, + std::vector* platforms, + std::string* error) +{ + if (platform_id.empty()) { + if (error) *error = "QUIC edge platform id is empty"; + return false; + } + if (!platform_ids->insert(platform_id).second) { + if (error) *error = "duplicate QUIC edge platform id: " + platform_id; + return false; + } + if (!endpoints->emplace(server_host, server_port).second) { + if (error) { + *error = "duplicate QUIC edge platform endpoint: " + server_host + + ':' + std::to_string(server_port); + } + return false; + } + + PlatformConfig platform; + platform.id = platform_id; + platform.media_enabled = media_enabled; + platform.config = root_config; + platform.config.set_server_host(server_host); + platform.config.set_server_port(server_port); + *platform.config.mutable_tls() = tls; + platform.config.clear_platforms(); + if (!media_enabled) platform.config.clear_tracks(); + platforms->push_back(std::move(platform)); + return true; +} + +bool expandPlatformConfigs(const config::QuicEdgeConfig& config, + std::vector* platforms, + std::string* error) +{ + if (!platforms) { + if (error) *error = "QUIC edge platform output is null"; + return false; + } + platforms->clear(); + std::unordered_set platform_ids; + std::set> endpoints; + + const bool has_primary_endpoint = !config.server_host().empty() || + config.server_port() != 0U || config.has_tls(); + if (has_primary_endpoint && + !appendPlatformConfig( + config, config.id(), config.server_host(), config.server_port(), + config.tls(), true, &platform_ids, &endpoints, platforms, error)) { + return false; + } + + for (const auto& configured_platform : config.platforms()) { + if (!appendPlatformConfig( + config, configured_platform.id(), + configured_platform.server_host(), + configured_platform.server_port(), configured_platform.tls(), + configured_platform.enable_media(), &platform_ids, &endpoints, + platforms, error)) { + return false; + } + } + if (platforms->empty()) { + if (error) *error = "QUIC edge has no platform endpoint"; + return false; + } + return true; +} + std::shared_ptr createQuicEdgeTask(const config::TaskConfigEntry& entry) { if (entry.id().empty() || entry.config_file().empty()) { @@ -31,17 +135,9 @@ std::shared_ptr createQuicEdgeTask(const config::TaskConfigEntry& entry) << ", config=" << config.id(); return nullptr; } - if (!config.tls().ca_file().empty()) { - config.mutable_tls()->set_ca_file( - ConfigHelper::resolveConfigFile(config.tls().ca_file())); - } - if (!config.tls().certificate_file().empty()) { - config.mutable_tls()->set_certificate_file( - ConfigHelper::resolveConfigFile(config.tls().certificate_file())); - } - if (!config.tls().private_key_file().empty()) { - config.mutable_tls()->set_private_key_file( - ConfigHelper::resolveConfigFile(config.tls().private_key_file())); + if (config.has_tls()) resolveTlsFiles(config.mutable_tls()); + for (auto& platform : *config.mutable_platforms()) { + if (platform.has_tls()) resolveTlsFiles(platform.mutable_tls()); } if (config.software_version().empty()) { config.set_software_version(device::DeviceManager::getInstance().version()); @@ -52,7 +148,19 @@ std::shared_ptr createQuicEdgeTask(const config::TaskConfigEntry& entry) } // namespace QuicEdgeTask::QuicEdgeTask(const config::QuicEdgeConfig& config) - : config_(config), id_(config.id()) + : QuicEdgeTask( + config, + [](config::QuicEdgeConfig platform_config) { + return std::make_unique( + std::move(platform_config)); + }) +{ +} + +QuicEdgeTask::QuicEdgeTask(const config::QuicEdgeConfig& config, + ServiceFactory service_factory) + : config_(config), id_(config.id()), + service_factory_(std::move(service_factory)) { } @@ -69,13 +177,40 @@ bool QuicEdgeTask::init() state_ = TaskState::FAILED; return false; } - service_ = std::make_unique(config_); + + std::vector platform_configs; std::string error; - if (!service_->initialize(&error)) { + if (!service_factory_) { + last_error_ = "QUIC edge service factory is empty"; + state_ = TaskState::FAILED; + return false; + } + if (!expandPlatformConfigs(config_, &platform_configs, &error)) { last_error_ = std::move(error); state_ = TaskState::FAILED; return false; } + + std::vector initialized_services; + initialized_services.reserve(platform_configs.size()); + for (auto& platform_config : platform_configs) { + auto service = service_factory_(platform_config.config); + if (!service) { + last_error_ = "failed to create QUIC edge service for platform " + + platform_config.id; + state_ = TaskState::FAILED; + return false; + } + if (!service->initialize(&error)) { + last_error_ = "platform " + platform_config.id + ": " + error; + state_ = TaskState::FAILED; + return false; + } + initialized_services.push_back(PlatformService{ + std::move(platform_config.id), platform_config.media_enabled, + std::move(platform_config.config), std::move(service)}); + } + services_ = std::move(initialized_services); last_error_.clear(); state_ = TaskState::IDLE; return true; @@ -101,20 +236,30 @@ bool QuicEdgeTask::start() admission.generation() == admission_generation; } if ((state_ != TaskState::IDLE && state_ != TaskState::STOPPED) || - !service_) { + services_.empty()) { last_error_ = "QUIC edge task is not initialized"; state_ = TaskState::FAILED; return false; } - std::string error; - if (!service_->start(&error)) { - last_error_ = std::move(error); - state_ = TaskState::FAILED; - return false; + + std::size_t started_services = 0U; + for (auto& platform : services_) { + std::string error; + if (!platform.service->start(&error)) { + for (std::size_t index = 0U; + index < started_services; ++index) { + services_[index].service->stop(); + } + last_error_ = "platform " + platform.id + ": " + error; + state_ = TaskState::FAILED; + return false; + } + ++started_services; } last_error_.clear(); state_ = TaskState::RUNNING; - CMVR_LOG(INFO) << "[QuicEdgeTask] Started, id=" << id_; + CMVR_LOG(INFO) << "[QuicEdgeTask] Started, id=" << id_ + << ", platforms=" << services_.size(); } bool admission_current = false; @@ -142,26 +287,37 @@ void QuicEdgeTask::stop() std::lock_guard lock(mutex_); // stop() is the task lifecycle terminator used by TaskManager shutdown. // Unlike StopAll's stopActivity(), it intentionally tears down transport. - if (service_) service_->stop(); + for (auto& platform : services_) platform.service->stop(); if (state_ != TaskState::FAILED) state_ = TaskState::STOPPED; } bool QuicEdgeTask::stopActivity() { std::lock_guard lock(mutex_); - if (!service_ || state_ == TaskState::FAILED) { + if (services_.empty() || state_ == TaskState::FAILED) { return false; } // System StopAll must preserve the QUIC presence channel. Only old media // subscriptions are fenced; registration and heartbeat stay online. - return service_->interruptMediaActivities(); + bool all_stopped = true; + for (auto& platform : services_) { + if (!platform.service->interruptMediaActivities()) { + all_stopped = false; + } + } + return all_stopped; } TaskState QuicEdgeTask::mappedState() const { - if (!service_ || state_ != TaskState::RUNNING) return state_; - return service_->state() == quic_edge::QuicEdgeServiceState::FAILED - ? TaskState::FAILED : state_; + if (services_.empty() || state_ != TaskState::RUNNING) return state_; + for (const auto& platform : services_) { + if (platform.service->state() == + quic_edge::QuicEdgeServiceState::FAILED) { + return TaskState::FAILED; + } + } + return state_; } TaskState QuicEdgeTask::state() const @@ -195,31 +351,38 @@ std::string QuicEdgeTask::detailStatusString() const std::lock_guard lock(mutex_); std::ostringstream output; output << taskStateToString(mappedState()); - if (service_) { - const auto stats = service_->stats(); - const auto service_status = service_->status(); - output << " service=" << quic_edge::toString(service_->state()) - << " target=" << config_.server_host() << ':' << config_.server_port() - << " node_id=" << service_status.node_id - << " registered=" << service_status.registered - << " connections=" << stats.successful_connections - << " registrations=" << stats.registrations_accepted - << " heartbeat_sequence=" << service_status.heartbeat_sequence - << " heartbeat_acks=" << stats.heartbeats_acknowledged - << " media_tracks=" << service_status.active_media_tracks - << " frames=" << stats.frames_queued - << " datagrams=" << stats.datagrams_queued; - if (!service_status.session_id.empty()) { - output << " session=" << service_status.session_id; + if (!services_.empty()) { + output << " platforms=" << services_.size(); + for (const auto& platform : services_) { + const auto stats = platform.service->stats(); + const auto service_status = platform.service->status(); + output << " platform[" << platform.id << "]={" + << "service=" << quic_edge::toString(platform.service->state()) + << ",target=" << platform.config.server_host() << ':' + << platform.config.server_port() + << ",media_enabled=" << platform.media_enabled + << ",node_id=" << service_status.node_id + << ",registered=" << service_status.registered + << ",connections=" << stats.successful_connections + << ",registrations=" << stats.registrations_accepted + << ",heartbeat_sequence=" << service_status.heartbeat_sequence + << ",heartbeat_acks=" << stats.heartbeats_acknowledged + << ",media_tracks=" << service_status.active_media_tracks + << ",frames=" << stats.frames_queued + << ",datagrams=" << stats.datagrams_queued; + if (!service_status.session_id.empty()) { + output << ",session=" << service_status.session_id; + } + if (!service_status.observed_source_ip.empty()) { + output << ",observed_ip=" << service_status.observed_source_ip; + } + if (!service_status.last_media_error.empty()) { + output << ",media_error=" << service_status.last_media_error; + } + const std::string service_error = platform.service->lastError(); + if (!service_error.empty()) output << ",error=" << service_error; + output << '}'; } - if (!service_status.observed_source_ip.empty()) { - output << " observed_ip=" << service_status.observed_source_ip; - } - if (!service_status.last_media_error.empty()) { - output << " media_error=" << service_status.last_media_error; - } - const std::string service_error = service_->lastError(); - if (!service_error.empty()) output << " error=" << service_error; } else if (!last_error_.empty()) { output << " error=" << last_error_; } diff --git a/cmvr-es/task/quic_edge_task/tests/quic_edge_task_test.cpp b/cmvr-es/task/quic_edge_task/tests/quic_edge_task_test.cpp index e0c1b0b4..99a05f00 100644 --- a/cmvr-es/task/quic_edge_task/tests/quic_edge_task_test.cpp +++ b/cmvr-es/task/quic_edge_task/tests/quic_edge_task_test.cpp @@ -1,10 +1,179 @@ +#include +#include +#include +#include +#include #include +#include +#include +#include +#include +#include +#include +#include "cmvr/quic_edge/v1/quic_edge.pb.h" +#include "manager/media_source_hub/include/media_source_hub.h" +#include "service/quic_edge/include/control_framing.h" #include "service/stop_all/include/stop_all_admission_gate.h" #include "task/quic_edge_task/include/quic_edge_task.h" namespace { +using Clock = std::chrono::steady_clock; +using QuicEdgeService = cmvr::quic_edge::QuicEdgeService; + +class HeartbeatTransport final : public cmvr::quic_edge::QuicTransport { +public: + explicit HeartbeatTransport(std::string platform_id) + : platform_id_(std::move(platform_id)) + { + } + + bool connect(const cmvr::config::QuicEdgeConfig&, + std::chrono::milliseconds, + std::string*) override + { + connected_.store(true); + return true; + } + + void disconnect() override + { + connected_.store(false); + condition_.notify_all(); + } + + bool isConnected() const override { return connected_.load(); } + std::size_t maximumDatagramBytes() const override { return 1200U; } + std::size_t maximumDatagramBatchPackets() const override { return 32U; } + + cmvr::quic_edge::TransportSendResult sendControl( + std::vector framed_message, + std::string*) override + { + if (!connected_.load()) { + return cmvr::quic_edge::TransportSendResult::DISCONNECTED; + } + + std::vector> frames; + std::string error; + cmvr::quic_edge::ControlFrameDecoder decoder(1024U * 1024U); + if (!decoder.push(framed_message, &frames, &error) || + frames.size() != 1U) { + return cmvr::quic_edge::TransportSendResult::ERROR; + } + cmvr::quic_edge::v1::EdgeControlEnvelope request; + if (!request.ParseFromArray( + frames.front().data(), + static_cast(frames.front().size()))) { + return cmvr::quic_edge::TransportSendResult::ERROR; + } + + cmvr::quic_edge::v1::EdgeControlEnvelope response; + response.set_protocol_version(cmvr::quic_edge::kProtocolVersion); + { + std::lock_guard lock(mutex_); + response.set_message_sequence(server_message_sequence_++); + if (request.has_node_register_request()) { + ++registrations_; + auto* registration = response.mutable_node_register_response(); + registration->set_accepted(true); + registration->set_session_id("session-" + platform_id_); + registration->set_heartbeat_interval_ms(250U); + } else if (request.has_node_heartbeat()) { + const auto& heartbeat = request.node_heartbeat(); + ++heartbeats_; + last_heartbeat_sequence_.store(heartbeat.sequence()); + auto* ack = response.mutable_node_heartbeat_ack(); + ack->set_accepted(true); + ack->set_session_id(heartbeat.session_id()); + ack->set_acknowledged_sequence(heartbeat.sequence()); + } else { + return cmvr::quic_edge::TransportSendResult::ERROR; + } + if (!enqueueResponse(response)) { + return cmvr::quic_edge::TransportSendResult::ERROR; + } + } + condition_.notify_all(); + return cmvr::quic_edge::TransportSendResult::QUEUED; + } + + cmvr::quic_edge::TransportReceiveResult receiveControl( + std::vector* chunk, + const std::chrono::milliseconds timeout, + std::string*) override + { + if (!chunk) return cmvr::quic_edge::TransportReceiveResult::ERROR; + std::unique_lock lock(mutex_); + condition_.wait_for(lock, timeout, [this] { + return !responses_.empty() || !connected_.load(); + }); + if (!responses_.empty()) { + *chunk = std::move(responses_.front()); + responses_.pop_front(); + return cmvr::quic_edge::TransportReceiveResult::DATA; + } + return connected_.load() + ? cmvr::quic_edge::TransportReceiveResult::TIMEOUT + : cmvr::quic_edge::TransportReceiveResult::DISCONNECTED; + } + + cmvr::quic_edge::TransportSendResult sendDatagramBatch( + std::vector, + std::string*) override + { + return connected_.load() + ? cmvr::quic_edge::TransportSendResult::QUEUED + : cmvr::quic_edge::TransportSendResult::DISCONNECTED; + } + + std::uint64_t registrations() const { return registrations_.load(); } + std::uint64_t heartbeats() const { return heartbeats_.load(); } + std::uint64_t lastHeartbeatSequence() const + { + return last_heartbeat_sequence_.load(); + } + +private: + bool enqueueResponse( + const cmvr::quic_edge::v1::EdgeControlEnvelope& response) + { + std::string serialized; + if (!response.SerializeToString(&serialized)) return false; + std::vector framed; + std::string error; + if (!cmvr::quic_edge::ControlFrameEncoder::encode( + reinterpret_cast(serialized.data()), + serialized.size(), 1024U * 1024U, &framed, &error)) { + return false; + } + responses_.push_back(std::move(framed)); + return true; + } + + std::string platform_id_; + std::atomic connected_{false}; + std::atomic registrations_{0U}; + std::atomic heartbeats_{0U}; + std::atomic last_heartbeat_sequence_{0U}; + mutable std::mutex mutex_; + std::condition_variable condition_; + std::deque> responses_; + std::uint64_t server_message_sequence_{0U}; +}; + +template +bool waitUntil(const std::chrono::milliseconds timeout, Predicate predicate) +{ + const auto deadline = Clock::now() + timeout; + while (Clock::now() < deadline) { + if (predicate()) return true; + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + return predicate(); +} + cmvr::config::QuicEdgeConfig validConfig() { cmvr::config::QuicEdgeConfig config; @@ -32,6 +201,36 @@ cmvr::config::QuicEdgeConfig validConfig() return config; } +cmvr::config::QuicEdgeConfig multiPlatformConfig() +{ + auto config = validConfig(); + config.set_id("quic-multi-platform-test"); + config.clear_server_host(); + config.clear_server_port(); + config.clear_tls(); + + auto* platform_a = config.add_platforms(); + platform_a->set_id("platform-a"); + platform_a->set_server_host("192.0.2.10"); + platform_a->set_server_port(4433U); + platform_a->mutable_tls()->set_allow_insecure(true); + + auto* platform_b = config.add_platforms(); + platform_b->set_id("platform-b"); + platform_b->set_server_host("198.51.100.20"); + platform_b->set_server_port(4434U); + platform_b->mutable_tls()->set_allow_insecure(true); + platform_b->set_enable_media(true); + + auto* disabled_track = config.add_tracks(); + disabled_track->set_track_id(1U); + disabled_track->set_source_kind( + cmvr::config::QuicEdgeTrackConfig::SOURCE_KIND_CAMERA); + disabled_track->set_device_id("disabled-test-camera"); + disabled_track->set_enable(false); + return config; +} + } // namespace int main() @@ -97,7 +296,93 @@ int main() std::cerr << "QUIC service did not remain available after StopAll\n"; return 1; } + admission_task.stop(); admission.clearForTesting(); + + cmvr::media::MediaSourceHub media_hub; + std::vector transports; + std::vector services; + std::vector expanded_configs; + cmvr::task::QuicEdgeTask multi_platform_task( + multiPlatformConfig(), + [&](cmvr::config::QuicEdgeConfig platform_config) { + expanded_configs.push_back(platform_config); + auto transport = std::make_unique( + platform_config.server_host()); + transports.push_back(transport.get()); + auto service = std::make_unique( + std::move(platform_config), std::move(transport), media_hub, + [] { return cmvr::device::DeviceManagerSnapshot{}; }); + services.push_back(service.get()); + return service; + }); + if (!multi_platform_task.init() || expanded_configs.size() != 2U || + transports.size() != 2U || services.size() != 2U) { + std::cerr << "multi-platform QUIC task did not create two services: " + << multi_platform_task.detailStatusString() << '\n'; + return 1; + } + if (expanded_configs[0].platforms_size() != 0 || + expanded_configs[1].platforms_size() != 0 || + expanded_configs[0].server_host() != "192.0.2.10" || + expanded_configs[1].server_host() != "198.51.100.20" || + expanded_configs[0].tracks_size() != 0 || + expanded_configs[1].tracks_size() != 1) { + std::cerr << "multi-platform QUIC task expanded invalid service configs\n"; + return 1; + } + if (!multi_platform_task.start()) { + std::cerr << "multi-platform QUIC task did not start: " + << multi_platform_task.detailStatusString() << '\n'; + return 1; + } + const bool both_platforms_online = waitUntil( + std::chrono::seconds(2), [&] { + return transports[0]->registrations() >= 1U && + transports[1]->registrations() >= 1U && + transports[0]->heartbeats() >= 1U && + transports[1]->heartbeats() >= 1U && + services[0]->stats().heartbeats_acknowledged >= 1U && + services[1]->stats().heartbeats_acknowledged >= 1U; + }); + if (!both_platforms_online || + transports[0]->lastHeartbeatSequence() != 1U || + transports[1]->lastHeartbeatSequence() != 1U) { + std::cerr << "both platforms did not independently ACK a heartbeat: " + << multi_platform_task.detailStatusString() << '\n'; + return 1; + } + const std::string multi_status = multi_platform_task.detailStatusString(); + if (multi_status.find("platforms=2") == std::string::npos || + multi_status.find("platform[platform-a]") == std::string::npos || + multi_status.find("platform[platform-b]") == std::string::npos) { + std::cerr << "multi-platform QUIC status is incomplete: " + << multi_status << '\n'; + return 1; + } + multi_platform_task.stop(); + if (multi_platform_task.state() != cmvr::task::TaskState::STOPPED) { + std::cerr << "multi-platform QUIC task did not stop cleanly\n"; + return 1; + } + + auto duplicate_endpoint_config = validConfig(); + auto* duplicate_platform = duplicate_endpoint_config.add_platforms(); + duplicate_platform->set_id("duplicate-primary"); + duplicate_platform->set_server_host( + duplicate_endpoint_config.server_host()); + duplicate_platform->set_server_port( + duplicate_endpoint_config.server_port()); + duplicate_platform->mutable_tls()->set_allow_insecure(true); + cmvr::task::QuicEdgeTask duplicate_endpoint_task( + duplicate_endpoint_config); + if (duplicate_endpoint_task.init() || + duplicate_endpoint_task.detailStatusString().find( + "duplicate QUIC edge platform endpoint") == std::string::npos) { + std::cerr << "duplicate QUIC platform endpoint was not rejected\n"; + return 1; + } + std::cout << "quic_edge_task_test: PASS\n"; return 0; } diff --git a/protos/cmvr/config/quic_edge_config/quic_edge_config.proto b/protos/cmvr/config/quic_edge_config/quic_edge_config.proto index 48486a9c..8e01ef93 100644 --- a/protos/cmvr/config/quic_edge_config/quic_edge_config.proto +++ b/protos/cmvr/config/quic_edge_config/quic_edge_config.proto @@ -43,9 +43,27 @@ message QuicEdgeTrackConfig { string source_track_id = 6; } +message QuicEdgePlatformConfig { + // Stable operator-facing identifier used in logs and task status. It must be + // unique within the task and differ from QuicEdgeConfig.id when the + // backward-compatible primary endpoint is also configured. + string id = 1; + string server_host = 2; + uint32 server_port = 3; + QuicEdgeTlsConfig tls = 4; + + // False keeps registration, heartbeat, IP and device-state reporting while + // preventing accidental duplication of configured media tracks. + bool enable_media = 5; +} + message QuicEdgeConfig { string id = 1; reserved 2; + + // Backward-compatible primary platform endpoint. When configured, it runs + // alongside every endpoint in platforms. New configurations may leave these + // fields empty and declare every destination in platforms instead. string server_host = 3; uint32 server_port = 4; string alpn = 5; @@ -87,6 +105,10 @@ message QuicEdgeConfig { // Immutable robot product identity, for example: // CN-CMVR-MBLRV1-CHAGAN-20260731-001. string robot_id = 22; + + // Every platform owns an independent QUIC connection, registration, + // heartbeat/ACK sequence and reconnect loop. + repeated QuicEdgePlatformConfig platforms = 23; } message QuicEdgeRootConfig { diff --git a/protos/cmvr/quic_edge/v1/README.md b/protos/cmvr/quic_edge/v1/README.md index be3c2178..f34377e8 100644 --- a/protos/cmvr/quic_edge/v1/README.md +++ b/protos/cmvr/quic_edge/v1/README.md @@ -85,8 +85,8 @@ their exact scope and manual gateway options. To enable node presence without media: -1. Configure the gateway address, ALPN `cmvr-quic-edge/1`, TLS trust, node ID, - advertised gRPC endpoint and heartbeat values in +1. Configure one or more gateway addresses, ALPN `cmvr-quic-edge/1`, TLS + trust, node ID, advertised gRPC endpoint and heartbeat values in `cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt`. Certificate paths are relative to the cmvr-es configuration root, for example `certs/quic_gateway_ca.pem`. @@ -99,6 +99,14 @@ media must not suppress node registration, IP reporting or heartbeat. `maximum_frame_bytes` must fit in one atomically admitted DATAGRAM batch; reduce it or increase `datagram_send_queue_depth` when changing the DATAGRAM size. +The backward-compatible top-level `server_host`, `server_port` and `tls` +describe the primary platform. Every `platforms` entry runs concurrently with +that primary endpoint, or the list may be used by itself. Platform IDs and +`host:port` pairs must be unique. Each platform owns its own connection, +registration session, heartbeat/ACK sequence and reconnect loop. Additional +platforms default to presence-only operation; set their `enable_media=true` to +forward the shared `tracks` as a separate media session. + ## Reliable control stream The edge opens one bidirectional reliable stream after the QUIC handshake. Both From b2b0b63fe077541b97822e0a256333dd73146875 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Mon, 17 Aug 2026 08:34:10 +0800 Subject: [PATCH 5/8] chore: remove legacy toppra gitlink --- assets/toppra | 1 - 1 file changed, 1 deletion(-) delete mode 160000 assets/toppra diff --git a/assets/toppra b/assets/toppra deleted file mode 160000 index 3089c789..00000000 --- a/assets/toppra +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 3089c7897a5711aceb39d25919aca8c57b5c5948 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 6/8] 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; From cb5f46c598382ac084db743e32462c966004eb50 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Mon, 17 Aug 2026 09:48:42 +0800 Subject: [PATCH 7/8] refactor: reorganize service and manager modules --- cmvr-es/CMakeLists.txt | 10 +- cmvr-es/common/README.md | 2 +- cmvr-es/devices/README.md | 10 +- cmvr-es/devices/arm/aubo_arm/README.md | 2 +- cmvr-es/manager/README.md | 28 +- .../CMakeLists.txt | 14 +- .../include/control_authority_manager.h | 0 .../src/control_authority_manager.cpp | 2 +- .../tests/control_authority_manager_test.cpp | 2 +- cmvr-es/manager/device_manager/CMakeLists.txt | 2 +- .../device_manager/include/device_manager.h | 12 +- .../include/device_safety_adapters.h | 2 +- .../device_manager/src/device_manager.cpp | 32 +- .../src/device_safety_adapters.cpp | 4 +- .../tests/device_manager_lifecycle_test.cpp | 6 +- .../CMakeLists.txt | 40 +- .../include/device_media_source_adapter.h | 12 +- .../include/media_source_manager.h} | 24 +- .../src/device_media_source_adapter.cpp | 38 +- .../src/media_source_manager.cpp} | 58 +- .../device_media_source_adapter_test.cpp | 8 +- .../tests/media_source_manager_test.cpp} | 200 +++---- .../{safety => safety_manager}/CMakeLists.txt | 32 +- .../include/command_ledger.h | 2 +- .../include/device_safety_endpoint.h | 2 +- .../include/safety_manager.h} | 34 +- .../include/safety_participant.h | 2 +- .../include/safety_reason.h | 0 .../include/safety_snapshot_store.h | 2 +- .../include/safety_types.h | 2 +- .../src/command_ledger.cpp | 2 +- .../src/safety_manager.cpp} | 70 +-- .../src/safety_reason.cpp | 2 +- .../src/safety_snapshot_store.cpp | 2 +- .../tests/command_ledger_test.cpp | 2 +- .../tests/safety_manager_test.cpp} | 74 +-- .../tests/safety_snapshot_store_test.cpp | 2 +- .../manager/task_manager/src/task_manager.cpp | 2 +- .../tests/task_manager_lifecycle_test.cpp | 2 +- cmvr-es/service/CMakeLists.txt | 552 +----------------- cmvr-es/service/README.md | 40 +- cmvr-es/service/grpc/CMakeLists.txt | 550 +++++++++++++++++ .../action/include/action_queue_executor.h | 2 +- .../action/src/action_queue_executor.cpp | 14 +- .../client}/CMakeLists.txt | 0 .../client}/include/grpc_arm_teleop_client.h | 0 .../client}/src/grpc_arm_teleop_client.cpp | 2 +- .../tests/grpc_arm_teleop_client_test.cpp | 2 +- .../camera_operational_activity_registry.h | 0 .../include/camera_ptz_activity_registry.h | 0 .../{ => server}/include/grpc_agv_service.h | 0 .../{ => server}/include/grpc_arm_service.h | 0 .../include/grpc_arm_teleop_service.h | 8 +- .../include/grpc_camera_service.h | 2 +- .../include/grpc_camera_stream_policy.h | 0 .../include/grpc_command_transaction.h | 16 +- .../include/grpc_dexhand_service.h | 0 .../include/grpc_error_logging_interceptor.h | 0 .../{ => server}/include/grpc_head_service.h | 0 .../{ => server}/include/grpc_hlc_service.h | 0 .../include/grpc_microphone_service.h | 0 .../{ => server}/include/grpc_motor_service.h | 2 +- .../include/grpc_recovery_audit.h | 0 .../include/grpc_robot_arm_teleop_backend.h | 2 +- .../include/grpc_safety_participants.h | 6 +- .../{ => server}/include/grpc_safety_proto.h | 2 +- .../grpc/{ => server}/include/grpc_security.h | 2 +- .../include/grpc_speaker_service.h | 0 .../include/grpc_system_service.h | 0 .../include/media_activity_coordinator.h | 2 +- .../include/motor_activity_coordinator.h | 2 +- .../camera_operational_activity_registry.cpp | 4 +- .../src/camera_ptz_activity_registry.cpp | 4 +- .../{ => server}/src/grpc_agv_service.cpp | 40 +- .../{ => server}/src/grpc_arm_service.cpp | 32 +- .../src/grpc_arm_teleop_service.cpp | 16 +- .../{ => server}/src/grpc_camera_service.cpp | 38 +- .../src/grpc_command_transaction.cpp | 14 +- .../{ => server}/src/grpc_dexhand_service.cpp | 20 +- .../src/grpc_error_logging_interceptor.cpp | 2 +- .../{ => server}/src/grpc_head_service.cpp | 16 +- .../{ => server}/src/grpc_hlc_service.cpp | 8 +- .../src/grpc_microphone_service.cpp | 26 +- .../{ => server}/src/grpc_motor_service.cpp | 24 +- .../{ => server}/src/grpc_recovery_audit.cpp | 2 +- .../src/grpc_robot_arm_teleop_backend.cpp | 2 +- .../src/grpc_safety_participants.cpp | 34 +- .../{ => server}/src/grpc_safety_proto.cpp | 2 +- .../grpc/{ => server}/src/grpc_security.cpp | 2 +- .../{ => server}/src/grpc_speaker_service.cpp | 18 +- .../{ => server}/src/grpc_system_service.cpp | 52 +- .../src/media_activity_coordinator.cpp | 4 +- .../src/motor_activity_coordinator.cpp | 2 +- ...era_operational_activity_registry_test.cpp | 4 +- .../camera_ptz_activity_registry_test.cpp | 4 +- .../tests/grpc_agv_service_test.cpp | 6 +- .../tests}/grpc_arm_client_test.cpp | 0 .../tests/grpc_arm_service_test.cpp | 10 +- .../tests/grpc_arm_teleop_service_test.cpp | 16 +- .../tests/grpc_camera_stream_policy_test.cpp | 2 +- .../tests/grpc_command_transaction_test.cpp | 32 +- .../tests/grpc_dexhand_service_test.cpp | 8 +- .../grpc_error_logging_interceptor_test.cpp | 2 +- .../tests/grpc_head_service_test.cpp | 6 +- .../tests}/grpc_hlc_client_test.cpp | 0 .../tests/grpc_motor_service_test.cpp | 8 +- .../grpc_robot_arm_teleop_backend_test.cpp | 2 +- .../{ => server}/tests/grpc_security_test.cpp | 2 +- .../tests/grpc_system_service_test.cpp | 50 +- .../tests/media_activity_coordinator_test.cpp | 2 +- .../tests/motor_activity_coordinator_test.cpp | 2 +- .../{ => grpc}/stop_all/CMakeLists.txt | 6 +- .../include/deferred_stop_operation.h | 0 .../include/stop_all_admission_gate.h | 0 .../include/stop_operation_dispatcher.h | 0 .../stop_all/src/stop_all_admission_gate.cpp | 2 +- .../src/stop_operation_dispatcher.cpp | 2 +- .../tests/stop_all_admission_gate_test.cpp | 2 +- .../tests/stop_operation_dispatcher_test.cpp | 2 +- cmvr-es/service/quic_edge/CMakeLists.txt | 4 +- .../quic_edge/include/quic_edge_service.h | 8 +- .../quic_edge/include/quic_edge_types.h | 2 +- .../src/quic_edge_device_adapter.cpp | 6 +- .../quic_edge/src/quic_edge_service.cpp | 24 +- .../tests/quic_edge_protocol_test.cpp | 66 +-- cmvr-es/task/CMakeLists.txt | 2 +- .../grpc_server_task/src/grpc_server_task.cpp | 32 +- .../quic_edge_task/src/quic_edge_task.cpp | 2 +- .../tests/quic_edge_task_test.cpp | 6 +- .../include/touch_screen_task.h | 2 +- .../src/touch_screen_admission_test.cpp | 4 +- .../src/touch_screen_task.cpp | 4 +- .../ume_teleop_task/include/ume_teleop_task.h | 2 +- .../ume_teleop_task/src/ume_teleop_task.cpp | 2 +- .../tests/ume_teleop_task_test.cpp | 4 +- ...evice_safety_control_plane_architecture.md | 64 +- .../device_manager_config.proto | 4 +- .../quic_edge_config/quic_edge_config.proto | 2 +- protos/cmvr/quic_edge/v1/README.md | 4 +- protos/cmvr/quic_edge/v1/quic_edge.proto | 2 +- test/e2e/CMakeLists.txt | 6 +- test/e2e/README.md | 2 +- test/e2e/quic_msquic_e2e_test.cpp | 20 +- test/quic_gateway/README.md | 2 +- 144 files changed, 1395 insertions(+), 1361 deletions(-) rename cmvr-es/manager/{control_authority => control_authority_manager}/CMakeLists.txt (62%) rename cmvr-es/manager/{control_authority => control_authority_manager}/include/control_authority_manager.h (100%) rename cmvr-es/manager/{control_authority => control_authority_manager}/src/control_authority_manager.cpp (99%) rename cmvr-es/manager/{control_authority => control_authority_manager}/tests/control_authority_manager_test.cpp (99%) rename cmvr-es/manager/{media_source_hub => media_source_manager}/CMakeLists.txt (70%) rename cmvr-es/manager/{media_source_hub => media_source_manager}/include/device_media_source_adapter.h (84%) rename cmvr-es/manager/{media_source_hub/include/media_source_hub.h => media_source_manager/include/media_source_manager.h} (90%) rename cmvr-es/manager/{media_source_hub => media_source_manager}/src/device_media_source_adapter.cpp (96%) rename cmvr-es/manager/{media_source_hub/src/media_source_hub.cpp => media_source_manager/src/media_source_manager.cpp} (92%) rename cmvr-es/manager/{media_source_hub => media_source_manager}/tests/device_media_source_adapter_test.cpp (93%) rename cmvr-es/manager/{media_source_hub/tests/media_source_hub_test.cpp => media_source_manager/tests/media_source_manager_test.cpp} (89%) rename cmvr-es/manager/{safety => safety_manager}/CMakeLists.txt (58%) rename cmvr-es/manager/{safety => safety_manager}/include/command_ledger.h (98%) rename cmvr-es/manager/{safety => safety_manager}/include/device_safety_endpoint.h (95%) rename cmvr-es/manager/{safety/include/safety_coordinator.h => safety_manager/include/safety_manager.h} (89%) rename cmvr-es/manager/{safety => safety_manager}/include/safety_participant.h (97%) rename cmvr-es/manager/{safety => safety_manager}/include/safety_reason.h (100%) rename cmvr-es/manager/{safety => safety_manager}/include/safety_snapshot_store.h (96%) rename cmvr-es/manager/{safety => safety_manager}/include/safety_types.h (99%) rename cmvr-es/manager/{safety => safety_manager}/src/command_ledger.cpp (99%) rename cmvr-es/manager/{safety/src/safety_coordinator.cpp => safety_manager/src/safety_manager.cpp} (97%) rename cmvr-es/manager/{safety => safety_manager}/src/safety_reason.cpp (98%) rename cmvr-es/manager/{safety => safety_manager}/src/safety_snapshot_store.cpp (99%) rename cmvr-es/manager/{safety => safety_manager}/tests/command_ledger_test.cpp (98%) rename cmvr-es/manager/{safety/tests/safety_coordinator_test.cpp => safety_manager/tests/safety_manager_test.cpp} (91%) rename cmvr-es/manager/{safety => safety_manager}/tests/safety_snapshot_store_test.cpp (98%) create mode 100644 cmvr-es/service/grpc/CMakeLists.txt rename cmvr-es/service/{ => grpc}/action/include/action_queue_executor.h (98%) rename cmvr-es/service/{ => grpc}/action/src/action_queue_executor.cpp (99%) rename cmvr-es/service/{arm_teleop_client => grpc/client}/CMakeLists.txt (100%) rename cmvr-es/service/{arm_teleop_client => grpc/client}/include/grpc_arm_teleop_client.h (100%) rename cmvr-es/service/{arm_teleop_client => grpc/client}/src/grpc_arm_teleop_client.cpp (98%) rename cmvr-es/service/{arm_teleop_client => grpc/client}/tests/grpc_arm_teleop_client_test.cpp (98%) rename cmvr-es/service/grpc/{ => server}/include/camera_operational_activity_registry.h (100%) rename cmvr-es/service/grpc/{ => server}/include/camera_ptz_activity_registry.h (100%) rename cmvr-es/service/grpc/{ => server}/include/grpc_agv_service.h (100%) rename cmvr-es/service/grpc/{ => server}/include/grpc_arm_service.h (100%) rename cmvr-es/service/grpc/{ => server}/include/grpc_arm_teleop_service.h (93%) rename cmvr-es/service/grpc/{ => server}/include/grpc_camera_service.h (97%) rename cmvr-es/service/grpc/{ => server}/include/grpc_camera_stream_policy.h (100%) rename cmvr-es/service/grpc/{ => server}/include/grpc_command_transaction.h (95%) rename cmvr-es/service/grpc/{ => server}/include/grpc_dexhand_service.h (100%) rename cmvr-es/service/grpc/{ => server}/include/grpc_error_logging_interceptor.h (100%) rename cmvr-es/service/grpc/{ => server}/include/grpc_head_service.h (100%) rename cmvr-es/service/grpc/{ => server}/include/grpc_hlc_service.h (100%) rename cmvr-es/service/grpc/{ => server}/include/grpc_microphone_service.h (100%) rename cmvr-es/service/grpc/{ => server}/include/grpc_motor_service.h (99%) rename cmvr-es/service/grpc/{ => server}/include/grpc_recovery_audit.h (100%) rename cmvr-es/service/grpc/{ => server}/include/grpc_robot_arm_teleop_backend.h (89%) rename cmvr-es/service/grpc/{ => server}/include/grpc_safety_participants.h (92%) rename cmvr-es/service/grpc/{ => server}/include/grpc_safety_proto.h (95%) rename cmvr-es/service/grpc/{ => server}/include/grpc_security.h (99%) rename cmvr-es/service/grpc/{ => server}/include/grpc_speaker_service.h (100%) rename cmvr-es/service/grpc/{ => server}/include/grpc_system_service.h (100%) rename cmvr-es/service/grpc/{ => server}/include/media_activity_coordinator.h (98%) rename cmvr-es/service/grpc/{ => server}/include/motor_activity_coordinator.h (98%) rename cmvr-es/service/grpc/{ => server}/src/camera_operational_activity_registry.cpp (98%) rename cmvr-es/service/grpc/{ => server}/src/camera_ptz_activity_registry.cpp (98%) rename cmvr-es/service/grpc/{ => server}/src/grpc_agv_service.cpp (97%) rename cmvr-es/service/grpc/{ => server}/src/grpc_arm_service.cpp (97%) rename cmvr-es/service/grpc/{ => server}/src/grpc_arm_teleop_service.cpp (99%) rename cmvr-es/service/grpc/{ => server}/src/grpc_camera_service.cpp (97%) rename cmvr-es/service/grpc/{ => server}/src/grpc_command_transaction.cpp (99%) rename cmvr-es/service/grpc/{ => server}/src/grpc_dexhand_service.cpp (98%) rename cmvr-es/service/grpc/{ => server}/src/grpc_error_logging_interceptor.cpp (99%) rename cmvr-es/service/grpc/{ => server}/src/grpc_head_service.cpp (98%) rename cmvr-es/service/grpc/{ => server}/src/grpc_hlc_service.cpp (96%) rename cmvr-es/service/grpc/{ => server}/src/grpc_microphone_service.cpp (95%) rename cmvr-es/service/grpc/{ => server}/src/grpc_motor_service.cpp (99%) rename cmvr-es/service/grpc/{ => server}/src/grpc_recovery_audit.cpp (98%) rename cmvr-es/service/grpc/{ => server}/src/grpc_robot_arm_teleop_backend.cpp (99%) rename cmvr-es/service/grpc/{ => server}/src/grpc_safety_participants.cpp (97%) rename cmvr-es/service/grpc/{ => server}/src/grpc_safety_proto.cpp (99%) rename cmvr-es/service/grpc/{ => server}/src/grpc_security.cpp (99%) rename cmvr-es/service/grpc/{ => server}/src/grpc_speaker_service.cpp (97%) rename cmvr-es/service/grpc/{ => server}/src/grpc_system_service.cpp (97%) rename cmvr-es/service/grpc/{ => server}/src/media_activity_coordinator.cpp (99%) rename cmvr-es/service/grpc/{ => server}/src/motor_activity_coordinator.cpp (99%) rename cmvr-es/service/grpc/{ => server}/tests/camera_operational_activity_registry_test.cpp (99%) rename cmvr-es/service/grpc/{ => server}/tests/camera_ptz_activity_registry_test.cpp (98%) rename cmvr-es/service/grpc/{ => server}/tests/grpc_agv_service_test.cpp (99%) rename cmvr-es/service/grpc/{src => server/tests}/grpc_arm_client_test.cpp (100%) rename cmvr-es/service/grpc/{ => server}/tests/grpc_arm_service_test.cpp (99%) rename cmvr-es/service/grpc/{ => server}/tests/grpc_arm_teleop_service_test.cpp (98%) rename cmvr-es/service/grpc/{ => server}/tests/grpc_camera_stream_policy_test.cpp (96%) rename cmvr-es/service/grpc/{ => server}/tests/grpc_command_transaction_test.cpp (95%) rename cmvr-es/service/grpc/{ => server}/tests/grpc_dexhand_service_test.cpp (97%) rename cmvr-es/service/grpc/{ => server}/tests/grpc_error_logging_interceptor_test.cpp (99%) rename cmvr-es/service/grpc/{ => server}/tests/grpc_head_service_test.cpp (98%) rename cmvr-es/service/grpc/{src => server/tests}/grpc_hlc_client_test.cpp (100%) rename cmvr-es/service/grpc/{ => server}/tests/grpc_motor_service_test.cpp (99%) rename cmvr-es/service/grpc/{ => server}/tests/grpc_robot_arm_teleop_backend_test.cpp (99%) rename cmvr-es/service/grpc/{ => server}/tests/grpc_security_test.cpp (99%) rename cmvr-es/service/grpc/{ => server}/tests/grpc_system_service_test.cpp (98%) rename cmvr-es/service/grpc/{ => server}/tests/media_activity_coordinator_test.cpp (99%) rename cmvr-es/service/grpc/{ => server}/tests/motor_activity_coordinator_test.cpp (99%) rename cmvr-es/service/{ => grpc}/stop_all/CMakeLists.txt (83%) rename cmvr-es/service/{ => grpc}/stop_all/include/deferred_stop_operation.h (100%) rename cmvr-es/service/{ => grpc}/stop_all/include/stop_all_admission_gate.h (100%) rename cmvr-es/service/{ => grpc}/stop_all/include/stop_operation_dispatcher.h (100%) rename cmvr-es/service/{ => grpc}/stop_all/src/stop_all_admission_gate.cpp (97%) rename cmvr-es/service/{ => grpc}/stop_all/src/stop_operation_dispatcher.cpp (98%) rename cmvr-es/service/{ => grpc}/stop_all/tests/stop_all_admission_gate_test.cpp (97%) rename cmvr-es/service/{ => grpc}/stop_all/tests/stop_operation_dispatcher_test.cpp (99%) diff --git a/cmvr-es/CMakeLists.txt b/cmvr-es/CMakeLists.txt index f98ed533..1d6f8080 100644 --- a/cmvr-es/CMakeLists.txt +++ b/cmvr-es/CMakeLists.txt @@ -6,15 +6,15 @@ add_subdirectory(hardware) add_subdirectory(algorithms) add_subdirectory(simulate) add_subdirectory(devices) -add_subdirectory(manager/control_authority) -add_subdirectory(manager/safety) +add_subdirectory(manager/control_authority_manager) +add_subdirectory(manager/safety_manager) add_subdirectory(manager/device_manager) -add_subdirectory(service/stop_all) -add_subdirectory(manager/media_source_hub) +add_subdirectory(service/grpc/stop_all) +add_subdirectory(manager/media_source_manager) add_subdirectory(service/quic_edge) -add_subdirectory(service/arm_teleop_client) add_subdirectory(task) add_subdirectory(task/quic_edge_task) +add_subdirectory(service/grpc/client) add_subdirectory(task/ume_teleop_task) add_subdirectory(manager/task_manager) add_subdirectory(service) diff --git a/cmvr-es/common/README.md b/cmvr-es/common/README.md index cf60bb8d..a8a21010 100644 --- a/cmvr-es/common/README.md +++ b/cmvr-es/common/README.md @@ -54,7 +54,7 @@ AGV 通用类型应参考 [`types/agv/agv_types.h`](types/agv/agv_types.h)，机 - AAC、Opus、PCM 明确 payload format、采样率和声道数； - 不把 QUIC、gRPC 或浏览器专有字段加入通用帧。 -设备媒体接入流程见 [`../manager/README.md`](../manager/README.md) 的 MediaSourceHub 章节。 +设备媒体接入流程见 [`../manager/README.md`](../manager/README.md) 的 MediaSourceManager 章节。 ## 环形队列选择 diff --git a/cmvr-es/devices/README.md b/cmvr-es/devices/README.md index b979284f..f7175729 100644 --- a/cmvr-es/devices/README.md +++ b/cmvr-es/devices/README.md @@ -222,7 +222,7 @@ CameraDeviceConfig / AGVDeviceConfig / ... 的外层 id ## 摄像头与麦克风实时流 -设备实现抽象流接口后，由 [`../manager/media_source_hub/`](../manager/media_source_hub/) 适配给 gRPC 和 QUIC，不应在设备后端实现两套协议代码。 +设备实现抽象流接口后，由 [`../manager/media_source_manager/`](../manager/media_source_manager/) 适配给 gRPC 和 QUIC，不应在设备后端实现两套协议代码。 当前 Hub 轨道： @@ -312,7 +312,7 @@ adapter 检测到描述变化后创建新 descriptor，设备后端不要自行 - 满队列覆盖旧数据是实时媒体的预期行为； - `waitEncodedFrame()` 必须有有限 timeout，不能永久阻塞。 -MediaSourceHub Subscription 同样是单消费者对象，不同协议或客户端必须各自订阅。 +MediaSourceManager Subscription 同样是单消费者对象，不同协议或客户端必须各自订阅。 发布后的 `MediaFrame`、`TrackDescriptor` 和 payload 不可再修改。 @@ -334,19 +334,19 @@ MediaSourceHub Subscription 同样是单消费者对象，不同协议或客户 无硬件参考测试： - [`camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp`](camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp) -- [`../manager/media_source_hub/tests/media_source_hub_test.cpp`](../manager/media_source_hub/tests/media_source_hub_test.cpp) +- [`../manager/media_source_manager/tests/media_source_manager_test.cpp`](../manager/media_source_manager/tests/media_source_manager_test.cpp) ```bash cmake -S . -B build \ -DCMVR_ARCH=x86 \ -DBUILD_TESTING=ON \ - -DCMVR_MEDIA_SOURCE_HUB_BUILD_TESTS=ON + -DCMVR_MEDIA_SOURCE_MANAGER_BUILD_TESTS=ON cmake --build build -j"$(nproc)" ctest \ --test-dir build \ - -R 'hikvision_camera_callback_test|media_source_hub_test' \ + -R 'hikvision_camera_callback_test|media_source_manager_test' \ --output-on-failure ``` diff --git a/cmvr-es/devices/arm/aubo_arm/README.md b/cmvr-es/devices/arm/aubo_arm/README.md index 55ef34f1..669aa42d 100644 --- a/cmvr-es/devices/arm/aubo_arm/README.md +++ b/cmvr-es/devices/arm/aubo_arm/README.md @@ -17,7 +17,7 @@ - DeviceManager 配置： [`../../../config/manager/device_manager.pb.txt`](../../../config/manager/device_manager.pb.txt) - ArmService 实现： - [`../../../service/grpc/src/grpc_arm_service.cpp`](../../../service/grpc/src/grpc_arm_service.cpp) + [`../../../service/grpc/server/src/grpc_arm_service.cpp`](../../../service/grpc/server/src/grpc_arm_service.cpp) - Proto：[`../../../../protos/cmvr/api/arm_service.proto`](../../../../protos/cmvr/api/arm_service.proto) 仓库配置使用 SDK RPC 端口 `30004`。现场部署必须填写真实控制器地址和凭据， diff --git a/cmvr-es/manager/README.md b/cmvr-es/manager/README.md index b30bc369..620a6288 100644 --- a/cmvr-es/manager/README.md +++ b/cmvr-es/manager/README.md @@ -6,13 +6,20 @@ ## 当前管理器 +管理模块目录统一使用 `*_manager` 后缀，主管理类使用 `*Manager` 后缀。工厂、适配器、 +账本、快照和结果结构体属于管理器内部的支撑类型，保留其职责名称，不强行改成 +`*Manager`。 + | 目录 | CMake target | 职责 | | --- | --- | --- | +| [`control_authority_manager/`](control_authority_manager/) | `cmvr_es::control_authority_manager` | 控制权租约、代际、dispatch fence 和 quarantine | | [`device_manager/`](device_manager/) | `cmvr_es::device_manager` | 按配置创建、初始化、查询和批量启停设备 | +| [`safety_manager/`](safety_manager/) | `cmvr_es::safety_manager` | Sensor/Control 安全准入、StopAll、恢复和命令账本 | | [`task_manager/`](task_manager/) | `cmvr_es::task_manager` | 创建任务、校验运行模式、统一启停和调度周期任务 | -| [`media_source_hub/`](media_source_hub/) | `cmvr_es::media_source_hub`、`cmvr_es::device_media_source_adapter` | 实时媒体源注册、按需启停和多消费者分发 | +| [`media_source_manager/`](media_source_manager/) | `cmvr_es::media_source_manager`、`cmvr_es::device_media_source_adapter` | 实时媒体源注册、按需启停和多消费者分发 | -`manager/` 当前没有聚合 `CMakeLists.txt`，三个子目录由 [`../CMakeLists.txt`](../CMakeLists.txt) 分别加入。新增 manager 时必须显式更新该文件。 +`manager/` 当前没有聚合 `CMakeLists.txt`，所有模块由 [`../CMakeLists.txt`](../CMakeLists.txt) +按依赖顺序加入。新增 manager 时必须同时更新目录、target、依赖顺序和本 README。 ## 进程生命周期 @@ -136,12 +143,12 @@ - 有顺序依赖的工作应放入同一协调任务或显式建模； - task 返回后，其内部状态并发安全由具体实现负责。 -## MediaSourceHub +## MediaSourceManager 关键文件： -- [`media_source_hub/include/media_source_hub.h`](media_source_hub/include/media_source_hub.h) -- [`media_source_hub/src/device_media_source_adapter.cpp`](media_source_hub/src/device_media_source_adapter.cpp) +- [`media_source_manager/include/media_source_manager.h`](media_source_manager/include/media_source_manager.h) +- [`media_source_manager/src/device_media_source_adapter.cpp`](media_source_manager/src/device_media_source_adapter.cpp) - [`../common/media/media_frame.h`](../common/media/media_frame.h) - [`../common/base/ring_buffer.h`](../common/base/ring_buffer.h) @@ -152,7 +159,8 @@ | 摄像头彩色流 | `/video/color` | 64 | | 麦克风主流 | `/audio/main` | 256 | -当前 gRPC RGB/麦克风流和 QUIC 彩色/麦克风轨道使用 Hub；gRPC Depth/RGBD 仍直接读取设备帧。 +当前 gRPC RGB/麦克风流和 QUIC 彩色/麦克风轨道使用 MediaSourceManager；gRPC Depth/RGBD +仍直接读取设备帧。 ### 注册新媒体源 @@ -211,7 +219,7 @@ ring generation 不等于 `TrackDescriptor::generation`，ring 的 `ReadResult.s ## 新增第四种 Manager -1. 先确认能力不是 DeviceManager、TaskManager 或 MediaSourceHub 的子职责； +1. 先确认能力不是 DeviceManager、TaskManager 或 MediaSourceManager 的子职责； 2. 定义所有权、初始化、start/stop 和线程模型； 3. 避免新增无必要的全局单例； 4. 新建独立目录、头文件、实现和 CMake target； @@ -221,13 +229,13 @@ ring generation 不等于 `TrackDescriptor::generation`，ring 的 `ReadResult.s ## 测试 -MediaSourceHub： +MediaSourceManager： ```bash -cmake --build build --target media_source_hub_test +cmake --build build --target media_source_manager_test ctest \ --test-dir build \ - -R '^media_source_hub_test$' \ + -R '^media_source_manager_test$' \ --output-on-failure ``` diff --git a/cmvr-es/manager/control_authority/CMakeLists.txt b/cmvr-es/manager/control_authority_manager/CMakeLists.txt similarity index 62% rename from cmvr-es/manager/control_authority/CMakeLists.txt rename to cmvr-es/manager/control_authority_manager/CMakeLists.txt index f2950a43..f69da862 100644 --- a/cmvr-es/manager/control_authority/CMakeLists.txt +++ b/cmvr-es/manager/control_authority_manager/CMakeLists.txt @@ -1,17 +1,17 @@ -add_library(control_authority STATIC +add_library(control_authority_manager STATIC src/control_authority_manager.cpp ) -target_compile_features(control_authority PUBLIC cxx_std_17) -target_include_directories(control_authority +target_compile_features(control_authority_manager PUBLIC cxx_std_17) +target_include_directories(control_authority_manager PUBLIC ${PROJECT_SOURCE_DIR}/cmvr-es ) add_library( - cmvr_es::control_authority - ALIAS control_authority + cmvr_es::control_authority_manager + ALIAS control_authority_manager ) -install(TARGETS control_authority ARCHIVE DESTINATION lib) +install(TARGETS control_authority_manager ARCHIVE DESTINATION lib) if(BUILD_TESTING) add_executable(control_authority_manager_test @@ -19,7 +19,7 @@ if(BUILD_TESTING) ) target_link_libraries(control_authority_manager_test PRIVATE - cmvr_es::control_authority + cmvr_es::control_authority_manager gtest gtest_main pthread diff --git a/cmvr-es/manager/control_authority/include/control_authority_manager.h b/cmvr-es/manager/control_authority_manager/include/control_authority_manager.h similarity index 100% rename from cmvr-es/manager/control_authority/include/control_authority_manager.h rename to cmvr-es/manager/control_authority_manager/include/control_authority_manager.h diff --git a/cmvr-es/manager/control_authority/src/control_authority_manager.cpp b/cmvr-es/manager/control_authority_manager/src/control_authority_manager.cpp similarity index 99% rename from cmvr-es/manager/control_authority/src/control_authority_manager.cpp rename to cmvr-es/manager/control_authority_manager/src/control_authority_manager.cpp index 614c32d4..19063685 100644 --- a/cmvr-es/manager/control_authority/src/control_authority_manager.cpp +++ b/cmvr-es/manager/control_authority_manager/src/control_authority_manager.cpp @@ -1,4 +1,4 @@ -#include "manager/control_authority/include/control_authority_manager.h" +#include "manager/control_authority_manager/include/control_authority_manager.h" #include diff --git a/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp b/cmvr-es/manager/control_authority_manager/tests/control_authority_manager_test.cpp similarity index 99% rename from cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp rename to cmvr-es/manager/control_authority_manager/tests/control_authority_manager_test.cpp index 02d92671..f0703b7b 100644 --- a/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp +++ b/cmvr-es/manager/control_authority_manager/tests/control_authority_manager_test.cpp @@ -1,4 +1,4 @@ -#include "manager/control_authority/include/control_authority_manager.h" +#include "manager/control_authority_manager/include/control_authority_manager.h" #include #include diff --git a/cmvr-es/manager/device_manager/CMakeLists.txt b/cmvr-es/manager/device_manager/CMakeLists.txt index 67029d2e..5183fced 100644 --- a/cmvr-es/manager/device_manager/CMakeLists.txt +++ b/cmvr-es/manager/device_manager/CMakeLists.txt @@ -8,7 +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::safety_manager 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 1a353457..a974bce0 100644 --- a/cmvr-es/manager/device_manager/include/device_manager.h +++ b/cmvr-es/manager/device_manager/include/device_manager.h @@ -15,7 +15,7 @@ #include "device_factory.h" #include "cmvr/config/device_manager_config/device_manager_config.pb.h" -#include "manager/safety/include/safety_coordinator.h" +#include "manager/safety_manager/include/safety_manager.h" namespace cmvr::device { @@ -48,13 +48,13 @@ namespace cmvr::device { std::vector inventorySnapshot() const; DeviceManagerSnapshot snapshot() const; - safety::SafetyCoordinator& safetyCoordinator() noexcept + safety::SafetyManager& safetyManager() noexcept { - return *safety_coordinator_; + return *safety_manager_; } - const safety::SafetyCoordinator& safetyCoordinator() const noexcept + const safety::SafetyManager& safetyManager() const noexcept { - return *safety_coordinator_; + return *safety_manager_; } std::string version() const; @@ -74,7 +74,7 @@ namespace cmvr::device { std::unordered_map devices_; std::unordered_map device_statuses_; std::unique_ptr dev_factory_; - std::unique_ptr safety_coordinator_; + std::unique_ptr safety_manager_; bool initialized_{false}; explicit DeviceManager(const config::DeviceManagerConfig &cfg); diff --git a/cmvr-es/manager/device_manager/include/device_safety_adapters.h b/cmvr-es/manager/device_manager/include/device_safety_adapters.h index 346cd10d..359d7f4e 100644 --- a/cmvr-es/manager/device_manager/include/device_safety_adapters.h +++ b/cmvr-es/manager/device_manager/include/device_safety_adapters.h @@ -4,7 +4,7 @@ #include #include "devices/abstract_device.h" -#include "manager/safety/include/safety_participant.h" +#include "manager/safety_manager/include/safety_participant.h" 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 c10ea258..2ffffbb1 100644 --- a/cmvr-es/manager/device_manager/src/device_manager.cpp +++ b/cmvr-es/manager/device_manager/src/device_manager.cpp @@ -38,10 +38,10 @@ using MotorJointSelections = std::unordered_map()), - safety_coordinator_(std::make_unique( + safety_manager_(std::make_unique( safetyConfigFrom(cfg))) { initialize_device_statuses_(); @@ -313,7 +313,7 @@ bool DeviceManager::start(){ bool started = false; std::string error_message; try { - (void)safety_coordinator_->advanceDeviceGeneration(id); + (void)safety_manager_->advanceDeviceGeneration(id); started = device->start(); if (!started) { error_message = "device start returned false: " + id; @@ -342,7 +342,7 @@ bool DeviceManager::start(){ } } if (all_started) { - const auto coverage = safety_coordinator_->validateStartupCoverage( + const auto coverage = safety_manager_->validateStartupCoverage( safety::SafetyClock::now() + kSafetyStartupValidationTimeout); if (!coverage.ready) { all_started = false; @@ -363,7 +363,7 @@ bool DeviceManager::start(){ // explicit stop() records Stopped/Error transitions. stop_devices_(false); } else { - safety_coordinator_->markStartupComplete(); + safety_manager_->markStartupComplete(); } return all_started; } @@ -422,7 +422,7 @@ 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( + safety_manager_->updateDeviceRuntimeState( id, ManagedDeviceState::Stopped, sample_device_health_(device)); } else { @@ -431,7 +431,7 @@ void DeviceManager::stop_devices_(const bool update_status) { id, ManagedDeviceState::Error, error_message); } CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id << " Failed"; - safety_coordinator_->updateDeviceRuntimeState( + safety_manager_->updateDeviceRuntimeState( id, ManagedDeviceState::Error, {DeviceHealthState::Fault, error_message}); } @@ -634,7 +634,7 @@ void DeviceManager::update_device_status_( status.status_updated_at_unix_ms = unixTimeMs(); health = status.health; } - safety_coordinator_->updateDeviceRuntimeState(device_id, state, health); + safety_manager_->updateDeviceRuntimeState(device_id, state, health); } DeviceManagerSnapshot DeviceManager::snapshot() const @@ -724,7 +724,7 @@ void DeviceManager::update_device_health_( status.status_updated_at_unix_ms = unixTimeMs(); health = status.health; } - safety_coordinator_->updateDeviceRuntimeState( + safety_manager_->updateDeviceRuntimeState( device_id, lifecycle, std::move(health)); } @@ -750,7 +750,7 @@ bool DeviceManager::register_device_safety_( return false; } const bool registered = - safety_coordinator_->registerDevice(std::move(registration)); + safety_manager_->registerDevice(std::move(registration)); return registered; } diff --git a/cmvr-es/manager/device_manager/src/device_safety_adapters.cpp b/cmvr-es/manager/device_manager/src/device_safety_adapters.cpp index 6486c2fa..eb627d0f 100644 --- a/cmvr-es/manager/device_manager/src/device_safety_adapters.cpp +++ b/cmvr-es/manager/device_manager/src/device_safety_adapters.cpp @@ -21,8 +21,8 @@ #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" +#include "manager/control_authority_manager/include/control_authority_manager.h" +#include "manager/safety_manager/include/device_safety_endpoint.h" 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 07f3a456..4149a835 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 @@ -127,14 +127,14 @@ TEST_F(DeviceManagerLifecycleTest, cmvr::config::DeviceManagerConfig config; auto* safety = config.mutable_safety(); safety->set_mode( - cmvr::config::SafetyCoordinatorConfig::ENFORCE_SELECTED); + cmvr::config::SafetyManagerConfig::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, + manager.safetyManager().snapshot().system_state, cmvr::safety::SystemAdmissionState::Starting); } @@ -143,7 +143,7 @@ TEST_F(DeviceManagerLifecycleTest, { cmvr::config::DeviceManagerConfig config; config.mutable_safety()->set_mode( - cmvr::config::SafetyCoordinatorConfig::ENFORCE_SELECTED); + cmvr::config::SafetyManagerConfig::ENFORCE_SELECTED); auto& manager = cmvr::device::DeviceManager::getInstance(config); ASSERT_TRUE(manager.initialized()); diff --git a/cmvr-es/manager/media_source_hub/CMakeLists.txt b/cmvr-es/manager/media_source_manager/CMakeLists.txt similarity index 70% rename from cmvr-es/manager/media_source_hub/CMakeLists.txt rename to cmvr-es/manager/media_source_manager/CMakeLists.txt index 49b6473e..b5369f7c 100644 --- a/cmvr-es/manager/media_source_hub/CMakeLists.txt +++ b/cmvr-es/manager/media_source_manager/CMakeLists.txt @@ -1,28 +1,28 @@ if(CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR) cmake_minimum_required(VERSION 3.22) - project(cmvr_media_source_hub LANGUAGES CXX) + project(cmvr_media_source_manager LANGUAGES CXX) enable_testing() add_subdirectory( - ${CMAKE_CURRENT_SOURCE_DIR}/../../service/stop_all + ${CMAKE_CURRENT_SOURCE_DIR}/../../service/grpc/stop_all ${CMAKE_CURRENT_BINARY_DIR}/stop_all ) endif() -add_library(media_source_hub STATIC - src/media_source_hub.cpp +add_library(media_source_manager STATIC + src/media_source_manager.cpp ) -target_compile_features(media_source_hub PUBLIC cxx_std_17) -target_include_directories(media_source_hub +target_compile_features(media_source_manager PUBLIC cxx_std_17) +target_include_directories(media_source_manager PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../.. ) -target_link_libraries(media_source_hub +target_link_libraries(media_source_manager PUBLIC cmvr_es::stop_all_admission_gate ) -add_library(cmvr_es::media_source_hub ALIAS media_source_hub) +add_library(cmvr_es::media_source_manager ALIAS media_source_manager) if(NOT CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR) add_library(device_media_source_adapter STATIC @@ -35,11 +35,11 @@ if(NOT CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR) ) target_link_libraries(device_media_source_adapter PUBLIC - cmvr_es::media_source_hub + cmvr_es::media_source_manager cmvr_es::common cmvr_es::proto cmvr_es::logging - cmvr_es::safety_coordinator + cmvr_es::safety_manager ) add_library(cmvr_es::device_media_source_adapter ALIAS device_media_source_adapter) @@ -71,27 +71,27 @@ if(NOT CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR) endif() endif() -option(CMVR_MEDIA_SOURCE_HUB_BUILD_TESTS - "Build the standalone MediaSourceHub self-test" +option(CMVR_MEDIA_SOURCE_MANAGER_BUILD_TESTS + "Build the standalone MediaSourceManager self-test" ${PROJECT_IS_TOP_LEVEL}) -if(CMVR_MEDIA_SOURCE_HUB_BUILD_TESTS) +if(CMVR_MEDIA_SOURCE_MANAGER_BUILD_TESTS) find_package(Threads REQUIRED) - add_executable(media_source_hub_test - tests/media_source_hub_test.cpp + add_executable(media_source_manager_test + tests/media_source_manager_test.cpp ) - target_compile_features(media_source_hub_test PRIVATE cxx_std_17) - target_link_libraries(media_source_hub_test + target_compile_features(media_source_manager_test PRIVATE cxx_std_17) + target_link_libraries(media_source_manager_test PRIVATE - cmvr_es::media_source_hub + cmvr_es::media_source_manager Threads::Threads ) # This self-test only links the static Hub and pthreads. In the root build, # the project-wide third-party RUNPATH can otherwise make the loader pick up # a vendor libstdc++.so (for example from the AUBO SDK), even though the test # has no dependency on that SDK. - set_target_properties(media_source_hub_test PROPERTIES + set_target_properties(media_source_manager_test PROPERTIES SKIP_BUILD_RPATH TRUE ) - add_test(NAME media_source_hub_test COMMAND media_source_hub_test) + add_test(NAME media_source_manager_test COMMAND media_source_manager_test) endif() diff --git a/cmvr-es/manager/media_source_hub/include/device_media_source_adapter.h b/cmvr-es/manager/media_source_manager/include/device_media_source_adapter.h similarity index 84% rename from cmvr-es/manager/media_source_hub/include/device_media_source_adapter.h rename to cmvr-es/manager/media_source_manager/include/device_media_source_adapter.h index b23b0535..9dcbcef5 100644 --- a/cmvr-es/manager/media_source_hub/include/device_media_source_adapter.h +++ b/cmvr-es/manager/media_source_manager/include/device_media_source_adapter.h @@ -9,13 +9,13 @@ #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" +#include "manager/media_source_manager/include/media_source_manager.h" +#include "manager/safety_manager/include/safety_manager.h" namespace cmvr::media { // Process-wide protocol-neutral media hub shared by gRPC and QUIC services. -MediaSourceHub& globalMediaSourceHub(); +MediaSourceManager& globalMediaSourceManager(); std::string cameraColorTrackId(const std::string& device_id); std::string microphoneTrackId(const std::string& device_id); @@ -24,7 +24,7 @@ std::string microphoneTrackId(const std::string& device_id); // 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, + safety::SafetyManager& coordinator, const std::string& device_id); // Registration is idempotent for an already registered track. The adapter owns a @@ -32,12 +32,12 @@ safety::DispatchGuard beginMediaSourceStartDispatch( // at least one Hub subscription is active. It ensures start() succeeds but deliberately // does not call stop(), because the base device lifecycle can also be owned by control RPCs. bool ensureCameraMediaSource( - MediaSourceHub& hub, + MediaSourceManager& hub, const std::shared_ptr& camera, size_t ring_capacity = 64); bool ensureMicrophoneMediaSource( - MediaSourceHub& hub, + MediaSourceManager& hub, const std::shared_ptr& microphone, size_t ring_capacity = 256); diff --git a/cmvr-es/manager/media_source_hub/include/media_source_hub.h b/cmvr-es/manager/media_source_manager/include/media_source_manager.h similarity index 90% rename from cmvr-es/manager/media_source_hub/include/media_source_hub.h rename to cmvr-es/manager/media_source_manager/include/media_source_manager.h index c09bf4d0..6efa6422 100644 --- a/cmvr-es/manager/media_source_hub/include/media_source_hub.h +++ b/cmvr-es/manager/media_source_manager/include/media_source_manager.h @@ -1,5 +1,5 @@ -#ifndef CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H -#define CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H +#ifndef CMVR_ES_MANAGER_MEDIA_SOURCE_MANAGER_H +#define CMVR_ES_MANAGER_MEDIA_SOURCE_MANAGER_H #pragma once @@ -21,15 +21,15 @@ class StopAllAdmissionGate; namespace cmvr::media { -// MediaSourceHub owns no protocol-specific state. A device or capture adapter registers +// MediaSourceManager owns no protocol-specific state. A device or capture adapter registers // start/stop callbacks and receives a sink callback when the first consumer subscribes. -class MediaSourceHub final { +class MediaSourceManager final { public: using FrameRing = BroadcastFrameRing; using FrameReadResult = FrameRing::ReadResult; using StartPosition = FrameRing::StartPosition; using FrameSink = std::function; - // Cancellation checks run while MediaSourceHub protects source lifecycle + // Cancellation checks run while MediaSourceManager protects source lifecycle // state. Predicates must therefore be fast, non-blocking and must not call // back into the same hub. using CancelPredicate = std::function; @@ -37,7 +37,7 @@ public: struct SourceCallbacks { // start() may run asynchronously. It must observe cancelled during any // potentially blocking startup work and return false promptly once set. - // MediaSourceHub retains the callback state until a non-cooperative start + // MediaSourceManager retains the callback state until a non-cooperative start // eventually returns, so late completion cannot access destroyed state. std::function source, FrameRing::Cursor cursor); std::shared_ptr source_; @@ -94,12 +94,12 @@ public: // Pass the process-wide StopAll gate for a hub whose sources are part of // whole-machine operational stopping. Test/private hubs may remain local. - explicit MediaSourceHub( + explicit MediaSourceManager( service::StopAllAdmissionGate* admission_gate = nullptr); - ~MediaSourceHub(); + ~MediaSourceManager(); - MediaSourceHub(const MediaSourceHub&) = delete; - MediaSourceHub& operator=(const MediaSourceHub&) = delete; + MediaSourceManager(const MediaSourceManager&) = delete; + MediaSourceManager& operator=(const MediaSourceManager&) = delete; bool registerSource( TrackDescriptorPtr initial_descriptor, @@ -159,4 +159,4 @@ private: } // namespace cmvr::media -#endif // CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H +#endif // CMVR_ES_MANAGER_MEDIA_SOURCE_MANAGER_H diff --git a/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp b/cmvr-es/manager/media_source_manager/src/device_media_source_adapter.cpp similarity index 96% rename from cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp rename to cmvr-es/manager/media_source_manager/src/device_media_source_adapter.cpp index 70cbf688..b8911a60 100644 --- a/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp +++ b/cmvr-es/manager/media_source_manager/src/device_media_source_adapter.cpp @@ -1,6 +1,6 @@ -#include "manager/media_source_hub/include/device_media_source_adapter.h" +#include "manager/media_source_manager/include/device_media_source_adapter.h" -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include #include @@ -131,8 +131,8 @@ struct PumpState : public std::enable_shared_from_this> { } bool begin( - const MediaSourceHub::FrameSink& frame_sink, - const MediaSourceHub::CancelPredicate& cancelled) { + const MediaSourceManager::FrameSink& frame_sink, + const MediaSourceManager::CancelPredicate& cancelled) { if (!frame_sink || !device) { return false; } @@ -288,7 +288,7 @@ struct PumpState : public std::enable_shared_from_this> { std::atomic running{false}; std::mutex mutex; std::thread worker; - MediaSourceHub::FrameSink sink; + MediaSourceManager::FrameSink sink; bool streaming_started{false}; }; @@ -448,7 +448,7 @@ struct CameraPump final : PumpState { frame.key_frame = source.bKey; frame.discontinuity = pending_discontinuity; - MediaSourceHub::FrameSink current_sink; + MediaSourceManager::FrameSink current_sink; { std::lock_guard lock(mutex); current_sink = sink; @@ -581,7 +581,7 @@ struct MicrophonePump final : PumpState { frame.key_frame = true; frame.discontinuity = pending_discontinuity; - MediaSourceHub::FrameSink current_sink; + MediaSourceManager::FrameSink current_sink; { std::lock_guard lock(mutex); current_sink = sink; @@ -619,8 +619,8 @@ TrackDescriptorPtr initialTrack( } // namespace -MediaSourceHub& globalMediaSourceHub() { - static MediaSourceHub hub(&service::globalStopAllAdmissionGate()); +MediaSourceManager& globalMediaSourceManager() { + static MediaSourceManager hub(&service::globalStopAllAdmissionGate()); return hub; } @@ -633,12 +633,12 @@ std::string microphoneTrackId(const std::string& device_id) { } safety::DispatchGuard beginMediaSourceStartDispatch( - safety::SafetyCoordinator& coordinator, + safety::SafetyManager& coordinator, const std::string& device_id) { safety::AdmissionRequest request; request.command = { - "cmvr.internal.MediaSourceHub/StartSource", + "cmvr.internal.MediaSourceManager/StartSource", safety::CommandIntent::StartActivity, safety::SafetyPolicyFamily::Sensor, true, @@ -670,7 +670,7 @@ safety::DispatchGuard beginMediaSourceStartDispatch( } bool ensureCameraMediaSource( - MediaSourceHub& hub, + MediaSourceManager& hub, const std::shared_ptr& camera, const size_t ring_capacity) { if (!camera || camera->id().empty()) { @@ -682,10 +682,10 @@ bool ensureCameraMediaSource( } const auto pump = std::make_shared(camera, track_id); - MediaSourceHub::SourceCallbacks callbacks; + MediaSourceManager::SourceCallbacks callbacks; callbacks.start = [pump]( - const MediaSourceHub::FrameSink& sink, - const MediaSourceHub::CancelPredicate& cancelled) { + const MediaSourceManager::FrameSink& sink, + const MediaSourceManager::CancelPredicate& cancelled) { return pump->begin(sink, cancelled); }; callbacks.stop_confirmed = [pump] { return pump->stop(); }; @@ -702,7 +702,7 @@ bool ensureCameraMediaSource( } bool ensureMicrophoneMediaSource( - MediaSourceHub& hub, + MediaSourceManager& hub, const std::shared_ptr& microphone, const size_t ring_capacity) { if (!microphone || microphone->id().empty()) { @@ -714,10 +714,10 @@ bool ensureMicrophoneMediaSource( } const auto pump = std::make_shared(microphone, track_id); - MediaSourceHub::SourceCallbacks callbacks; + MediaSourceManager::SourceCallbacks callbacks; callbacks.start = [pump]( - const MediaSourceHub::FrameSink& sink, - const MediaSourceHub::CancelPredicate& cancelled) { + const MediaSourceManager::FrameSink& sink, + const MediaSourceManager::CancelPredicate& cancelled) { return pump->begin(sink, cancelled); }; callbacks.stop_confirmed = [pump] { return pump->stop(); }; diff --git a/cmvr-es/manager/media_source_hub/src/media_source_hub.cpp b/cmvr-es/manager/media_source_manager/src/media_source_manager.cpp similarity index 92% rename from cmvr-es/manager/media_source_hub/src/media_source_hub.cpp rename to cmvr-es/manager/media_source_manager/src/media_source_manager.cpp index 5942024d..1a69522d 100644 --- a/cmvr-es/manager/media_source_hub/src/media_source_hub.cpp +++ b/cmvr-es/manager/media_source_manager/src/media_source_manager.cpp @@ -1,4 +1,4 @@ -#include "manager/media_source_hub/include/media_source_hub.h" +#include "manager/media_source_manager/include/media_source_manager.h" #include #include @@ -9,11 +9,11 @@ #include #include -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" namespace cmvr::media { -struct MediaSourceHub::SourceState final : public std::enable_shared_from_this { +struct MediaSourceManager::SourceState final : public std::enable_shared_from_this { enum class Lifecycle { STOPPED, STARTING, @@ -406,7 +406,7 @@ struct MediaSourceHub::SourceState final : public std::enable_shared_from_this start_attempt; }; -struct MediaSourceHub::Impl final { +struct MediaSourceManager::Impl final { explicit Impl(service::StopAllAdmissionGate* source_admission_gate) : admission_gate(source_admission_gate) {} @@ -420,25 +420,25 @@ struct MediaSourceHub::Impl final { service::StopAllAdmissionGate* const admission_gate; }; -MediaSourceHub::Subscription::Subscription( +MediaSourceManager::Subscription::Subscription( std::shared_ptr source, FrameRing::Cursor cursor) : source_(std::move(source)), cursor_(std::move(cursor)), active_(static_cast(source_)) {} -MediaSourceHub::Subscription::~Subscription() { +MediaSourceManager::Subscription::~Subscription() { reset(); } -MediaSourceHub::Subscription::Subscription(Subscription&& other) noexcept +MediaSourceManager::Subscription::Subscription(Subscription&& other) noexcept : source_(std::move(other.source_)), cursor_(other.cursor_), active_(other.active_) { other.active_ = false; } -MediaSourceHub::Subscription& MediaSourceHub::Subscription::operator=(Subscription&& other) noexcept { +MediaSourceManager::Subscription& MediaSourceManager::Subscription::operator=(Subscription&& other) noexcept { if (this == &other) { return *this; } @@ -450,22 +450,22 @@ MediaSourceHub::Subscription& MediaSourceHub::Subscription::operator=(Subscripti return *this; } -bool MediaSourceHub::Subscription::valid() const { +bool MediaSourceManager::Subscription::valid() const { return active_ && source_ && source_->validForSubscription(); } -TrackDescriptorPtr MediaSourceHub::Subscription::descriptor() const { +TrackDescriptorPtr MediaSourceManager::Subscription::descriptor() const { return source_ ? source_->currentDescriptor() : nullptr; } -std::optional MediaSourceHub::Subscription::tryRead() { +std::optional MediaSourceManager::Subscription::tryRead() { if (!active_ || !source_) { return std::nullopt; } return source_->ring.tryRead(cursor_); } -std::optional MediaSourceHub::Subscription::waitRead( +std::optional MediaSourceManager::Subscription::waitRead( const std::chrono::milliseconds timeout) { if (!active_ || !source_) { return std::nullopt; @@ -473,7 +473,7 @@ std::optional MediaSourceHub::Subscription::wai return source_->ring.waitRead(cursor_, timeout); } -uint64_t MediaSourceHub::Subscription::discardPendingIfExceeds( +uint64_t MediaSourceManager::Subscription::discardPendingIfExceeds( const size_t maximum_pending_frames) { if (!active_ || !source_) { return 0; @@ -481,11 +481,11 @@ uint64_t MediaSourceHub::Subscription::discardPendingIfExceeds( return source_->ring.discardPendingIfExceeds(cursor_, maximum_pending_frames); } -uint64_t MediaSourceHub::Subscription::droppedCount() const noexcept { +uint64_t MediaSourceManager::Subscription::droppedCount() const noexcept { return cursor_.dropped_count; } -void MediaSourceHub::Subscription::reset() { +void MediaSourceManager::Subscription::reset() { if (active_ && source_) { source_->release(); } @@ -493,15 +493,15 @@ void MediaSourceHub::Subscription::reset() { source_.reset(); } -MediaSourceHub::MediaSourceHub( +MediaSourceManager::MediaSourceManager( service::StopAllAdmissionGate* admission_gate) : impl_(std::make_shared(admission_gate)) {} -MediaSourceHub::~MediaSourceHub() { +MediaSourceManager::~MediaSourceManager() { shutdown(); } -bool MediaSourceHub::registerSource( +bool MediaSourceManager::registerSource( TrackDescriptorPtr initial_descriptor, SourceCallbacks callbacks, const size_t ring_capacity) { @@ -556,7 +556,7 @@ bool MediaSourceHub::registerSource( return inserted; } -bool MediaSourceHub::unregisterSource(const std::string& track_id) { +bool MediaSourceManager::unregisterSource(const std::string& track_id) { if (!impl_ || track_id.empty()) { return false; } @@ -590,7 +590,7 @@ bool MediaSourceHub::unregisterSource(const std::string& track_id) { return false; } -bool MediaSourceHub::hasSource(const std::string& track_id) const { +bool MediaSourceManager::hasSource(const std::string& track_id) const { if (!impl_) { return false; } @@ -598,7 +598,7 @@ bool MediaSourceHub::hasSource(const std::string& track_id) const { return impl_->sources.find(track_id) != impl_->sources.end(); } -std::vector MediaSourceHub::listTracks() const { +std::vector MediaSourceManager::listTracks() const { std::vector> sources; if (!impl_) { return {}; @@ -625,7 +625,7 @@ std::vector MediaSourceHub::listTracks() const { return descriptors; } -std::vector MediaSourceHub::trackedSourceIds() const { +std::vector MediaSourceManager::trackedSourceIds() const { if (!impl_) { return {}; } @@ -644,7 +644,7 @@ std::vector MediaSourceHub::trackedSourceIds() const { return source_ids; } -size_t MediaSourceHub::subscriberCount(const std::string& track_id) const { +size_t MediaSourceManager::subscriberCount(const std::string& track_id) const { if (!impl_) { return 0; } @@ -660,7 +660,7 @@ size_t MediaSourceHub::subscriberCount(const std::string& track_id) const { return source->subscriberCount(); } -bool MediaSourceHub::requestKeyFrame(const std::string& track_id) const { +bool MediaSourceManager::requestKeyFrame(const std::string& track_id) const { if (!impl_) { return false; } @@ -676,7 +676,7 @@ bool MediaSourceHub::requestKeyFrame(const std::string& track_id) const { return source->requestKeyFrame(); } -MediaSourceHub::Subscription MediaSourceHub::subscribe( +MediaSourceManager::Subscription MediaSourceManager::subscribe( const std::string& track_id, const StartPosition start_position, CancelPredicate cancelled) { @@ -701,7 +701,7 @@ MediaSourceHub::Subscription MediaSourceHub::subscribe( return Subscription(std::move(source), std::move(cursor)); } -bool MediaSourceHub::stopSourcesForDevice( +bool MediaSourceManager::stopSourcesForDevice( const std::string& source_id, std::vector* failures) { if (source_id.empty()) { @@ -713,11 +713,11 @@ bool MediaSourceHub::stopSourcesForDevice( return stopSources(source_id, failures); } -bool MediaSourceHub::stopAllSources(std::vector* failures) { +bool MediaSourceManager::stopAllSources(std::vector* failures) { return stopSources(std::nullopt, failures); } -bool MediaSourceHub::stopSources( +bool MediaSourceManager::stopSources( const std::optional& source_id, std::vector* failures) { if (failures) { @@ -796,7 +796,7 @@ bool MediaSourceHub::stopSources( return quarantined.empty(); } -void MediaSourceHub::shutdown() { +void MediaSourceManager::shutdown() { (void)stopAllSources(); } diff --git a/cmvr-es/manager/media_source_hub/tests/device_media_source_adapter_test.cpp b/cmvr-es/manager/media_source_manager/tests/device_media_source_adapter_test.cpp similarity index 93% rename from cmvr-es/manager/media_source_hub/tests/device_media_source_adapter_test.cpp rename to cmvr-es/manager/media_source_manager/tests/device_media_source_adapter_test.cpp index 8516204c..0c007166 100644 --- a/cmvr-es/manager/media_source_hub/tests/device_media_source_adapter_test.cpp +++ b/cmvr-es/manager/media_source_manager/tests/device_media_source_adapter_test.cpp @@ -1,4 +1,4 @@ -#include "manager/media_source_hub/include/device_media_source_adapter.h" +#include "manager/media_source_manager/include/device_media_source_adapter.h" #include #include @@ -7,7 +7,7 @@ #include -#include "manager/safety/include/device_safety_endpoint.h" +#include "manager/safety_manager/include/device_safety_endpoint.h" namespace cmvr::media { namespace { @@ -81,9 +81,9 @@ private: TEST(DeviceMediaSourceAdapterTest, SensorStartUsesFinalCheckAndQuarantineRejectsRestart) { - safety::SafetyCoordinatorConfig config; + safety::SafetyManagerConfig config; config.enforcement_mode = safety::EnforcementMode::EnforceAll; - safety::SafetyCoordinator coordinator(config); + safety::SafetyManager coordinator(config); auto endpoint = std::make_shared("camera"); ASSERT_TRUE(coordinator.registerDevice( {endpoint->descriptor(), endpoint, {}})); diff --git a/cmvr-es/manager/media_source_hub/tests/media_source_hub_test.cpp b/cmvr-es/manager/media_source_manager/tests/media_source_manager_test.cpp similarity index 89% rename from cmvr-es/manager/media_source_hub/tests/media_source_hub_test.cpp rename to cmvr-es/manager/media_source_manager/tests/media_source_manager_test.cpp index 59afca7d..a7fd3907 100644 --- a/cmvr-es/manager/media_source_hub/tests/media_source_hub_test.cpp +++ b/cmvr-es/manager/media_source_manager/tests/media_source_manager_test.cpp @@ -1,5 +1,5 @@ -#include "manager/media_source_hub/include/media_source_hub.h" -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "manager/media_source_manager/include/media_source_manager.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include #include @@ -20,7 +20,7 @@ using cmvr::media::Codec; using cmvr::media::MediaFrame; using cmvr::media::MediaFramePtr; using cmvr::media::MediaKind; -using cmvr::media::MediaSourceHub; +using cmvr::media::MediaSourceManager; using cmvr::media::PayloadFormat; using cmvr::media::Rational; using cmvr::media::TrackDescriptor; @@ -349,17 +349,17 @@ void testBroadcastConcurrency() { } void testHubLifecycleAndDescriptorRefresh() { - MediaSourceHub hub; + MediaSourceManager hub; const auto initial_descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1); std::atomic start_count{0}; std::atomic stop_count{0}; std::atomic key_frame_requests{0}; std::mutex sink_mutex; - MediaSourceHub::FrameSink sink; + MediaSourceManager::FrameSink sink; - MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [&](const MediaSourceHub::FrameSink& callback_sink, - const MediaSourceHub::CancelPredicate&) { + MediaSourceManager::SourceCallbacks callbacks; + callbacks.start = [&](const MediaSourceManager::FrameSink& callback_sink, + const MediaSourceManager::CancelPredicate&) { { std::lock_guard lock(sink_mutex); sink = callback_sink; @@ -394,7 +394,7 @@ void testHubLifecycleAndDescriptorRefresh() { // Content changes at the same generation must atomically replace the initial descriptor. const auto actual_descriptor = makeVideoDescriptor(Codec::H264, 1, {0, 0, 0, 1, 0x67}); - MediaSourceHub::FrameSink producer; + MediaSourceManager::FrameSink producer; { std::lock_guard lock(sink_mutex); producer = sink; @@ -429,14 +429,14 @@ void testHubLifecycleAndDescriptorRefresh() { } void testSubscriptionDiscardPending() { - MediaSourceHub hub; + MediaSourceManager hub; const auto descriptor = makeVideoDescriptor(Codec::H264, 1); std::mutex sink_mutex; - MediaSourceHub::FrameSink sink; + MediaSourceManager::FrameSink sink; - MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [&](const MediaSourceHub::FrameSink& callback_sink, - const MediaSourceHub::CancelPredicate&) { + MediaSourceManager::SourceCallbacks callbacks; + callbacks.start = [&](const MediaSourceManager::FrameSink& callback_sink, + const MediaSourceManager::CancelPredicate&) { std::lock_guard lock(sink_mutex); sink = callback_sink; return true; @@ -450,7 +450,7 @@ void testSubscriptionDiscardPending() { auto subscription = hub.subscribe(descriptor->id); CHECK_TRUE(subscription.valid()); - MediaSourceHub::FrameSink producer; + MediaSourceManager::FrameSink producer; { std::lock_guard lock(sink_mutex); producer = sink; @@ -471,13 +471,13 @@ void testSubscriptionDiscardPending() { } void testHubFailedStartAndShutdown() { - MediaSourceHub hub; + MediaSourceManager hub; const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1); std::atomic start_attempts{0}; std::atomic retry_stop_count{0}; - MediaSourceHub::SourceCallbacks failed_callbacks; - failed_callbacks.start = [&](const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { + MediaSourceManager::SourceCallbacks failed_callbacks; + failed_callbacks.start = [&](const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { return ++start_attempts >= 2; }; failed_callbacks.stop = [&] { ++retry_stop_count; }; @@ -492,9 +492,9 @@ void testHubFailedStartAndShutdown() { CHECK_TRUE(hub.unregisterSource(descriptor->id)); std::atomic stop_count{0}; - MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [](const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { + MediaSourceManager::SourceCallbacks callbacks; + callbacks.start = [](const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { return true; }; callbacks.stop = [&] { ++stop_count; }; @@ -508,13 +508,13 @@ void testHubFailedStartAndShutdown() { } void testHubStopAllSourcesAllowsReregistration() { - MediaSourceHub hub; + MediaSourceManager hub; const auto first_descriptor = makeVideoDescriptor(Codec::H264, 1); std::atomic first_stop_count{0}; - MediaSourceHub::SourceCallbacks first_callbacks; - first_callbacks.start = [](const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { + MediaSourceManager::SourceCallbacks first_callbacks; + first_callbacks.start = [](const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { return true; }; first_callbacks.stop = [&] { ++first_stop_count; }; @@ -538,9 +538,9 @@ void testHubStopAllSourcesAllowsReregistration() { const auto second_descriptor = makeVideoDescriptor(Codec::H264, 2); std::atomic second_start_count{0}; std::atomic second_stop_count{0}; - MediaSourceHub::SourceCallbacks second_callbacks; - second_callbacks.start = [&](const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { + MediaSourceManager::SourceCallbacks second_callbacks; + second_callbacks.start = [&](const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { ++second_start_count; return true; }; @@ -559,13 +559,13 @@ void testHubStopAllSourcesAllowsReregistration() { } void testStopAllSourcesReportsAndRetriesUnconfirmedStop() { - MediaSourceHub hub; + MediaSourceManager hub; const auto descriptor = makeVideoDescriptor(Codec::H264, 1); std::atomic stop_attempts{0}; - MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [](const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { + MediaSourceManager::SourceCallbacks callbacks; + callbacks.start = [](const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { return true; }; callbacks.stop_confirmed = [&] { @@ -590,7 +590,7 @@ void testStopAllSourcesReportsAndRetriesUnconfirmedStop() { } void testStopSourcesForDeviceIsSelectiveAndRetriesFailures() { - MediaSourceHub hub; + MediaSourceManager hub; const auto front_video = makeVideoDescriptor( Codec::H264, 1, {}, "front.video", "camera.front"); const auto front_depth = makeVideoDescriptor( @@ -604,10 +604,10 @@ void testStopSourcesForDeviceIsSelectiveAndRetriesFailures() { auto register_source = [&]( const TrackDescriptorPtr& descriptor, std::function stop_confirmed) { - MediaSourceHub::SourceCallbacks callbacks; + MediaSourceManager::SourceCallbacks callbacks; callbacks.start = []( - const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { return true; }; + const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { return true; }; callbacks.stop_confirmed = std::move(stop_confirmed); return hub.registerSource(descriptor, std::move(callbacks), 2); }; @@ -665,7 +665,7 @@ void testStopSourcesForDeviceIsSelectiveAndRetriesFailures() { } void testDeviceStopsRunConcurrentlyAndSerializeMatchingRegistration() { - MediaSourceHub hub; + MediaSourceManager hub; const auto first = makeVideoDescriptor( Codec::H264, 1, {}, "first.video", "camera.first"); const auto second = makeVideoDescriptor( @@ -677,10 +677,10 @@ void testDeviceStopsRunConcurrentlyAndSerializeMatchingRegistration() { auto register_blocking_source = [&]( const TrackDescriptorPtr& descriptor, std::atomic& entered) { - MediaSourceHub::SourceCallbacks callbacks; + MediaSourceManager::SourceCallbacks callbacks; callbacks.start = []( - const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { return true; }; + const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { return true; }; callbacks.stop = [&entered, &release_stops] { entered.store(true, std::memory_order_release); while (!release_stops.load(std::memory_order_acquire)) { @@ -720,10 +720,10 @@ void testDeviceStopsRunConcurrentlyAndSerializeMatchingRegistration() { } CHECK_TRUE(second_stop_entered.load(std::memory_order_acquire)); - MediaSourceHub::SourceCallbacks replacement_callbacks; + MediaSourceManager::SourceCallbacks replacement_callbacks; replacement_callbacks.start = []( - const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { return true; }; + const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { return true; }; replacement_callbacks.stop = [] {}; auto matching_registration = std::async(std::launch::async, [&] { return hub.registerSource( @@ -760,7 +760,7 @@ void testDeviceStopsRunConcurrentlyAndSerializeMatchingRegistration() { } void testStopAllWaitsForDeviceStopAndRetainsItsConcurrentRegistrationRule() { - MediaSourceHub hub; + MediaSourceManager hub; const auto first = makeVideoDescriptor( Codec::H264, 1, {}, "first.video", "camera.first"); const auto other = makeVideoDescriptor( @@ -769,10 +769,10 @@ void testStopAllWaitsForDeviceStopAndRetainsItsConcurrentRegistrationRule() { std::atomic release_first_stop{false}; std::atomic other_stops{0}; - MediaSourceHub::SourceCallbacks first_callbacks; + MediaSourceManager::SourceCallbacks first_callbacks; first_callbacks.start = []( - const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { return true; }; + const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { return true; }; first_callbacks.stop = [&] { first_stop_entered.store(true, std::memory_order_release); while (!release_first_stop.load(std::memory_order_acquire)) { @@ -798,10 +798,10 @@ void testStopAllWaitsForDeviceStopAndRetainsItsConcurrentRegistrationRule() { }); CHECK_TRUE(stop_all.wait_for(20ms) == std::future_status::timeout); - MediaSourceHub::SourceCallbacks other_callbacks; + MediaSourceManager::SourceCallbacks other_callbacks; other_callbacks.start = []( - const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { return true; }; + const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { return true; }; other_callbacks.stop = [&] { ++other_stops; }; CHECK_TRUE(hub.registerSource(other, std::move(other_callbacks), 2)); auto other_subscription = hub.subscribe(other->id); @@ -824,15 +824,15 @@ void testStopAllWaitsForDeviceStopAndRetainsItsConcurrentRegistrationRule() { } void testConcurrentRegistrationWaitsForStopAllSources() { - MediaSourceHub hub; + MediaSourceManager hub; const auto descriptor = makeVideoDescriptor(Codec::H264, 1); std::atomic stop_entered{false}; std::atomic release_stop{false}; std::atomic old_stop_count{0}; - MediaSourceHub::SourceCallbacks old_callbacks; - old_callbacks.start = [](const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { + MediaSourceManager::SourceCallbacks old_callbacks; + old_callbacks.start = [](const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { return true; }; old_callbacks.stop = [&] { @@ -856,9 +856,9 @@ void testConcurrentRegistrationWaitsForStopAllSources() { CHECK_TRUE(stop_entered.load(std::memory_order_acquire)); std::atomic new_start_count{0}; - MediaSourceHub::SourceCallbacks new_callbacks; - new_callbacks.start = [&](const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { + MediaSourceManager::SourceCallbacks new_callbacks; + new_callbacks.start = [&](const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { ++new_start_count; return true; }; @@ -889,7 +889,7 @@ void testConcurrentRegistrationWaitsForStopAllSources() { void testSystemStopAllAdmissionFencesRegistrationAndStartup() { cmvr::service::StopAllAdmissionGate admission_gate; - MediaSourceHub hub(&admission_gate); + MediaSourceManager hub(&admission_gate); const auto dormant = makeVideoDescriptor( Codec::H264, 1, {}, "dormant.video", "camera.dormant"); const auto new_source = makeVideoDescriptor( @@ -897,10 +897,10 @@ void testSystemStopAllAdmissionFencesRegistrationAndStartup() { std::atomic dormant_starts{0}; std::atomic new_starts{0}; - MediaSourceHub::SourceCallbacks dormant_callbacks; + MediaSourceManager::SourceCallbacks dormant_callbacks; dormant_callbacks.start = [&]( - const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { + const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { ++dormant_starts; return true; }; @@ -911,10 +911,10 @@ void testSystemStopAllAdmissionFencesRegistrationAndStartup() { const auto stop_ticket = admission_gate.beginStopAll(); CHECK_TRUE(stop_ticket.valid()); - MediaSourceHub::SourceCallbacks rejected_callbacks; + MediaSourceManager::SourceCallbacks rejected_callbacks; rejected_callbacks.start = [&]( - const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { + const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { ++new_starts; return true; }; @@ -928,10 +928,10 @@ void testSystemStopAllAdmissionFencesRegistrationAndStartup() { CHECK_TRUE(admission_gate.finishStopAll(stop_ticket, true)); - MediaSourceHub::SourceCallbacks recovered_callbacks; + MediaSourceManager::SourceCallbacks recovered_callbacks; recovered_callbacks.start = [&]( - const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { + const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { ++new_starts; return true; }; @@ -949,7 +949,7 @@ void testSystemStopAllAdmissionFencesRegistrationAndStartup() { void testSystemStopAllRejectsRegistrationWaitingForLocalStop() { cmvr::service::StopAllAdmissionGate admission_gate; - MediaSourceHub hub(&admission_gate); + MediaSourceManager hub(&admission_gate); const auto old_source = makeVideoDescriptor( Codec::H264, 1, {}, "old.video", "camera.shared"); const auto replacement = makeVideoDescriptor( @@ -957,10 +957,10 @@ void testSystemStopAllRejectsRegistrationWaitingForLocalStop() { std::atomic stop_entered{false}; std::atomic release_stop{false}; - MediaSourceHub::SourceCallbacks old_callbacks; + MediaSourceManager::SourceCallbacks old_callbacks; old_callbacks.start = []( - const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { return true; }; + const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { return true; }; old_callbacks.stop = [&] { stop_entered.store(true, std::memory_order_release); while (!release_stop.load(std::memory_order_acquire)) { @@ -983,10 +983,10 @@ void testSystemStopAllRejectsRegistrationWaitingForLocalStop() { CHECK_TRUE(stop_entered.load(std::memory_order_acquire)); auto make_replacement_callbacks = [] { - MediaSourceHub::SourceCallbacks callbacks; + MediaSourceManager::SourceCallbacks callbacks; callbacks.start = []( - const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { return true; }; + const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { return true; }; callbacks.stop = [] {}; return callbacks; }; @@ -1021,17 +1021,17 @@ void testSystemStopAllRejectsRegistrationWaitingForLocalStop() { void testSystemStopAllRejectsSubscriptionWaitingForLocalStop() { cmvr::service::StopAllAdmissionGate admission_gate; - MediaSourceHub hub(&admission_gate); + MediaSourceManager hub(&admission_gate); const auto descriptor = makeVideoDescriptor( Codec::H264, 1, {}, "waiting.video", "camera.waiting"); std::atomic stop_entered{false}; std::atomic release_stop{false}; std::atomic starts{0}; - MediaSourceHub::SourceCallbacks callbacks; + MediaSourceManager::SourceCallbacks callbacks; callbacks.start = [&]( - const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { + const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { ++starts; return true; }; @@ -1084,15 +1084,15 @@ void testSystemStopAllRejectsSubscriptionWaitingForLocalStop() { } void testStopAllSourcesCancelsStartingSourceBeforeReuse() { - MediaSourceHub hub; + MediaSourceManager hub; const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1); std::atomic old_start_entered{false}; std::atomic release_old_start{false}; std::atomic old_stop_count{0}; - MediaSourceHub::SourceCallbacks old_callbacks; - old_callbacks.start = [&](const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate& cancelled) { + MediaSourceManager::SourceCallbacks old_callbacks; + old_callbacks.start = [&](const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate& cancelled) { old_start_entered.store(true, std::memory_order_release); while (!release_old_start.load(std::memory_order_acquire)) { if (cancelled()) { @@ -1134,9 +1134,9 @@ void testStopAllSourcesCancelsStartingSourceBeforeReuse() { CHECK_TRUE(hub.stopAllSources()); std::atomic new_start_count{0}; - MediaSourceHub::SourceCallbacks new_callbacks; - new_callbacks.start = [&](const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { + MediaSourceManager::SourceCallbacks new_callbacks; + new_callbacks.start = [&](const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { ++new_start_count; return true; }; @@ -1150,15 +1150,15 @@ void testStopAllSourcesCancelsStartingSourceBeforeReuse() { } void testKeyFrameRequestIsOrderedBeforeStop() { - MediaSourceHub hub; + MediaSourceManager hub; const auto descriptor = makeVideoDescriptor(Codec::H264, 1); std::atomic key_frame_entered{false}; std::atomic release_key_frame{false}; std::atomic stop_count{0}; - MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [](const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { + MediaSourceManager::SourceCallbacks callbacks; + callbacks.start = [](const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { return true; }; callbacks.stop = [&] { ++stop_count; }; @@ -1196,14 +1196,14 @@ void testKeyFrameRequestIsOrderedBeforeStop() { } void testHubCancelsBlockedStartWithoutBlockingShutdown() { - MediaSourceHub hub; + MediaSourceManager hub; const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1); std::atomic start_entered{false}; std::atomic start_exited{false}; - MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [&](const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate& cancelled) { + MediaSourceManager::SourceCallbacks callbacks; + callbacks.start = [&](const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate& cancelled) { start_entered.store(true, std::memory_order_release); while (!cancelled()) { std::this_thread::sleep_for(2ms); @@ -1246,16 +1246,16 @@ void testHubCancelsBlockedStartWithoutBlockingShutdown() { } void testHubQuarantinesNonCooperativeStart() { - MediaSourceHub hub; + MediaSourceManager hub; const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1); std::atomic start_entered{false}; std::atomic release_start{false}; std::atomic start_exited{false}; std::atomic stop_count{0}; - MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [&](const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { + MediaSourceManager::SourceCallbacks callbacks; + callbacks.start = [&](const MediaSourceManager::FrameSink&, + const MediaSourceManager::CancelPredicate&) { start_entered.store(true, std::memory_order_release); while (!release_start.load(std::memory_order_acquire)) { std::this_thread::sleep_for(2ms); @@ -1330,9 +1330,9 @@ int main() { testHubQuarantinesNonCooperativeStart(); if (failures != 0) { - std::cerr << failures << " media_source_hub checks failed\n"; + std::cerr << failures << " media_source_manager checks failed\n"; return 1; } - std::cout << "media_source_hub self-test passed\n"; + std::cout << "media_source_manager self-test passed\n"; return 0; } diff --git a/cmvr-es/manager/safety/CMakeLists.txt b/cmvr-es/manager/safety_manager/CMakeLists.txt similarity index 58% rename from cmvr-es/manager/safety/CMakeLists.txt rename to cmvr-es/manager/safety_manager/CMakeLists.txt index e63a7d57..f647fb1e 100644 --- a/cmvr-es/manager/safety/CMakeLists.txt +++ b/cmvr-es/manager/safety_manager/CMakeLists.txt @@ -1,28 +1,28 @@ -add_library(safety_coordinator STATIC +add_library(safety_manager STATIC src/command_ledger.cpp - src/safety_coordinator.cpp + src/safety_manager.cpp src/safety_reason.cpp src/safety_snapshot_store.cpp ) -target_include_directories(safety_coordinator PUBLIC +target_include_directories(safety_manager PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_SOURCE_DIR}/cmvr-es ) -target_link_libraries(safety_coordinator PUBLIC - cmvr_es::control_authority +target_link_libraries(safety_manager PUBLIC + cmvr_es::control_authority_manager ) -add_library(cmvr_es::safety_coordinator ALIAS safety_coordinator) -install(TARGETS safety_coordinator LIBRARY DESTINATION lib) +add_library(cmvr_es::safety_manager ALIAS safety_manager) +install(TARGETS safety_manager 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 + cmvr_es::safety_manager gtest gtest_main pthread @@ -37,7 +37,7 @@ if(BUILD_TESTING) tests/command_ledger_test.cpp ) target_link_libraries(command_ledger_test PRIVATE - cmvr_es::safety_coordinator + cmvr_es::safety_manager gtest gtest_main pthread @@ -48,18 +48,18 @@ if(BUILD_TESTING) ) set_tests_properties(command_ledger_test PROPERTIES TIMEOUT 10) - add_executable(safety_coordinator_test - tests/safety_coordinator_test.cpp + add_executable(safety_manager_test + tests/safety_manager_test.cpp ) - target_link_libraries(safety_coordinator_test PRIVATE - cmvr_es::safety_coordinator + target_link_libraries(safety_manager_test PRIVATE + cmvr_es::safety_manager gtest gtest_main pthread ) add_test( - NAME safety_coordinator_test - COMMAND safety_coordinator_test + NAME safety_manager_test + COMMAND safety_manager_test ) - set_tests_properties(safety_coordinator_test PROPERTIES TIMEOUT 15) + set_tests_properties(safety_manager_test PROPERTIES TIMEOUT 15) endif() diff --git a/cmvr-es/manager/safety/include/command_ledger.h b/cmvr-es/manager/safety_manager/include/command_ledger.h similarity index 98% rename from cmvr-es/manager/safety/include/command_ledger.h rename to cmvr-es/manager/safety_manager/include/command_ledger.h index deeeb37b..f47a7214 100644 --- a/cmvr-es/manager/safety/include/command_ledger.h +++ b/cmvr-es/manager/safety_manager/include/command_ledger.h @@ -9,7 +9,7 @@ #include #include -#include "manager/safety/include/safety_types.h" +#include "manager/safety_manager/include/safety_types.h" namespace cmvr::safety { diff --git a/cmvr-es/manager/safety/include/device_safety_endpoint.h b/cmvr-es/manager/safety_manager/include/device_safety_endpoint.h similarity index 95% rename from cmvr-es/manager/safety/include/device_safety_endpoint.h rename to cmvr-es/manager/safety_manager/include/device_safety_endpoint.h index 9d743683..836ca6b8 100644 --- a/cmvr-es/manager/safety/include/device_safety_endpoint.h +++ b/cmvr-es/manager/safety_manager/include/device_safety_endpoint.h @@ -3,7 +3,7 @@ #include #include -#include "manager/safety/include/safety_types.h" +#include "manager/safety_manager/include/safety_types.h" namespace cmvr::safety { diff --git a/cmvr-es/manager/safety/include/safety_coordinator.h b/cmvr-es/manager/safety_manager/include/safety_manager.h similarity index 89% rename from cmvr-es/manager/safety/include/safety_coordinator.h rename to cmvr-es/manager/safety_manager/include/safety_manager.h index 55179b0d..1ea4c982 100644 --- a/cmvr-es/manager/safety/include/safety_coordinator.h +++ b/cmvr-es/manager/safety_manager/include/safety_manager.h @@ -10,14 +10,14 @@ #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" +#include "manager/safety_manager/include/command_ledger.h" +#include "manager/safety_manager/include/device_safety_endpoint.h" +#include "manager/safety_manager/include/safety_participant.h" +#include "manager/safety_manager/include/safety_snapshot_store.h" namespace cmvr::safety { -struct SafetyCoordinatorConfig { +struct SafetyManagerConfig { EnforcementMode enforcement_mode{EnforcementMode::Shadow}; std::unordered_set enforced_device_ids; std::chrono::milliseconds stop_all_timeout{15000}; @@ -72,7 +72,7 @@ struct ParticipantSafetyStateView { ParticipantResultView last_release; }; -struct SafetyCoordinatorSnapshot { +struct SafetyManagerSnapshot { SystemAdmissionState system_state{SystemAdmissionState::Starting}; std::uint64_t safety_epoch{0}; std::string service_instance_id; @@ -135,7 +135,7 @@ struct RecoveryResult { std::vector targets; }; -class SafetyCoordinator; +class SafetyManager; class DispatchGuard final { public: @@ -153,23 +153,23 @@ public: } private: - friend class SafetyCoordinator; - DispatchGuard(SafetyCoordinator* coordinator, + friend class SafetyManager; + DispatchGuard(SafetyManager* coordinator, std::string device_id, HardwareCheckResult hardware_check) noexcept; void reset_() noexcept; - SafetyCoordinator* coordinator_{nullptr}; + SafetyManager* coordinator_{nullptr}; std::string device_id_; HardwareCheckResult hardware_check_; }; -class SafetyCoordinator final { +class SafetyManager final { public: - explicit SafetyCoordinator(SafetyCoordinatorConfig config = {}); - ~SafetyCoordinator(); - SafetyCoordinator(const SafetyCoordinator&) = delete; - SafetyCoordinator& operator=(const SafetyCoordinator&) = delete; + explicit SafetyManager(SafetyManagerConfig config = {}); + ~SafetyManager(); + SafetyManager(const SafetyManager&) = delete; + SafetyManager& operator=(const SafetyManager&) = delete; bool registerDevice(DeviceSafetyRegistration registration); bool unregisterDevice(const std::string& device_id); @@ -205,13 +205,13 @@ public: SafetyClock::time_point deadline = SafetyClock::time_point::max()); RecoveryResult recover(const RecoveryRequest& request); - SafetyCoordinatorSnapshot snapshot() const; + SafetyManagerSnapshot 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; + const SafetyManagerConfig& config() const noexcept; private: friend class DispatchGuard; diff --git a/cmvr-es/manager/safety/include/safety_participant.h b/cmvr-es/manager/safety_manager/include/safety_participant.h similarity index 97% rename from cmvr-es/manager/safety/include/safety_participant.h rename to cmvr-es/manager/safety_manager/include/safety_participant.h index 69072164..1a4557ce 100644 --- a/cmvr-es/manager/safety/include/safety_participant.h +++ b/cmvr-es/manager/safety_manager/include/safety_participant.h @@ -5,7 +5,7 @@ #include #include -#include "manager/safety/include/safety_types.h" +#include "manager/safety_manager/include/safety_types.h" namespace cmvr::safety { diff --git a/cmvr-es/manager/safety/include/safety_reason.h b/cmvr-es/manager/safety_manager/include/safety_reason.h similarity index 100% rename from cmvr-es/manager/safety/include/safety_reason.h rename to cmvr-es/manager/safety_manager/include/safety_reason.h diff --git a/cmvr-es/manager/safety/include/safety_snapshot_store.h b/cmvr-es/manager/safety_manager/include/safety_snapshot_store.h similarity index 96% rename from cmvr-es/manager/safety/include/safety_snapshot_store.h rename to cmvr-es/manager/safety_manager/include/safety_snapshot_store.h index b2871eaa..b52c1c4a 100644 --- a/cmvr-es/manager/safety/include/safety_snapshot_store.h +++ b/cmvr-es/manager/safety_manager/include/safety_snapshot_store.h @@ -7,7 +7,7 @@ #include #include -#include "manager/safety/include/safety_types.h" +#include "manager/safety_manager/include/safety_types.h" namespace cmvr::safety { diff --git a/cmvr-es/manager/safety/include/safety_types.h b/cmvr-es/manager/safety_manager/include/safety_types.h similarity index 99% rename from cmvr-es/manager/safety/include/safety_types.h rename to cmvr-es/manager/safety_manager/include/safety_types.h index 66144b62..743629bd 100644 --- a/cmvr-es/manager/safety/include/safety_types.h +++ b/cmvr-es/manager/safety_manager/include/safety_types.h @@ -7,7 +7,7 @@ #include #include "devices/device_types.h" -#include "manager/safety/include/safety_reason.h" +#include "manager/safety_manager/include/safety_reason.h" namespace cmvr::safety { diff --git a/cmvr-es/manager/safety/src/command_ledger.cpp b/cmvr-es/manager/safety_manager/src/command_ledger.cpp similarity index 99% rename from cmvr-es/manager/safety/src/command_ledger.cpp rename to cmvr-es/manager/safety_manager/src/command_ledger.cpp index e4c6e790..95a28681 100644 --- a/cmvr-es/manager/safety/src/command_ledger.cpp +++ b/cmvr-es/manager/safety_manager/src/command_ledger.cpp @@ -1,4 +1,4 @@ -#include "manager/safety/include/command_ledger.h" +#include "manager/safety_manager/include/command_ledger.h" #include #include diff --git a/cmvr-es/manager/safety/src/safety_coordinator.cpp b/cmvr-es/manager/safety_manager/src/safety_manager.cpp similarity index 97% rename from cmvr-es/manager/safety/src/safety_coordinator.cpp rename to cmvr-es/manager/safety_manager/src/safety_manager.cpp index fb0c012e..bbb31eeb 100644 --- a/cmvr-es/manager/safety/src/safety_coordinator.cpp +++ b/cmvr-es/manager/safety_manager/src/safety_manager.cpp @@ -1,4 +1,4 @@ -#include "manager/safety/include/safety_coordinator.h" +#include "manager/safety_manager/include/safety_manager.h" #include #include @@ -187,10 +187,10 @@ int phaseRank(const ParticipantPhase phase) noexcept } // namespace -struct SafetyCoordinator::Impl { +struct SafetyManager::Impl { struct PublisherBinding { std::atomic active{true}; - SafetyCoordinator* coordinator{nullptr}; + SafetyManager* coordinator{nullptr}; }; struct DeviceSlot { @@ -240,7 +240,7 @@ struct SafetyCoordinator::Impl { RecoveryResult result; }; - explicit Impl(SafetyCoordinatorConfig source) + explicit Impl(SafetyManagerConfig source) : config(std::move(source)), ledger(config.command_ledger), service_instance_id(makeInstanceId()), @@ -249,7 +249,7 @@ struct SafetyCoordinator::Impl { 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"); + throw std::invalid_argument("invalid SafetyManagerConfig"); } } @@ -339,7 +339,7 @@ struct SafetyCoordinator::Impl { return false; } - SafetyCoordinatorConfig config; + SafetyManagerConfig config; SafetySnapshotStore snapshots; CommandLedger ledger; const std::string service_instance_id; @@ -367,7 +367,7 @@ struct SafetyCoordinator::Impl { }; DispatchGuard::DispatchGuard( - SafetyCoordinator* coordinator, + SafetyManager* coordinator, std::string device_id, HardwareCheckResult hardware_check) noexcept : coordinator_(coordinator), @@ -408,12 +408,12 @@ void DispatchGuard::reset_() noexcept coordinator->endDispatch_(device_id_); } -SafetyCoordinator::SafetyCoordinator(SafetyCoordinatorConfig config) +SafetyManager::SafetyManager(SafetyManagerConfig config) : impl_(std::make_unique(std::move(config))) { } -SafetyCoordinator::~SafetyCoordinator() +SafetyManager::~SafetyManager() { beginShutdown(); std::vector> endpoints; @@ -436,7 +436,7 @@ SafetyCoordinator::~SafetyCoordinator() } } -bool SafetyCoordinator::registerDevice(DeviceSafetyRegistration registration) +bool SafetyManager::registerDevice(DeviceSafetyRegistration registration) { if (registration.descriptor.device_id.empty() || registration.descriptor.maximum_snapshot_age <= @@ -531,7 +531,7 @@ bool SafetyCoordinator::registerDevice(DeviceSafetyRegistration registration) return true; } -bool SafetyCoordinator::unregisterDevice(const std::string& device_id) +bool SafetyManager::unregisterDevice(const std::string& device_id) { std::shared_ptr endpoint; std::string participant_id; @@ -576,7 +576,7 @@ bool SafetyCoordinator::unregisterDevice(const std::string& device_id) return true; } -bool SafetyCoordinator::registerParticipant( +bool SafetyManager::registerParticipant( std::shared_ptr participant) { if (!participant) { @@ -597,7 +597,7 @@ bool SafetyCoordinator::registerParticipant( return true; } -bool SafetyCoordinator::unregisterParticipant( +bool SafetyManager::unregisterParticipant( const std::string& participant_id) { std::lock_guard lock(impl_->mutex); @@ -615,7 +615,7 @@ bool SafetyCoordinator::unregisterParticipant( return true; } -void SafetyCoordinator::updateDeviceRuntimeState( +void SafetyManager::updateDeviceRuntimeState( const std::string& device_id, const device::ManagedDeviceState lifecycle, device::DeviceHealthSnapshot health) @@ -633,7 +633,7 @@ void SafetyCoordinator::updateDeviceRuntimeState( impl_->state_changed.notify_all(); } -bool SafetyCoordinator::publishSafetySnapshot(DeviceSafetySnapshot snapshot) +bool SafetyManager::publishSafetySnapshot(DeviceSafetySnapshot snapshot) { const std::string device_id = snapshot.device_id; if (!impl_->snapshots.publish(std::move(snapshot))) { @@ -679,7 +679,7 @@ bool SafetyCoordinator::publishSafetySnapshot(DeviceSafetySnapshot snapshot) return true; } -std::optional SafetyCoordinator::advanceDeviceGeneration( +std::optional SafetyManager::advanceDeviceGeneration( const std::string& device_id) { const auto generation = impl_->snapshots.bumpGeneration(device_id); @@ -709,7 +709,7 @@ std::optional SafetyCoordinator::advanceDeviceGeneration( return generation; } -StartupCoverageResult SafetyCoordinator::validateStartupCoverage( +StartupCoverageResult SafetyManager::validateStartupCoverage( const SafetyClock::time_point deadline) { struct Target { @@ -844,7 +844,7 @@ StartupCoverageResult SafetyCoordinator::validateStartupCoverage( return result; } -void SafetyCoordinator::markStartupComplete() +void SafetyManager::markStartupComplete() { std::lock_guard lock(impl_->mutex); if (impl_->system_state != SystemAdmissionState::Starting) { @@ -863,7 +863,7 @@ void SafetyCoordinator::markStartupComplete() impl_->state_changed.notify_all(); } -void SafetyCoordinator::beginShutdown() noexcept +void SafetyManager::beginShutdown() noexcept { if (!impl_) { return; @@ -885,7 +885,7 @@ void SafetyCoordinator::beginShutdown() noexcept } } -AdmissionDecision SafetyCoordinator::evaluate( +AdmissionDecision SafetyManager::evaluate( const AdmissionRequest& request) const { AdmissionDecision decision; @@ -1039,7 +1039,7 @@ AdmissionDecision SafetyCoordinator::evaluate( return allow(); } -AdmissionResult SafetyCoordinator::admit(const AdmissionRequest& request) +AdmissionResult SafetyManager::admit(const AdmissionRequest& request) { AdmissionResult result; result.decision = evaluate(request); @@ -1060,7 +1060,7 @@ AdmissionResult SafetyCoordinator::admit(const AdmissionRequest& request) return result; } -HardwareCheckResult SafetyCoordinator::revalidatePermit( +HardwareCheckResult SafetyManager::revalidatePermit( const AdmissionPermit& permit) const { const auto rejected = [](const SafetyReason reason, std::string detail) { @@ -1114,7 +1114,7 @@ HardwareCheckResult SafetyCoordinator::revalidatePermit( return {true, SafetyReason::None, {}}; } -DispatchGuard SafetyCoordinator::beginDispatch( +DispatchGuard SafetyManager::beginDispatch( const AdmissionPermit& permit) { const auto rejected = [&permit]( @@ -1205,7 +1205,7 @@ DispatchGuard SafetyCoordinator::beginDispatch( return DispatchGuard(this, permit.device_id, std::move(hardware_check)); } -void SafetyCoordinator::endDispatch_(const std::string& device_id) noexcept +void SafetyManager::endDispatch_(const std::string& device_id) noexcept { try { std::lock_guard lock(impl_->mutex); @@ -1219,7 +1219,7 @@ void SafetyCoordinator::endDispatch_(const std::string& device_id) noexcept } } -void SafetyCoordinator::quarantineDevice( +void SafetyManager::quarantineDevice( const std::string& device_id, const SafetyReason reason, std::string operation_id) @@ -1245,7 +1245,7 @@ void SafetyCoordinator::quarantineDevice( impl_->state_changed.notify_all(); } -StopAllResult SafetyCoordinator::stopAll( +StopAllResult SafetyManager::stopAll( std::string operation_id, SafetyClock::time_point deadline) { @@ -1754,7 +1754,7 @@ StopAllResult SafetyCoordinator::stopAll( return result; } -RecoveryResult SafetyCoordinator::recover(const RecoveryRequest& request) +RecoveryResult SafetyManager::recover(const RecoveryRequest& request) { RecoveryResult invalid; invalid.recovery_id = request.recovery_id; @@ -2323,9 +2323,9 @@ RecoveryResult SafetyCoordinator::recover(const RecoveryRequest& request) return result; } -SafetyCoordinatorSnapshot SafetyCoordinator::snapshot() const +SafetyManagerSnapshot SafetyManager::snapshot() const { - SafetyCoordinatorSnapshot result; + SafetyManagerSnapshot result; std::lock_guard lock(impl_->mutex); result.system_state = impl_->system_state; result.safety_epoch = impl_->safety_epoch; @@ -2376,32 +2376,32 @@ SafetyCoordinatorSnapshot SafetyCoordinator::snapshot() const return result; } -SafetySnapshotStore& SafetyCoordinator::snapshotStore() noexcept +SafetySnapshotStore& SafetyManager::snapshotStore() noexcept { return impl_->snapshots; } -const SafetySnapshotStore& SafetyCoordinator::snapshotStore() const noexcept +const SafetySnapshotStore& SafetyManager::snapshotStore() const noexcept { return impl_->snapshots; } -CommandLedger& SafetyCoordinator::commandLedger() noexcept +CommandLedger& SafetyManager::commandLedger() noexcept { return impl_->ledger; } -const CommandLedger& SafetyCoordinator::commandLedger() const noexcept +const CommandLedger& SafetyManager::commandLedger() const noexcept { return impl_->ledger; } -const std::string& SafetyCoordinator::serviceInstanceId() const noexcept +const std::string& SafetyManager::serviceInstanceId() const noexcept { return impl_->service_instance_id; } -const SafetyCoordinatorConfig& SafetyCoordinator::config() const noexcept +const SafetyManagerConfig& SafetyManager::config() const noexcept { return impl_->config; } diff --git a/cmvr-es/manager/safety/src/safety_reason.cpp b/cmvr-es/manager/safety_manager/src/safety_reason.cpp similarity index 98% rename from cmvr-es/manager/safety/src/safety_reason.cpp rename to cmvr-es/manager/safety_manager/src/safety_reason.cpp index 14a9717b..87e22e39 100644 --- a/cmvr-es/manager/safety/src/safety_reason.cpp +++ b/cmvr-es/manager/safety_manager/src/safety_reason.cpp @@ -1,4 +1,4 @@ -#include "manager/safety/include/safety_reason.h" +#include "manager/safety_manager/include/safety_reason.h" namespace cmvr::safety { diff --git a/cmvr-es/manager/safety/src/safety_snapshot_store.cpp b/cmvr-es/manager/safety_manager/src/safety_snapshot_store.cpp similarity index 99% rename from cmvr-es/manager/safety/src/safety_snapshot_store.cpp rename to cmvr-es/manager/safety_manager/src/safety_snapshot_store.cpp index dc7341d8..5a80a38b 100644 --- a/cmvr-es/manager/safety/src/safety_snapshot_store.cpp +++ b/cmvr-es/manager/safety_manager/src/safety_snapshot_store.cpp @@ -1,4 +1,4 @@ -#include "manager/safety/include/safety_snapshot_store.h" +#include "manager/safety_manager/include/safety_snapshot_store.h" #include #include diff --git a/cmvr-es/manager/safety/tests/command_ledger_test.cpp b/cmvr-es/manager/safety_manager/tests/command_ledger_test.cpp similarity index 98% rename from cmvr-es/manager/safety/tests/command_ledger_test.cpp rename to cmvr-es/manager/safety_manager/tests/command_ledger_test.cpp index 4515e356..bad8e503 100644 --- a/cmvr-es/manager/safety/tests/command_ledger_test.cpp +++ b/cmvr-es/manager/safety_manager/tests/command_ledger_test.cpp @@ -1,4 +1,4 @@ -#include "manager/safety/include/command_ledger.h" +#include "manager/safety_manager/include/command_ledger.h" #include #include diff --git a/cmvr-es/manager/safety/tests/safety_coordinator_test.cpp b/cmvr-es/manager/safety_manager/tests/safety_manager_test.cpp similarity index 91% rename from cmvr-es/manager/safety/tests/safety_coordinator_test.cpp rename to cmvr-es/manager/safety_manager/tests/safety_manager_test.cpp index fe43d4b6..1667e251 100644 --- a/cmvr-es/manager/safety/tests/safety_coordinator_test.cpp +++ b/cmvr-es/manager/safety_manager/tests/safety_manager_test.cpp @@ -1,4 +1,4 @@ -#include "manager/safety/include/safety_coordinator.h" +#include "manager/safety_manager/include/safety_manager.h" #include #include @@ -166,9 +166,9 @@ AdmissionRequest actuateRequest() return request; } -TEST(SafetyCoordinatorTest, ShadowReportsDenyWithoutChangingLegacyBehavior) +TEST(SafetyManagerTest, ShadowReportsDenyWithoutChangingLegacyBehavior) { - SafetyCoordinator coordinator; + SafetyManager coordinator; ASSERT_TRUE(coordinator.registerDevice({controlDescriptor(), {}, {}})); coordinator.markStartupComplete(); @@ -180,22 +180,22 @@ TEST(SafetyCoordinatorTest, ShadowReportsDenyWithoutChangingLegacyBehavior) EXPECT_TRUE(result.permit.has_value()); } -TEST(SafetyCoordinatorTest, +TEST(SafetyManagerTest, EnforceSelectedStartupRejectsEmptyOrUnknownCoverage) { - SafetyCoordinatorConfig empty_config; + SafetyManagerConfig empty_config; empty_config.enforcement_mode = EnforcementMode::EnforceSelected; - SafetyCoordinator empty(empty_config); + SafetyManager 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; + SafetyManagerConfig missing_config; missing_config.enforcement_mode = EnforcementMode::EnforceSelected; missing_config.enforced_device_ids.insert("missing-arm"); - SafetyCoordinator missing(missing_config); + SafetyManager missing(missing_config); const auto missing_result = missing.validateStartupCoverage( SafetyClock::now() + std::chrono::milliseconds(10)); ASSERT_FALSE(missing_result.ready); @@ -204,13 +204,13 @@ TEST(SafetyCoordinatorTest, EXPECT_EQ(missing_result.issues.front().reason, SafetyReason::DeviceNotFound); } -TEST(SafetyCoordinatorTest, +TEST(SafetyManagerTest, EnforceAllStartupRequiresEndpointParticipantAndFreshSnapshot) { - SafetyCoordinatorConfig config; + SafetyManagerConfig config; config.enforcement_mode = EnforcementMode::EnforceAll; - SafetyCoordinator missing_capability(config); + SafetyManager missing_capability(config); ASSERT_TRUE(missing_capability.registerDevice( {controlDescriptor(), {}, {}})); const auto structural = missing_capability.validateStartupCoverage( @@ -218,7 +218,7 @@ TEST(SafetyCoordinatorTest, EXPECT_FALSE(structural.ready); EXPECT_EQ(structural.issues.size(), 2U); - SafetyCoordinator missing_sample(config); + SafetyManager missing_sample(config); auto silent_endpoint = std::make_shared(controlDescriptor()); silent_endpoint->publish_on_refresh = false; @@ -233,12 +233,12 @@ TEST(SafetyCoordinatorTest, EXPECT_EQ(stale.issues.front().reason, SafetyReason::SafetyStateMissing); } -TEST(SafetyCoordinatorTest, +TEST(SafetyManagerTest, HardwareUnsafeSnapshotBlocksAdmissionButNotStructuralStartup) { - SafetyCoordinatorConfig config; + SafetyManagerConfig config; config.enforcement_mode = EnforcementMode::EnforceAll; - SafetyCoordinator coordinator(config); + SafetyManager coordinator(config); auto endpoint = std::make_shared(controlDescriptor()); endpoint->condition = SafetyCondition::Unsafe; endpoint->emergency_stop = TriState::True; @@ -258,11 +258,11 @@ TEST(SafetyCoordinatorTest, admission.decision.reason, SafetyReason::EmergencyStopActive); } -TEST(SafetyCoordinatorTest, EnforceAllFailsClosedOnUnknownControlState) +TEST(SafetyManagerTest, EnforceAllFailsClosedOnUnknownControlState) { - SafetyCoordinatorConfig config; + SafetyManagerConfig config; config.enforcement_mode = EnforcementMode::EnforceAll; - SafetyCoordinator coordinator(config); + SafetyManager coordinator(config); ASSERT_TRUE(coordinator.registerDevice({controlDescriptor(), {}, {}})); coordinator.markStartupComplete(); @@ -273,11 +273,11 @@ TEST(SafetyCoordinatorTest, EnforceAllFailsClosedOnUnknownControlState) EXPECT_FALSE(result.permit.has_value()); } -TEST(SafetyCoordinatorTest, ControlSafetyBitsMustBeExplicitlyFalse) +TEST(SafetyManagerTest, ControlSafetyBitsMustBeExplicitlyFalse) { - SafetyCoordinatorConfig config; + SafetyManagerConfig config; config.enforcement_mode = EnforcementMode::EnforceAll; - SafetyCoordinator coordinator(config); + SafetyManager coordinator(config); auto endpoint = std::make_shared(controlDescriptor()); endpoint->protective_stop = TriState::Unknown; ASSERT_TRUE(coordinator.registerDevice( @@ -296,11 +296,11 @@ TEST(SafetyCoordinatorTest, ControlSafetyBitsMustBeExplicitlyFalse) DeviceAdmissionState::Blocked); } -TEST(SafetyCoordinatorTest, EnforcedDispatchRunsFinalHardwareCheck) +TEST(SafetyManagerTest, EnforcedDispatchRunsFinalHardwareCheck) { - SafetyCoordinatorConfig config; + SafetyManagerConfig config; config.enforcement_mode = EnforcementMode::EnforceAll; - SafetyCoordinator coordinator(config); + SafetyManager coordinator(config); auto endpoint = std::make_shared(controlDescriptor()); ASSERT_TRUE(coordinator.registerDevice( {controlDescriptor(), endpoint, {}})); @@ -317,12 +317,12 @@ TEST(SafetyCoordinatorTest, EnforcedDispatchRunsFinalHardwareCheck) EXPECT_EQ(endpoint->hardware_checks.load(), 1); } -TEST(SafetyCoordinatorTest, +TEST(SafetyManagerTest, StartActivityMayEnterFromRestrictedButActuationMayNot) { - SafetyCoordinatorConfig config; + SafetyManagerConfig config; config.enforcement_mode = EnforcementMode::EnforceAll; - SafetyCoordinator coordinator(config); + SafetyManager coordinator(config); auto endpoint = std::make_shared(controlDescriptor()); endpoint->condition = SafetyCondition::Restricted; endpoint->ready = TriState::False; @@ -346,11 +346,11 @@ TEST(SafetyCoordinatorTest, EXPECT_EQ(actuation.decision.reason, SafetyReason::HardwareUnsafe); } -TEST(SafetyCoordinatorTest, SuccessfulStopInvalidatesOldPermitAndReopens) +TEST(SafetyManagerTest, SuccessfulStopInvalidatesOldPermitAndReopens) { - SafetyCoordinatorConfig config; + SafetyManagerConfig config; config.enforcement_mode = EnforcementMode::EnforceAll; - SafetyCoordinator coordinator(config); + SafetyManager coordinator(config); auto endpoint = std::make_shared(controlDescriptor()); auto participant = std::make_shared(); ASSERT_TRUE(coordinator.registerDevice( @@ -388,11 +388,11 @@ TEST(SafetyCoordinatorTest, SuccessfulStopInvalidatesOldPermitAndReopens) EXPECT_TRUE(snapshot.participants.front().last_release.success); } -TEST(SafetyCoordinatorTest, FailedStopRequiresVerifiedRecovery) +TEST(SafetyManagerTest, FailedStopRequiresVerifiedRecovery) { - SafetyCoordinatorConfig config; + SafetyManagerConfig config; config.enforcement_mode = EnforcementMode::EnforceAll; - SafetyCoordinator coordinator(config); + SafetyManager coordinator(config); auto endpoint = std::make_shared(controlDescriptor()); auto participant = std::make_shared(); participant->verify_result = { @@ -437,9 +437,9 @@ TEST(SafetyCoordinatorTest, FailedStopRequiresVerifiedRecovery) EXPECT_TRUE(reopened.participants.front().last_release.success); } -TEST(SafetyCoordinatorTest, RecoveryCannotIgnoreEmergencyStop) +TEST(SafetyManagerTest, RecoveryCannotIgnoreEmergencyStop) { - SafetyCoordinator coordinator; + SafetyManager coordinator; auto endpoint = std::make_shared(controlDescriptor()); auto participant = std::make_shared(); ASSERT_TRUE(coordinator.registerDevice( @@ -466,9 +466,9 @@ TEST(SafetyCoordinatorTest, RecoveryCannotIgnoreEmergencyStop) EXPECT_EQ(endpoint->recoveries.load(), 0); } -TEST(SafetyCoordinatorTest, RecoveryAuditFailureCannotReleaseLatch) +TEST(SafetyManagerTest, RecoveryAuditFailureCannotReleaseLatch) { - SafetyCoordinator coordinator; + SafetyManager coordinator; auto endpoint = std::make_shared(controlDescriptor()); auto participant = std::make_shared(); participant->verify_result = { diff --git a/cmvr-es/manager/safety/tests/safety_snapshot_store_test.cpp b/cmvr-es/manager/safety_manager/tests/safety_snapshot_store_test.cpp similarity index 98% rename from cmvr-es/manager/safety/tests/safety_snapshot_store_test.cpp rename to cmvr-es/manager/safety_manager/tests/safety_snapshot_store_test.cpp index c5cdba4d..7d2c93d6 100644 --- a/cmvr-es/manager/safety/tests/safety_snapshot_store_test.cpp +++ b/cmvr-es/manager/safety_manager/tests/safety_snapshot_store_test.cpp @@ -1,4 +1,4 @@ -#include "manager/safety/include/safety_snapshot_store.h" +#include "manager/safety_manager/include/safety_snapshot_store.h" #include #include diff --git a/cmvr-es/manager/task_manager/src/task_manager.cpp b/cmvr-es/manager/task_manager/src/task_manager.cpp index 85ba6df9..12e0a53b 100644 --- a/cmvr-es/manager/task_manager/src/task_manager.cpp +++ b/cmvr-es/manager/task_manager/src/task_manager.cpp @@ -9,7 +9,7 @@ #include "common/base/logging/logger.h" #include "common/config/config_files.h" -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include "task/task_factory.h" using namespace cmvr; diff --git a/cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp b/cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp index 1402bc29..bc7a4bf8 100644 --- a/cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp +++ b/cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp @@ -12,7 +12,7 @@ #include -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include "task/task_factory.h" namespace { diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index 75f57cc9..4e0971cf 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -1,548 +1,4 @@ - -add_library(service - stop_all/src/stop_operation_dispatcher.cpp - action/src/action_queue_executor.cpp - grpc/src/camera_ptz_activity_registry.cpp - 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 - grpc/src/grpc_head_service.cpp - grpc/src/grpc_dexhand_service.cpp - grpc/src/grpc_arm_service.cpp - grpc/src/grpc_arm_teleop_service.cpp - grpc/src/grpc_robot_arm_teleop_backend.cpp - grpc/src/grpc_motor_service.cpp - grpc/src/grpc_agv_service.cpp - grpc/src/grpc_hlc_service.cpp - ../task/grpc_server_task/src/grpc_server_task.cpp -) - -target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -target_link_libraries(service PRIVATE - cmvr_es::proto - cmvr_es::stop_all_admission_gate - cmvr_es::camera_operational_activity_registry - osqp - cmvr_es::control_authority - cmvr_es::device_manager - cmvr_es::task_manager - cmvr_es::algorithms::controller - cmvr_es::task - cmvr_es::media_source_hub - cmvr_es::device_media_source_adapter - protobuf::libprotobuf -) - -add_library(cmvr_es::service ALIAS service) -install(TARGETS service LIBRARY DESTINATION lib) - -if(BUILD_TESTING) - add_executable(stop_all_admission_gate_test - stop_all/tests/stop_all_admission_gate_test.cpp - stop_all/src/stop_all_admission_gate.cpp - ) - target_include_directories(stop_all_admission_gate_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(stop_all_admission_gate_test PRIVATE - gtest - gtest_main - pthread - ) - add_test( - NAME stop_all_admission_gate_test - COMMAND stop_all_admission_gate_test - ) - set_tests_properties(stop_all_admission_gate_test PROPERTIES TIMEOUT 10) - - add_executable(stop_operation_dispatcher_test - stop_all/tests/stop_operation_dispatcher_test.cpp - stop_all/src/stop_operation_dispatcher.cpp - ) - target_include_directories(stop_operation_dispatcher_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(stop_operation_dispatcher_test PRIVATE - gtest - gtest_main - pthread - ) - add_test( - NAME stop_operation_dispatcher_test - COMMAND stop_operation_dispatcher_test - ) - set_tests_properties(stop_operation_dispatcher_test PROPERTIES TIMEOUT 10) - - add_executable(camera_operational_activity_registry_test - grpc/tests/camera_operational_activity_registry_test.cpp - ) - target_include_directories(camera_operational_activity_registry_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(camera_operational_activity_registry_test PRIVATE - cmvr_es::proto - cmvr_es::camera_operational_activity_registry - gtest - gtest_main - pthread - ) - add_test( - NAME camera_operational_activity_registry_test - COMMAND camera_operational_activity_registry_test - ) - set_tests_properties(camera_operational_activity_registry_test PROPERTIES - TIMEOUT 10) - - add_executable(camera_ptz_activity_registry_test - grpc/tests/camera_ptz_activity_registry_test.cpp - grpc/src/camera_ptz_activity_registry.cpp - stop_all/src/stop_all_admission_gate.cpp - ) - target_include_directories(camera_ptz_activity_registry_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(camera_ptz_activity_registry_test PRIVATE - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME camera_ptz_activity_registry_test - COMMAND camera_ptz_activity_registry_test - ) - set_tests_properties(camera_ptz_activity_registry_test PROPERTIES TIMEOUT 10) - - add_executable(media_activity_coordinator_test - grpc/tests/media_activity_coordinator_test.cpp - grpc/src/media_activity_coordinator.cpp - stop_all/src/stop_all_admission_gate.cpp - ) - target_include_directories(media_activity_coordinator_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(media_activity_coordinator_test PRIVATE - cmvr_es::logging - pthread - ) - add_test( - NAME media_activity_coordinator_test - COMMAND media_activity_coordinator_test - ) - set(_grpc_media_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _grpc_media_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(media_activity_coordinator_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_grpc_media_test_environment}" - ) - - add_executable(motor_activity_coordinator_test - grpc/tests/motor_activity_coordinator_test.cpp - grpc/src/motor_activity_coordinator.cpp - ) - target_include_directories(motor_activity_coordinator_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(motor_activity_coordinator_test PRIVATE - cmvr_es::logging - pthread - ) - add_test( - NAME motor_activity_coordinator_test - COMMAND motor_activity_coordinator_test - ) - set_tests_properties(motor_activity_coordinator_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_grpc_media_test_environment}" - ) - - add_executable(grpc_camera_stream_policy_test - grpc/tests/grpc_camera_stream_policy_test.cpp - ) - target_include_directories(grpc_camera_stream_policy_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - add_test( - NAME grpc_camera_stream_policy_test - COMMAND grpc_camera_stream_policy_test - ) - set_tests_properties(grpc_camera_stream_policy_test PROPERTIES TIMEOUT 10) - - add_executable(grpc_system_service_test - grpc/tests/grpc_system_service_test.cpp - ) - target_include_directories(grpc_system_service_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager - ) - target_link_libraries(grpc_system_service_test - PRIVATE - service - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_system_service_test - COMMAND grpc_system_service_test - ) - set(_grpc_system_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _grpc_system_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(grpc_system_service_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_grpc_system_test_environment}" - ) - - add_executable(grpc_error_logging_interceptor_test - grpc/tests/grpc_error_logging_interceptor_test.cpp - grpc/src/grpc_error_logging_interceptor.cpp - ) - target_include_directories(grpc_error_logging_interceptor_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(grpc_error_logging_interceptor_test - PRIVATE - cmvr_es::logging - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_error_logging_interceptor_test - COMMAND grpc_error_logging_interceptor_test - ) - set_tests_properties(grpc_error_logging_interceptor_test PROPERTIES - TIMEOUT 10 - 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 - ) - target_include_directories(grpc_arm_service_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager - ) - target_link_libraries(grpc_arm_service_test - PRIVATE - service - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_arm_service_test - COMMAND grpc_arm_service_test - ) - set_tests_properties(grpc_arm_service_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_grpc_system_test_environment}" - ) - - add_executable(grpc_arm_teleop_service_test - grpc/tests/grpc_arm_teleop_service_test.cpp - ) - target_include_directories(grpc_arm_teleop_service_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(grpc_arm_teleop_service_test - PRIVATE - service - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_arm_teleop_service_test - COMMAND grpc_arm_teleop_service_test - ) - set(_grpc_arm_teleop_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _grpc_arm_teleop_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(grpc_arm_teleop_service_test PROPERTIES - TIMEOUT 20 - ENVIRONMENT "${_grpc_arm_teleop_test_environment}" - ) - - add_executable(grpc_robot_arm_teleop_backend_test - grpc/tests/grpc_robot_arm_teleop_backend_test.cpp - ) - target_include_directories(grpc_robot_arm_teleop_backend_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(grpc_robot_arm_teleop_backend_test - PRIVATE - service - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_robot_arm_teleop_backend_test - COMMAND grpc_robot_arm_teleop_backend_test - ) - set_tests_properties(grpc_robot_arm_teleop_backend_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_grpc_arm_teleop_test_environment}" - ) - - add_executable(grpc_motor_service_test - grpc/tests/grpc_motor_service_test.cpp - ) - target_include_directories(grpc_motor_service_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager - ) - target_link_libraries(grpc_motor_service_test - PRIVATE - service - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_motor_service_test - COMMAND grpc_motor_service_test - ) - set(_grpc_motor_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _grpc_motor_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(grpc_motor_service_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_grpc_motor_test_environment}" - ) - - add_executable(grpc_agv_service_test - grpc/tests/grpc_agv_service_test.cpp - ) - target_include_directories(grpc_agv_service_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager - ) - target_link_libraries(grpc_agv_service_test - PRIVATE - service - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_agv_service_test - COMMAND grpc_agv_service_test - ) - set(_grpc_agv_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _grpc_agv_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(grpc_agv_service_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_grpc_agv_test_environment}" - ) - - add_executable(grpc_head_service_test - grpc/tests/grpc_head_service_test.cpp - ) - target_include_directories(grpc_head_service_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager - ) - target_link_libraries(grpc_head_service_test - PRIVATE - service - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_head_service_test - COMMAND grpc_head_service_test - ) - set_tests_properties(grpc_head_service_test PROPERTIES - TIMEOUT 15 - ENVIRONMENT "${_grpc_system_test_environment}" - ) - - add_executable(grpc_dexhand_service_test - grpc/tests/grpc_dexhand_service_test.cpp - ) - target_include_directories(grpc_dexhand_service_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager - ) - target_link_libraries(grpc_dexhand_service_test - PRIVATE - service - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_dexhand_service_test - COMMAND grpc_dexhand_service_test - ) - set_tests_properties(grpc_dexhand_service_test PROPERTIES - TIMEOUT 15 - ENVIRONMENT "${_grpc_system_test_environment}" - ) - -endif() - -# -------------------------------------------------------- -# Unit test -# -------------------------------------------------------- -find_package(OpenCV REQUIRED) - -include_directories( - ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include -) - -link_directories( - ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib -) - - -add_executable(grpc_arm_client_test - grpc/src/grpc_arm_client_test.cpp -) - - -target_link_libraries(grpc_arm_client_test - PRIVATE - cmvr_es::device::canbus - cmvr_es::device::ti5_canopen_motor_driver - osqp - gtest - gtest_main - pthread - glog - cmvr_es::proto - ccd - fcl - cmvr_es::device_manager - ${OpenCV_LIBS} -) - - -add_executable(grpc_hlc_client_test - grpc/src/grpc_hlc_client_test.cpp -) - - -target_link_libraries(grpc_hlc_client_test - PRIVATE - cmvr_es::device::canbus - cmvr_es::device::ti5_canopen_motor_driver - osqp - gtest - gtest_main - pthread - glog - cmvr_es::proto - ccd - fcl - cmvr_es::device_manager -) +# Service is intentionally split by transport. The QUIC edge target is added +# from the project root before task targets; the gRPC tree is added here after +# its manager and task dependencies are available. +add_subdirectory(grpc) diff --git a/cmvr-es/service/README.md b/cmvr-es/service/README.md index 9a78d3e6..39a1ee13 100644 --- a/cmvr-es/service/README.md +++ b/cmvr-es/service/README.md @@ -6,14 +6,33 @@ ## 当前结构 +`service/` 顶层只按传输协议保留两个子目录：`grpc/` 和 `quic_edge/`。gRPC +内部再按运行角色分层，避免把队列、停止控制、客户端和服务端实现混在同一层。 + +```text +service/ +├── grpc/ +│ ├── action/ # ActionQueue 校验、账本和 FIFO 执行器 +│ ├── client/ # 边缘端使用的 gRPC client +│ ├── server/ # gRPC service、协调器和安全扩展 +│ │ ├── include/ +│ │ ├── src/ +│ │ └── tests/ +│ └── stop_all/ # 高优先级 StopAll 通道 +└── quic_edge/ # QUIC client、控制状态机和媒体 packetizer +``` + | 目录 | 职责 | | --- | --- | -| `action/` | SystemService ActionQueue 的校验、幂等账本和边缘端 FIFO 执行器 | -| `grpc/` | 入站设备控制、状态查询和兼容流式接口 | +| `grpc/action/` | SystemService ActionQueue 的校验、幂等账本和边缘端 FIFO 执行器 | +| `grpc/client/` | 面向边缘端内部调用的 gRPC client | +| `grpc/server/` | 入站设备控制、状态查询、兼容流式接口和安全控制面 | +| `grpc/stop_all/` | 不进入普通命令队列的高优先级停止通道 | | `quic_edge/` | 边缘端主动连接平台的 QUIC client、控制状态机和媒体 packetizer | | `quic_edge/tests/` | 已登记到 CTest 的 QUIC 协议测试 | -两个遗留 gRPC client test 位于 `grpc/src/*_client_test.cpp`，当前没有通过 `add_test()` 登记。 +两个遗留 gRPC client test 位于 `grpc/server/tests/*_client_test.cpp`，当前没有通过 +`add_test()` 登记；它们是历史可执行文件，不代表默认自动覆盖。 gRPC 和 QUIC 的职责边界： @@ -99,10 +118,10 @@ ActionQueue 遵循以下执行语义： - Proto：[`../../protos/cmvr/api/motor_service.proto`](../../protos/cmvr/api/motor_service.proto) 和 [`../../protos/cmvr/api/motor_command.proto`](../../protos/cmvr/api/motor_command.proto) -- 实现：[`grpc/include/grpc_motor_service.h`](grpc/include/grpc_motor_service.h) - 和 [`grpc/src/grpc_motor_service.cpp`](grpc/src/grpc_motor_service.cpp) +- 实现：[`grpc/server/include/grpc_motor_service.h`](grpc/server/include/grpc_motor_service.h) + 和 [`grpc/server/src/grpc_motor_service.cpp`](grpc/server/src/grpc_motor_service.cpp) - 注册：[`../task/grpc_server_task/src/grpc_server_task.cpp`](../task/grpc_server_task/src/grpc_server_task.cpp) -- 单元测试：[`grpc/tests/grpc_motor_service_test.cpp`](grpc/tests/grpc_motor_service_test.cpp) +- 单元测试：[`grpc/server/tests/grpc_motor_service_test.cpp`](grpc/server/tests/grpc_motor_service_test.cpp) 服务按单电机仲裁。同步 Profile 命令、Cyclic Position/Velocity 双向流、 `setEnabled`、状态读取和软件 `emergencyStop` 共用同一控制权状态： @@ -140,8 +159,9 @@ import 路径必须相对于 `protos/`。兼容规则见 [`../../protos/README.m ```text service/grpc/ -├── include/grpc_example_service.h -└── src/grpc_example_service.cpp +└── server/ + ├── include/grpc_example_service.h + └── src/grpc_example_service.cpp ``` 实现类继承生成的： @@ -203,7 +223,7 @@ cmvr::api::ExampleService::Service - 检查 `context->IsCancelled()`； - 检查 `Read()` / `Write()` 返回； -- 使用 RAII 或 MediaSourceHub Subscription 释放 producer lease； +- 使用 RAII 或 MediaSourceManager Subscription 释放 producer lease； - 不持有设备状态锁进行网络写； - 为 wait/read 使用有限 timeout； - 慢客户端不能阻塞设备生产线程； @@ -211,7 +231,7 @@ cmvr::api::ExampleService::Service - gRPC RGB 流在积压超过 `camera_stream_max_pending_frames` 或帧龄超过 `camera_stream_max_frame_age_ms` 时主动丢弃旧帧，请求 IDR，并从下一个关键帧恢复。 -当前仅 gRPC RGB 和麦克风流使用 MediaSourceHub；Depth/RGBD 仍直接读取设备帧。 +当前仅 gRPC RGB 和麦克风流使用 MediaSourceManager；Depth/RGBD 仍直接读取设备帧。 gRPC 相机实时流默认最多保留 2 帧积压、最大允许 250 ms 帧龄。两个配置项填 0 时使用上述默认值。该策略以低延迟为目标，不保证每个视频帧都到达客户端；控制命令 diff --git a/cmvr-es/service/grpc/CMakeLists.txt b/cmvr-es/service/grpc/CMakeLists.txt new file mode 100644 index 00000000..cff56bd2 --- /dev/null +++ b/cmvr-es/service/grpc/CMakeLists.txt @@ -0,0 +1,550 @@ + +add_library(service + stop_all/src/stop_operation_dispatcher.cpp + action/src/action_queue_executor.cpp + server/src/camera_ptz_activity_registry.cpp + server/src/media_activity_coordinator.cpp + server/src/motor_activity_coordinator.cpp + server/src/grpc_camera_service.cpp + server/src/grpc_command_transaction.cpp + server/src/grpc_error_logging_interceptor.cpp + server/src/grpc_recovery_audit.cpp + server/src/grpc_safety_proto.cpp + server/src/grpc_safety_participants.cpp + server/src/grpc_security.cpp + server/src/grpc_system_service.cpp + server/src/grpc_speaker_service.cpp + server/src/grpc_microphone_service.cpp + server/src/grpc_head_service.cpp + server/src/grpc_dexhand_service.cpp + server/src/grpc_arm_service.cpp + server/src/grpc_arm_teleop_service.cpp + server/src/grpc_robot_arm_teleop_backend.cpp + server/src/grpc_motor_service.cpp + server/src/grpc_agv_service.cpp + server/src/grpc_hlc_service.cpp + ../../task/grpc_server_task/src/grpc_server_task.cpp +) + +target_include_directories(service PUBLIC + ${CMAKE_SOURCE_DIR}/cmvr-es +) + +target_link_libraries(service PRIVATE + cmvr_es::proto + cmvr_es::stop_all_admission_gate + cmvr_es::camera_operational_activity_registry + osqp + cmvr_es::control_authority_manager + cmvr_es::device_manager + cmvr_es::task_manager + cmvr_es::algorithms::controller + cmvr_es::task + cmvr_es::media_source_manager + cmvr_es::device_media_source_adapter + protobuf::libprotobuf +) + +add_library(cmvr_es::service ALIAS service) +install(TARGETS service LIBRARY DESTINATION lib) + +if(BUILD_TESTING) + add_executable(stop_all_admission_gate_test + stop_all/tests/stop_all_admission_gate_test.cpp + stop_all/src/stop_all_admission_gate.cpp + ) + target_include_directories(stop_all_admission_gate_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(stop_all_admission_gate_test PRIVATE + gtest + gtest_main + pthread + ) + add_test( + NAME stop_all_admission_gate_test + COMMAND stop_all_admission_gate_test + ) + set_tests_properties(stop_all_admission_gate_test PROPERTIES TIMEOUT 10) + + add_executable(stop_operation_dispatcher_test + stop_all/tests/stop_operation_dispatcher_test.cpp + stop_all/src/stop_operation_dispatcher.cpp + ) + target_include_directories(stop_operation_dispatcher_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(stop_operation_dispatcher_test PRIVATE + gtest + gtest_main + pthread + ) + add_test( + NAME stop_operation_dispatcher_test + COMMAND stop_operation_dispatcher_test + ) + set_tests_properties(stop_operation_dispatcher_test PROPERTIES TIMEOUT 10) + + add_executable(camera_operational_activity_registry_test + server/tests/camera_operational_activity_registry_test.cpp + ) + target_include_directories(camera_operational_activity_registry_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(camera_operational_activity_registry_test PRIVATE + cmvr_es::proto + cmvr_es::camera_operational_activity_registry + gtest + gtest_main + pthread + ) + add_test( + NAME camera_operational_activity_registry_test + COMMAND camera_operational_activity_registry_test + ) + set_tests_properties(camera_operational_activity_registry_test PROPERTIES + TIMEOUT 10) + + add_executable(camera_ptz_activity_registry_test + server/tests/camera_ptz_activity_registry_test.cpp + server/src/camera_ptz_activity_registry.cpp + stop_all/src/stop_all_admission_gate.cpp + ) + target_include_directories(camera_ptz_activity_registry_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(camera_ptz_activity_registry_test PRIVATE + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME camera_ptz_activity_registry_test + COMMAND camera_ptz_activity_registry_test + ) + set_tests_properties(camera_ptz_activity_registry_test PROPERTIES TIMEOUT 10) + + add_executable(media_activity_coordinator_test + server/tests/media_activity_coordinator_test.cpp + server/src/media_activity_coordinator.cpp + stop_all/src/stop_all_admission_gate.cpp + ) + target_include_directories(media_activity_coordinator_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(media_activity_coordinator_test PRIVATE + cmvr_es::logging + pthread + ) + add_test( + NAME media_activity_coordinator_test + COMMAND media_activity_coordinator_test + ) + set(_grpc_media_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _grpc_media_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(media_activity_coordinator_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_grpc_media_test_environment}" + ) + + add_executable(motor_activity_coordinator_test + server/tests/motor_activity_coordinator_test.cpp + server/src/motor_activity_coordinator.cpp + ) + target_include_directories(motor_activity_coordinator_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(motor_activity_coordinator_test PRIVATE + cmvr_es::logging + pthread + ) + add_test( + NAME motor_activity_coordinator_test + COMMAND motor_activity_coordinator_test + ) + set_tests_properties(motor_activity_coordinator_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_grpc_media_test_environment}" + ) + + add_executable(grpc_camera_stream_policy_test + server/tests/grpc_camera_stream_policy_test.cpp + ) + target_include_directories(grpc_camera_stream_policy_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + add_test( + NAME grpc_camera_stream_policy_test + COMMAND grpc_camera_stream_policy_test + ) + set_tests_properties(grpc_camera_stream_policy_test PROPERTIES TIMEOUT 10) + + add_executable(grpc_system_service_test + server/tests/grpc_system_service_test.cpp + ) + target_include_directories(grpc_system_service_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager + ) + target_link_libraries(grpc_system_service_test + PRIVATE + service + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_system_service_test + COMMAND grpc_system_service_test + ) + set(_grpc_system_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _grpc_system_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(grpc_system_service_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_grpc_system_test_environment}" + ) + + add_executable(grpc_error_logging_interceptor_test + server/tests/grpc_error_logging_interceptor_test.cpp + server/src/grpc_error_logging_interceptor.cpp + ) + target_include_directories(grpc_error_logging_interceptor_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(grpc_error_logging_interceptor_test + PRIVATE + cmvr_es::logging + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_error_logging_interceptor_test + COMMAND grpc_error_logging_interceptor_test + ) + set_tests_properties(grpc_error_logging_interceptor_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_grpc_system_test_environment}" + ) + + add_executable(grpc_security_test + server/tests/grpc_security_test.cpp + server/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 + server/tests/grpc_command_transaction_test.cpp + server/src/grpc_command_transaction.cpp + server/src/grpc_safety_proto.cpp + server/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_manager + 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 + server/tests/grpc_arm_service_test.cpp + ) + target_include_directories(grpc_arm_service_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager + ) + target_link_libraries(grpc_arm_service_test + PRIVATE + service + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_arm_service_test + COMMAND grpc_arm_service_test + ) + set_tests_properties(grpc_arm_service_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_grpc_system_test_environment}" + ) + + add_executable(grpc_arm_teleop_service_test + server/tests/grpc_arm_teleop_service_test.cpp + ) + target_include_directories(grpc_arm_teleop_service_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(grpc_arm_teleop_service_test + PRIVATE + service + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_arm_teleop_service_test + COMMAND grpc_arm_teleop_service_test + ) + set(_grpc_arm_teleop_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _grpc_arm_teleop_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(grpc_arm_teleop_service_test PROPERTIES + TIMEOUT 20 + ENVIRONMENT "${_grpc_arm_teleop_test_environment}" + ) + + add_executable(grpc_robot_arm_teleop_backend_test + server/tests/grpc_robot_arm_teleop_backend_test.cpp + ) + target_include_directories(grpc_robot_arm_teleop_backend_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(grpc_robot_arm_teleop_backend_test + PRIVATE + service + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_robot_arm_teleop_backend_test + COMMAND grpc_robot_arm_teleop_backend_test + ) + set_tests_properties(grpc_robot_arm_teleop_backend_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_grpc_arm_teleop_test_environment}" + ) + + add_executable(grpc_motor_service_test + server/tests/grpc_motor_service_test.cpp + ) + target_include_directories(grpc_motor_service_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager + ) + target_link_libraries(grpc_motor_service_test + PRIVATE + service + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_motor_service_test + COMMAND grpc_motor_service_test + ) + set(_grpc_motor_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _grpc_motor_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(grpc_motor_service_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_grpc_motor_test_environment}" + ) + + add_executable(grpc_agv_service_test + server/tests/grpc_agv_service_test.cpp + ) + target_include_directories(grpc_agv_service_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager + ) + target_link_libraries(grpc_agv_service_test + PRIVATE + service + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_agv_service_test + COMMAND grpc_agv_service_test + ) + set(_grpc_agv_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _grpc_agv_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(grpc_agv_service_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_grpc_agv_test_environment}" + ) + + add_executable(grpc_head_service_test + server/tests/grpc_head_service_test.cpp + ) + target_include_directories(grpc_head_service_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager + ) + target_link_libraries(grpc_head_service_test + PRIVATE + service + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_head_service_test + COMMAND grpc_head_service_test + ) + set_tests_properties(grpc_head_service_test PROPERTIES + TIMEOUT 15 + ENVIRONMENT "${_grpc_system_test_environment}" + ) + + add_executable(grpc_dexhand_service_test + server/tests/grpc_dexhand_service_test.cpp + ) + target_include_directories(grpc_dexhand_service_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager + ) + target_link_libraries(grpc_dexhand_service_test + PRIVATE + service + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_dexhand_service_test + COMMAND grpc_dexhand_service_test + ) + set_tests_properties(grpc_dexhand_service_test PROPERTIES + TIMEOUT 15 + ENVIRONMENT "${_grpc_system_test_environment}" + ) + +endif() + +# -------------------------------------------------------- +# Unit test +# -------------------------------------------------------- +find_package(OpenCV REQUIRED) + +include_directories( + ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include +) + +link_directories( + ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib +) + + +add_executable(grpc_arm_client_test + server/tests/grpc_arm_client_test.cpp +) + + +target_link_libraries(grpc_arm_client_test + PRIVATE + cmvr_es::device::canbus + cmvr_es::device::ti5_canopen_motor_driver + osqp + gtest + gtest_main + pthread + glog + cmvr_es::proto + ccd + fcl + cmvr_es::device_manager + ${OpenCV_LIBS} +) + + +add_executable(grpc_hlc_client_test + server/tests/grpc_hlc_client_test.cpp +) + + +target_link_libraries(grpc_hlc_client_test + PRIVATE + cmvr_es::device::canbus + cmvr_es::device::ti5_canopen_motor_driver + osqp + gtest + gtest_main + pthread + glog + cmvr_es::proto + ccd + fcl + cmvr_es::device_manager +) diff --git a/cmvr-es/service/action/include/action_queue_executor.h b/cmvr-es/service/grpc/action/include/action_queue_executor.h similarity index 98% rename from cmvr-es/service/action/include/action_queue_executor.h rename to cmvr-es/service/grpc/action/include/action_queue_executor.h index 84587832..a80eb1be 100644 --- a/cmvr-es/service/action/include/action_queue_executor.h +++ b/cmvr-es/service/grpc/action/include/action_queue_executor.h @@ -9,7 +9,7 @@ #include #include "cmvr/api/system_command.pb.h" -#include "manager/safety/include/safety_types.h" +#include "manager/safety_manager/include/safety_types.h" namespace cmvr::device { class DeviceManager; diff --git a/cmvr-es/service/action/src/action_queue_executor.cpp b/cmvr-es/service/grpc/action/src/action_queue_executor.cpp similarity index 99% rename from cmvr-es/service/action/src/action_queue_executor.cpp rename to cmvr-es/service/grpc/action/src/action_queue_executor.cpp index 7a4508b6..07289f7f 100644 --- a/cmvr-es/service/action/src/action_queue_executor.cpp +++ b/cmvr-es/service/grpc/action/src/action_queue_executor.cpp @@ -1,4 +1,4 @@ -#include "service/action/include/action_queue_executor.h" +#include "service/grpc/action/include/action_queue_executor.h" #include #include @@ -36,10 +36,10 @@ #include "common/base/logging/logger.h" #include "devices/agv/abstract_agv.h" #include "devices/arm/robot_arm.h" -#include "manager/control_authority/include/control_authority_manager.h" +#include "manager/control_authority_manager/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" +#include "manager/safety_manager/include/safety_manager.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { namespace { @@ -1965,7 +1965,7 @@ struct ActionQueueExecutor::Impl { } request.authority_generation = token.generation; request.deadline = deadline; - return device_manager.safetyCoordinator().admit(request); + return device_manager.safetyManager().admit(request); } static std::string admissionFailure( @@ -2010,7 +2010,7 @@ struct ActionQueueExecutor::Impl { StepOutcome::Canceled, "RobotArm ActionQueue control was preempted before dispatch"}; } - auto safety_dispatch = device_manager.safetyCoordinator() + auto safety_dispatch = device_manager.safetyManager() .beginDispatch(*safety_admission.permit); if (!safety_dispatch.acquired()) { return { @@ -2171,7 +2171,7 @@ struct ActionQueueExecutor::Impl { StepOutcome::Canceled, "AGV ActionQueue control was preempted before dispatch"}; } - auto safety_dispatch = device_manager.safetyCoordinator() + auto safety_dispatch = device_manager.safetyManager() .beginDispatch(*safety_admission.permit); if (!safety_dispatch.acquired()) { return { diff --git a/cmvr-es/service/arm_teleop_client/CMakeLists.txt b/cmvr-es/service/grpc/client/CMakeLists.txt similarity index 100% rename from cmvr-es/service/arm_teleop_client/CMakeLists.txt rename to cmvr-es/service/grpc/client/CMakeLists.txt diff --git a/cmvr-es/service/arm_teleop_client/include/grpc_arm_teleop_client.h b/cmvr-es/service/grpc/client/include/grpc_arm_teleop_client.h similarity index 100% rename from cmvr-es/service/arm_teleop_client/include/grpc_arm_teleop_client.h rename to cmvr-es/service/grpc/client/include/grpc_arm_teleop_client.h diff --git a/cmvr-es/service/arm_teleop_client/src/grpc_arm_teleop_client.cpp b/cmvr-es/service/grpc/client/src/grpc_arm_teleop_client.cpp similarity index 98% rename from cmvr-es/service/arm_teleop_client/src/grpc_arm_teleop_client.cpp rename to cmvr-es/service/grpc/client/src/grpc_arm_teleop_client.cpp index 0c686937..5f12d043 100644 --- a/cmvr-es/service/arm_teleop_client/src/grpc_arm_teleop_client.cpp +++ b/cmvr-es/service/grpc/client/src/grpc_arm_teleop_client.cpp @@ -1,4 +1,4 @@ -#include "service/arm_teleop_client/include/grpc_arm_teleop_client.h" +#include "service/grpc/client/include/grpc_arm_teleop_client.h" #include #include diff --git a/cmvr-es/service/arm_teleop_client/tests/grpc_arm_teleop_client_test.cpp b/cmvr-es/service/grpc/client/tests/grpc_arm_teleop_client_test.cpp similarity index 98% rename from cmvr-es/service/arm_teleop_client/tests/grpc_arm_teleop_client_test.cpp rename to cmvr-es/service/grpc/client/tests/grpc_arm_teleop_client_test.cpp index 8be21e70..3e1caa7c 100644 --- a/cmvr-es/service/arm_teleop_client/tests/grpc_arm_teleop_client_test.cpp +++ b/cmvr-es/service/grpc/client/tests/grpc_arm_teleop_client_test.cpp @@ -12,7 +12,7 @@ #include #include "cmvr/api/arm_teleop_v1.grpc.pb.h" -#include "service/arm_teleop_client/include/grpc_arm_teleop_client.h" +#include "service/grpc/client/include/grpc_arm_teleop_client.h" namespace { diff --git a/cmvr-es/service/grpc/include/camera_operational_activity_registry.h b/cmvr-es/service/grpc/server/include/camera_operational_activity_registry.h similarity index 100% rename from cmvr-es/service/grpc/include/camera_operational_activity_registry.h rename to cmvr-es/service/grpc/server/include/camera_operational_activity_registry.h diff --git a/cmvr-es/service/grpc/include/camera_ptz_activity_registry.h b/cmvr-es/service/grpc/server/include/camera_ptz_activity_registry.h similarity index 100% rename from cmvr-es/service/grpc/include/camera_ptz_activity_registry.h rename to cmvr-es/service/grpc/server/include/camera_ptz_activity_registry.h diff --git a/cmvr-es/service/grpc/include/grpc_agv_service.h b/cmvr-es/service/grpc/server/include/grpc_agv_service.h similarity index 100% rename from cmvr-es/service/grpc/include/grpc_agv_service.h rename to cmvr-es/service/grpc/server/include/grpc_agv_service.h diff --git a/cmvr-es/service/grpc/include/grpc_arm_service.h b/cmvr-es/service/grpc/server/include/grpc_arm_service.h similarity index 100% rename from cmvr-es/service/grpc/include/grpc_arm_service.h rename to cmvr-es/service/grpc/server/include/grpc_arm_service.h diff --git a/cmvr-es/service/grpc/include/grpc_arm_teleop_service.h b/cmvr-es/service/grpc/server/include/grpc_arm_teleop_service.h similarity index 93% rename from cmvr-es/service/grpc/include/grpc_arm_teleop_service.h rename to cmvr-es/service/grpc/server/include/grpc_arm_teleop_service.h index 4e7eaa12..d368428d 100644 --- a/cmvr-es/service/grpc/include/grpc_arm_teleop_service.h +++ b/cmvr-es/service/grpc/server/include/grpc_arm_teleop_service.h @@ -9,7 +9,7 @@ #include #include "cmvr/api/arm_teleop_v1.grpc.pb.h" -#include "manager/control_authority/include/control_authority_manager.h" +#include "manager/control_authority_manager/include/control_authority_manager.h" namespace cmvr::service { @@ -18,7 +18,7 @@ class GrpcSecurityGateway; } // namespace cmvr::service namespace cmvr::safety { -class SafetyCoordinator; +class SafetyManager; } namespace cmvr::service { @@ -87,7 +87,7 @@ public: makeDisabledArmTeleopBackend(), control::ControlAuthorityManager* authority = nullptr, std::shared_ptr security_gateway = nullptr, - safety::SafetyCoordinator* safety_coordinator = nullptr); + safety::SafetyManager* safety_manager = nullptr); ~ArmTeleopServiceImpl() override = default; grpc::Status Teleoperate( @@ -99,7 +99,7 @@ private: std::shared_ptr backend_; control::ControlAuthorityManager* authority_{nullptr}; std::shared_ptr security_gateway_; - safety::SafetyCoordinator* safety_coordinator_{nullptr}; + safety::SafetyManager* safety_manager_{nullptr}; }; } // namespace cmvr::service diff --git a/cmvr-es/service/grpc/include/grpc_camera_service.h b/cmvr-es/service/grpc/server/include/grpc_camera_service.h similarity index 97% rename from cmvr-es/service/grpc/include/grpc_camera_service.h rename to cmvr-es/service/grpc/server/include/grpc_camera_service.h index c6c2f07d..799647e0 100644 --- a/cmvr-es/service/grpc/include/grpc_camera_service.h +++ b/cmvr-es/service/grpc/server/include/grpc_camera_service.h @@ -11,7 +11,7 @@ #include "common/base/grpc_utils.h" #include "manager/device_manager/include/device_manager.h" #include "devices/camera/abstract_camera.h" -#include "service/grpc/include/grpc_camera_stream_policy.h" +#include "service/grpc/server/include/grpc_camera_stream_policy.h" namespace cmvr::service { diff --git a/cmvr-es/service/grpc/include/grpc_camera_stream_policy.h b/cmvr-es/service/grpc/server/include/grpc_camera_stream_policy.h similarity index 100% rename from cmvr-es/service/grpc/include/grpc_camera_stream_policy.h rename to cmvr-es/service/grpc/server/include/grpc_camera_stream_policy.h diff --git a/cmvr-es/service/grpc/include/grpc_command_transaction.h b/cmvr-es/service/grpc/server/include/grpc_command_transaction.h similarity index 95% rename from cmvr-es/service/grpc/include/grpc_command_transaction.h rename to cmvr-es/service/grpc/server/include/grpc_command_transaction.h index 94818f26..5008ce21 100644 --- a/cmvr-es/service/grpc/include/grpc_command_transaction.h +++ b/cmvr-es/service/grpc/server/include/grpc_command_transaction.h @@ -11,8 +11,8 @@ #include #include "cmvr/api/common.pb.h" -#include "manager/safety/include/safety_coordinator.h" -#include "service/grpc/include/grpc_security.h" +#include "manager/safety_manager/include/safety_manager.h" +#include "service/grpc/server/include/grpc_security.h" namespace cmvr::service { @@ -38,7 +38,7 @@ struct GrpcStreamingSafetyOpen { class GrpcStreamingSafetySession final { public: GrpcStreamingSafetySession( - safety::SafetyCoordinator& coordinator, + safety::SafetyManager& coordinator, const GrpcRequestContext& request_context, GrpcStreamingSafetyOpen open); @@ -67,7 +67,7 @@ public: private: void reject_(safety::SafetyReason reason, std::string detail); - safety::SafetyCoordinator* coordinator_{nullptr}; + safety::SafetyManager* coordinator_{nullptr}; std::optional permit_; safety::AdmissionDecision admission_decision_; grpc::Status status_; @@ -79,7 +79,7 @@ private: class GrpcCommandTransaction final { public: GrpcCommandTransaction( - safety::SafetyCoordinator& coordinator, + safety::SafetyManager& coordinator, GrpcRequestContext request_context, GrpcMethodPolicy method_policy, const google::protobuf::Message& request, @@ -153,7 +153,7 @@ private: const std::string& detail); void abandon_() noexcept; - safety::SafetyCoordinator* coordinator_{nullptr}; + safety::SafetyManager* coordinator_{nullptr}; GrpcRequestContext request_context_; GrpcMethodPolicy method_policy_; google::protobuf::Message* response_{nullptr}; @@ -181,7 +181,7 @@ using GrpcUnaryCommandOperation = grpc::Status executeRegisteredGrpcCommand( const std::shared_ptr& gateway, grpc::ServerContext* server_context, - safety::SafetyCoordinator& coordinator, + safety::SafetyManager& coordinator, const std::string& full_method_name, const google::protobuf::Message* request, google::protobuf::Message* response, @@ -194,7 +194,7 @@ grpc::Status executeRegisteredGrpcCommand( grpc::Status executeServerDerivedGrpcCommand( const std::shared_ptr& gateway, grpc::ServerContext* server_context, - safety::SafetyCoordinator& coordinator, + safety::SafetyManager& coordinator, const std::string& full_method_name, GrpcMethodPolicy effective_policy, const google::protobuf::Message* request, diff --git a/cmvr-es/service/grpc/include/grpc_dexhand_service.h b/cmvr-es/service/grpc/server/include/grpc_dexhand_service.h similarity index 100% rename from cmvr-es/service/grpc/include/grpc_dexhand_service.h rename to cmvr-es/service/grpc/server/include/grpc_dexhand_service.h diff --git a/cmvr-es/service/grpc/include/grpc_error_logging_interceptor.h b/cmvr-es/service/grpc/server/include/grpc_error_logging_interceptor.h similarity index 100% rename from cmvr-es/service/grpc/include/grpc_error_logging_interceptor.h rename to cmvr-es/service/grpc/server/include/grpc_error_logging_interceptor.h diff --git a/cmvr-es/service/grpc/include/grpc_head_service.h b/cmvr-es/service/grpc/server/include/grpc_head_service.h similarity index 100% rename from cmvr-es/service/grpc/include/grpc_head_service.h rename to cmvr-es/service/grpc/server/include/grpc_head_service.h diff --git a/cmvr-es/service/grpc/include/grpc_hlc_service.h b/cmvr-es/service/grpc/server/include/grpc_hlc_service.h similarity index 100% rename from cmvr-es/service/grpc/include/grpc_hlc_service.h rename to cmvr-es/service/grpc/server/include/grpc_hlc_service.h diff --git a/cmvr-es/service/grpc/include/grpc_microphone_service.h b/cmvr-es/service/grpc/server/include/grpc_microphone_service.h similarity index 100% rename from cmvr-es/service/grpc/include/grpc_microphone_service.h rename to cmvr-es/service/grpc/server/include/grpc_microphone_service.h diff --git a/cmvr-es/service/grpc/include/grpc_motor_service.h b/cmvr-es/service/grpc/server/include/grpc_motor_service.h similarity index 99% rename from cmvr-es/service/grpc/include/grpc_motor_service.h rename to cmvr-es/service/grpc/server/include/grpc_motor_service.h index bbcd54f4..8d7a11d8 100644 --- a/cmvr-es/service/grpc/include/grpc_motor_service.h +++ b/cmvr-es/service/grpc/server/include/grpc_motor_service.h @@ -10,7 +10,7 @@ #include "cmvr/api/motor_service.grpc.pb.h" #include "devices/motor/abstract_motor.h" -#include "service/grpc/include/motor_activity_coordinator.h" +#include "service/grpc/server/include/motor_activity_coordinator.h" namespace cmvr::device { class DeviceManager; diff --git a/cmvr-es/service/grpc/include/grpc_recovery_audit.h b/cmvr-es/service/grpc/server/include/grpc_recovery_audit.h similarity index 100% rename from cmvr-es/service/grpc/include/grpc_recovery_audit.h rename to cmvr-es/service/grpc/server/include/grpc_recovery_audit.h diff --git a/cmvr-es/service/grpc/include/grpc_robot_arm_teleop_backend.h b/cmvr-es/service/grpc/server/include/grpc_robot_arm_teleop_backend.h similarity index 89% rename from cmvr-es/service/grpc/include/grpc_robot_arm_teleop_backend.h rename to cmvr-es/service/grpc/server/include/grpc_robot_arm_teleop_backend.h index c15c3562..39d241ba 100644 --- a/cmvr-es/service/grpc/include/grpc_robot_arm_teleop_backend.h +++ b/cmvr-es/service/grpc/server/include/grpc_robot_arm_teleop_backend.h @@ -4,7 +4,7 @@ #include "cmvr/config/grpc_server_config/grpc_server_config.pb.h" #include "devices/arm/robot_arm.h" -#include "service/grpc/include/grpc_arm_teleop_service.h" +#include "service/grpc/server/include/grpc_arm_teleop_service.h" namespace cmvr::service { diff --git a/cmvr-es/service/grpc/include/grpc_safety_participants.h b/cmvr-es/service/grpc/server/include/grpc_safety_participants.h similarity index 92% rename from cmvr-es/service/grpc/include/grpc_safety_participants.h rename to cmvr-es/service/grpc/server/include/grpc_safety_participants.h index d3fe5328..bb4252b4 100644 --- a/cmvr-es/service/grpc/include/grpc_safety_participants.h +++ b/cmvr-es/service/grpc/server/include/grpc_safety_participants.h @@ -3,7 +3,7 @@ #include namespace cmvr::safety { -class SafetyCoordinator; +class SafetyManager; } namespace cmvr::service { @@ -26,7 +26,7 @@ public: private: friend std::unique_ptr registerGrpcSafetyParticipants( - safety::SafetyCoordinator&, + safety::SafetyManager&, std::shared_ptr, std::shared_ptr); struct Impl; @@ -36,7 +36,7 @@ private: std::unique_ptr registerGrpcSafetyParticipants( - safety::SafetyCoordinator& coordinator, + safety::SafetyManager& coordinator, std::shared_ptr action_queue, std::shared_ptr stop_dispatcher); diff --git a/cmvr-es/service/grpc/include/grpc_safety_proto.h b/cmvr-es/service/grpc/server/include/grpc_safety_proto.h similarity index 95% rename from cmvr-es/service/grpc/include/grpc_safety_proto.h rename to cmvr-es/service/grpc/server/include/grpc_safety_proto.h index 68515d9f..c5bbae99 100644 --- a/cmvr-es/service/grpc/include/grpc_safety_proto.h +++ b/cmvr-es/service/grpc/server/include/grpc_safety_proto.h @@ -1,7 +1,7 @@ #pragma once #include "cmvr/api/safety_command.pb.h" -#include "manager/safety/include/safety_coordinator.h" +#include "manager/safety_manager/include/safety_manager.h" namespace cmvr::service { diff --git a/cmvr-es/service/grpc/include/grpc_security.h b/cmvr-es/service/grpc/server/include/grpc_security.h similarity index 99% rename from cmvr-es/service/grpc/include/grpc_security.h rename to cmvr-es/service/grpc/server/include/grpc_security.h index 9ed1cf01..9e8de77f 100644 --- a/cmvr-es/service/grpc/include/grpc_security.h +++ b/cmvr-es/service/grpc/server/include/grpc_security.h @@ -13,7 +13,7 @@ #include #include "cmvr/config/grpc_server_config/grpc_server_config.pb.h" -#include "manager/safety/include/safety_types.h" +#include "manager/safety_manager/include/safety_types.h" namespace cmvr::service { diff --git a/cmvr-es/service/grpc/include/grpc_speaker_service.h b/cmvr-es/service/grpc/server/include/grpc_speaker_service.h similarity index 100% rename from cmvr-es/service/grpc/include/grpc_speaker_service.h rename to cmvr-es/service/grpc/server/include/grpc_speaker_service.h diff --git a/cmvr-es/service/grpc/include/grpc_system_service.h b/cmvr-es/service/grpc/server/include/grpc_system_service.h similarity index 100% rename from cmvr-es/service/grpc/include/grpc_system_service.h rename to cmvr-es/service/grpc/server/include/grpc_system_service.h diff --git a/cmvr-es/service/grpc/include/media_activity_coordinator.h b/cmvr-es/service/grpc/server/include/media_activity_coordinator.h similarity index 98% rename from cmvr-es/service/grpc/include/media_activity_coordinator.h rename to cmvr-es/service/grpc/server/include/media_activity_coordinator.h index d234c0b4..b9f0c4d9 100644 --- a/cmvr-es/service/grpc/include/media_activity_coordinator.h +++ b/cmvr-es/service/grpc/server/include/media_activity_coordinator.h @@ -8,7 +8,7 @@ #include #include -#include "service/stop_all/include/deferred_stop_operation.h" +#include "service/grpc/stop_all/include/deferred_stop_operation.h" namespace cmvr::service { diff --git a/cmvr-es/service/grpc/include/motor_activity_coordinator.h b/cmvr-es/service/grpc/server/include/motor_activity_coordinator.h similarity index 98% rename from cmvr-es/service/grpc/include/motor_activity_coordinator.h rename to cmvr-es/service/grpc/server/include/motor_activity_coordinator.h index 6692482b..b3c14483 100644 --- a/cmvr-es/service/grpc/include/motor_activity_coordinator.h +++ b/cmvr-es/service/grpc/server/include/motor_activity_coordinator.h @@ -9,7 +9,7 @@ #include #include -#include "service/stop_all/include/deferred_stop_operation.h" +#include "service/grpc/stop_all/include/deferred_stop_operation.h" namespace cmvr::service { diff --git a/cmvr-es/service/grpc/src/camera_operational_activity_registry.cpp b/cmvr-es/service/grpc/server/src/camera_operational_activity_registry.cpp similarity index 98% rename from cmvr-es/service/grpc/src/camera_operational_activity_registry.cpp rename to cmvr-es/service/grpc/server/src/camera_operational_activity_registry.cpp index 32e769a8..55607cb7 100644 --- a/cmvr-es/service/grpc/src/camera_operational_activity_registry.cpp +++ b/cmvr-es/service/grpc/server/src/camera_operational_activity_registry.cpp @@ -1,9 +1,9 @@ -#include "service/grpc/include/camera_operational_activity_registry.h" +#include "service/grpc/server/include/camera_operational_activity_registry.h" #include #include -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { namespace { diff --git a/cmvr-es/service/grpc/src/camera_ptz_activity_registry.cpp b/cmvr-es/service/grpc/server/src/camera_ptz_activity_registry.cpp similarity index 98% rename from cmvr-es/service/grpc/src/camera_ptz_activity_registry.cpp rename to cmvr-es/service/grpc/server/src/camera_ptz_activity_registry.cpp index a1a50f5e..609c0807 100644 --- a/cmvr-es/service/grpc/src/camera_ptz_activity_registry.cpp +++ b/cmvr-es/service/grpc/server/src/camera_ptz_activity_registry.cpp @@ -1,10 +1,10 @@ -#include "service/grpc/include/camera_ptz_activity_registry.h" +#include "service/grpc/server/include/camera_ptz_activity_registry.h" #include #include #include -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { diff --git a/cmvr-es/service/grpc/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/server/src/grpc_agv_service.cpp similarity index 97% rename from cmvr-es/service/grpc/src/grpc_agv_service.cpp rename to cmvr-es/service/grpc/server/src/grpc_agv_service.cpp index c04ac789..319b9455 100644 --- a/cmvr-es/service/grpc/src/grpc_agv_service.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_agv_service.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_agv_service.h" +#include "service/grpc/server/include/grpc_agv_service.h" #include #include @@ -11,10 +11,10 @@ #include #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" +#include "manager/control_authority_manager/include/control_authority_manager.h" +#include "service/grpc/server/include/grpc_command_transaction.h" +#include "service/grpc/server/include/grpc_security.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" using google::protobuf::util::TimeUtil; @@ -668,7 +668,7 @@ grpc::Status gRPCAgvServiceImpl::emergencyStop(grpc::ServerContext* context, api::CommandHeader_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.AgvService/emergencyStop", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); @@ -696,7 +696,7 @@ grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext* context, api::CommandHeader_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.AgvService/clearFault", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); @@ -727,7 +727,7 @@ grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext* context, api::AgvNavigateToPoseCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.AgvService/navigateToPose", request, response, [this, context, request, response](GrpcCommandTransaction& command) { if (context && context->IsCancelled()) { @@ -766,7 +766,7 @@ grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext* context, api::AgvNavigateToStationCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.AgvService/navigateToStation", request, response, [this, context, request, response](GrpcCommandTransaction& command) { if (context && context->IsCancelled()) { @@ -806,7 +806,7 @@ grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext* context, api::AgvFollowPathCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.AgvService/followPath", request, response, [this, context, request, response](GrpcCommandTransaction& command) { if (context && context->IsCancelled()) { @@ -854,7 +854,7 @@ grpc::Status gRPCAgvServiceImpl::translate( api::AgvTranslateCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.AgvService/translate", request, response, [this, context, request, response](GrpcCommandTransaction& command) { if (context && context->IsCancelled()) { @@ -892,7 +892,7 @@ grpc::Status gRPCAgvServiceImpl::pauseNavigation(grpc::ServerContext* context, api::CommandHeader_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.AgvService/pauseNavigation", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); @@ -924,7 +924,7 @@ grpc::Status gRPCAgvServiceImpl::resumeNavigation(grpc::ServerContext* context, api::CommandHeader_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.AgvService/resumeNavigation", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); @@ -956,7 +956,7 @@ grpc::Status gRPCAgvServiceImpl::cancelNavigation(grpc::ServerContext* context, api::CommandHeader_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.AgvService/cancelNavigation", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); @@ -984,7 +984,7 @@ grpc::Status gRPCAgvServiceImpl::setVelocity(grpc::ServerContext* context, api::AgvSetVelocityCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.AgvService/setVelocity", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); @@ -1015,7 +1015,7 @@ grpc::Status gRPCAgvServiceImpl::stopVelocityControl(grpc::ServerContext* contex api::CommandHeader_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.AgvService/stopVelocityControl", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); @@ -1095,7 +1095,7 @@ grpc::Status gRPCAgvServiceImpl::switchMap(grpc::ServerContext* context, api::AgvMapCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.AgvService/switchMap", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); @@ -1126,7 +1126,7 @@ grpc::Status gRPCAgvServiceImpl::uploadMap(grpc::ServerContext* context, api::AgvMapCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.AgvService/uploadMap", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); @@ -1181,7 +1181,7 @@ grpc::Status gRPCAgvServiceImpl::startMapping(grpc::ServerContext* context, api::AgvStartMappingCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.AgvService/startMapping", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); @@ -1292,7 +1292,7 @@ grpc::Status gRPCAgvServiceImpl::stopMapping(grpc::ServerContext* context, api::CommandHeader_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.AgvService/stopMapping", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/server/src/grpc_arm_service.cpp similarity index 97% rename from cmvr-es/service/grpc/src/grpc_arm_service.cpp rename to cmvr-es/service/grpc/server/src/grpc_arm_service.cpp index 8ffda776..3c04f608 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_arm_service.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_arm_service.h" +#include "service/grpc/server/include/grpc_arm_service.h" #include #include @@ -7,10 +7,10 @@ #include #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" +#include "manager/control_authority_manager/include/control_authority_manager.h" +#include "service/grpc/server/include/grpc_command_transaction.h" +#include "service/grpc/server/include/grpc_security.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" using google::protobuf::util::TimeUtil; @@ -381,7 +381,7 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext* context, api::CommandHeader_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.ArmService/torqueOff", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); @@ -415,7 +415,7 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext* context, api::CommandHeader_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.ArmService/torqueOn", request, response, [this, context, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); @@ -471,7 +471,7 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext* context, api::MoveJ_Response* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.ArmService/moveJ", request, response, [this, context, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); @@ -511,7 +511,7 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext* context, api::MoveL_Response* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.ArmService/moveL", request, response, [this, context, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); @@ -553,7 +553,7 @@ grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext* context, api::SpeedJ_Response* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.ArmService/speedJ", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); @@ -593,7 +593,7 @@ grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext* context, api::SpeedL_Response* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.ArmService/speedL", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); @@ -634,7 +634,7 @@ grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext* context, api::ServoJ_Response* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.ArmService/servoJ", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); @@ -670,7 +670,7 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext* context, api::CommandHeader_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.ArmService/stopMotion", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); @@ -763,7 +763,7 @@ grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext* context, api::CalibrateZeroQ_Response* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.ArmService/calibrateZeroQ", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); @@ -821,7 +821,7 @@ grpc::Status gRPCArmServiceImpl::ExecuteJsonCommand( api::JsonDeviceCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.ArmService/ExecuteJsonCommand", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->header().device_id(); @@ -870,7 +870,7 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context, cmvr::api::CommandHeader_Feedback *response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.ArmService/clearFault", request, response, [this, request, response](GrpcCommandTransaction& command) { const std::string device_id = request->device_id(); diff --git a/cmvr-es/service/grpc/src/grpc_arm_teleop_service.cpp b/cmvr-es/service/grpc/server/src/grpc_arm_teleop_service.cpp similarity index 99% rename from cmvr-es/service/grpc/src/grpc_arm_teleop_service.cpp rename to cmvr-es/service/grpc/server/src/grpc_arm_teleop_service.cpp index 486659ed..b2f5d809 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_teleop_service.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_arm_teleop_service.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_arm_teleop_service.h" +#include "service/grpc/server/include/grpc_arm_teleop_service.h" #include #include @@ -14,9 +14,9 @@ #include #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" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/server/include/grpc_command_transaction.h" +#include "service/grpc/server/include/grpc_security.h" namespace cmvr::service { @@ -440,7 +440,7 @@ ArmTeleopServiceImpl::ArmTeleopServiceImpl( std::shared_ptr backend, control::ControlAuthorityManager* authority, std::shared_ptr security_gateway, - safety::SafetyCoordinator* safety_coordinator) + safety::SafetyManager* safety_manager) : backend_(std::move(backend)), authority_( authority ? authority @@ -448,7 +448,7 @@ ArmTeleopServiceImpl::ArmTeleopServiceImpl( security_gateway_(security_gateway ? std::move(security_gateway) : makeDefaultGrpcSecurityGateway()), - safety_coordinator_(safety_coordinator) + safety_manager_(safety_manager) { if (!backend_) { backend_ = makeDisabledArmTeleopBackend(); @@ -541,7 +541,7 @@ grpc::Status ArmTeleopServiceImpl::Teleoperate( }); std::optional safety_session; - if (safety_coordinator_) { + if (safety_manager_) { GrpcStreamingSafetyOpen safety_open; safety_open.full_method_name = "/cmvr.api.armteleop.v1.ArmTeleopService/Teleoperate"; @@ -550,7 +550,7 @@ grpc::Status ArmTeleopServiceImpl::Teleoperate( safety_open.authority_generation = control_lease.generation; safety_open.deadline = cmvr_grpc_call_guard.context().deadline; safety_session.emplace( - *safety_coordinator_, + *safety_manager_, cmvr_grpc_call_guard.context(), std::move(safety_open)); if (!safety_session->admitted()) { diff --git a/cmvr-es/service/grpc/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/server/src/grpc_camera_service.cpp similarity index 97% rename from cmvr-es/service/grpc/src/grpc_camera_service.cpp rename to cmvr-es/service/grpc/server/src/grpc_camera_service.cpp index 4c590814..3c7bc9b9 100644 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_camera_service.cpp @@ -1,10 +1,10 @@ #include "common/base/logging/logger.h" -#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" +#include "manager/media_source_manager/include/device_media_source_adapter.h" +#include "service/grpc/server/include/camera_operational_activity_registry.h" +#include "service/grpc/server/include/camera_ptz_activity_registry.h" +#include "service/grpc/server/include/grpc_command_transaction.h" +#include "service/grpc/server/include/media_activity_coordinator.h" +#include "service/grpc/server/include/grpc_security.h" // // Created by xtkuang on 2025/6/1. // @@ -72,7 +72,7 @@ bool toPtzCommand(cmvr::api::ControlPtzCommand_Command command, PtzCommand& out) } // The legacy depth/RGBD RPCs acquire the camera's shared producer directly -// instead of going through MediaSourceHub. Keep that lease exception-safe: +// instead of going through MediaSourceManager. Keep that lease exception-safe: // cancellation, a failed Write(), or any conversion error must release exactly // the one startStreaming() reference acquired by this call. class CameraStreamingLease final { @@ -80,7 +80,7 @@ public: CameraStreamingLease( std::shared_ptr camera, const MediaActivityCoordinator::Session& session, - cmvr::safety::SafetyCoordinator& coordinator) + cmvr::safety::SafetyManager& coordinator) : camera_(std::move(camera)) { (void)session.runIfCurrent([this, &coordinator] { auto dispatch = cmvr::media::beginMediaSourceStartDispatch( @@ -184,7 +184,7 @@ grpc::Status gRPCCameraServiceImpl::StartCamera(grpc::ServerContext* context, const api::StartCameraCommand_Request* request, api::StartCameraCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.CameraService/StartCamera", request, response, [this, request, response](GrpcCommandTransaction& command) { auto media_session = globalMediaActivityCoordinator().beginSession(); @@ -234,7 +234,7 @@ grpc::Status gRPCCameraServiceImpl::StopCamera(grpc::ServerContext* context, const api::StopCameraCommand_Request* request, api::StopCameraCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.CameraService/StopCamera", request, response, [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); @@ -502,7 +502,7 @@ grpc::Status gRPCCameraServiceImpl::StartRecording(grpc::ServerContext* context, const api::StartCameraRecordingCommand_Request* request, api::StartCameraRecordingCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.CameraService/StartRecording", request, response, [this, request, response](GrpcCommandTransaction& command) { auto media_session = globalMediaActivityCoordinator().beginSession(); @@ -540,7 +540,7 @@ grpc::Status gRPCCameraServiceImpl::StopRecording(grpc::ServerContext* context, const api::StopCameraRecordingCommand_Request* request, api::StopCameraRecordingCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.CameraService/StopRecording", request, response, [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); @@ -581,7 +581,7 @@ grpc::Status gRPCCameraServiceImpl::ControlPtz(grpc::ServerContext* context, } return executeServerDerivedGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.CameraService/ControlPtz", std::move(effective_policy), request, response, [this, request, response](GrpcCommandTransaction& command_tx) { @@ -666,7 +666,7 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con return grpc::Status::OK; } CameraStreamingLease stream_lease( - dev, media_session, dmgr_.safetyCoordinator()); + dev, media_session, dmgr_.safetyManager()); if (!stream_lease) { api::GetDepthImageStreamCommand_Feedback response; response.mutable_header()->set_success(false); @@ -763,7 +763,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con return grpc::Status::OK; } CameraStreamingLease stream_lease( - dev, media_session, dmgr_.safetyCoordinator()); + dev, media_session, dmgr_.safetyManager()); if (!stream_lease) { api::GetRGBDImagesStreamCommand_Feedback response; response.mutable_header()->set_success(false); @@ -867,7 +867,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte stream->Write(response); return grpc::Status::OK; } - auto& media_hub = cmvr::media::globalMediaSourceHub(); + auto& media_hub = cmvr::media::globalMediaSourceManager(); const std::string track_id = cmvr::media::cameraColorTrackId(dev_id); bool source_ready = false; const bool source_setup_allowed = media_session.runIfCurrent([&] { @@ -885,15 +885,15 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte return grpc::Status::OK; } auto source_dispatch = cmvr::media::beginMediaSourceStartDispatch( - dmgr_.safetyCoordinator(), dev_id); + dmgr_.safetyManager(), dev_id); auto subscription = source_dispatch.acquired() ? media_hub.subscribe( track_id, - cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED, + cmvr::media::MediaSourceManager::StartPosition::NEXT_PUBLISHED, [context, &media_session] { return context->IsCancelled() || media_session.cancelled(); }) - : cmvr::media::MediaSourceHub::Subscription{}; + : cmvr::media::MediaSourceManager::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/server/src/grpc_command_transaction.cpp similarity index 99% rename from cmvr-es/service/grpc/src/grpc_command_transaction.cpp rename to cmvr-es/service/grpc/server/src/grpc_command_transaction.cpp index f097ea11..bba5b75e 100644 --- a/cmvr-es/service/grpc/src/grpc_command_transaction.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_command_transaction.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_command_transaction.h" +#include "service/grpc/server/include/grpc_command_transaction.h" #include #include @@ -15,7 +15,7 @@ #include #include -#include "service/grpc/include/grpc_safety_proto.h" +#include "service/grpc/server/include/grpc_safety_proto.h" namespace cmvr::service { @@ -145,7 +145,7 @@ std::uint64_t stableHash( } bool isEnforced( - const safety::SafetyCoordinatorConfig& config, + const safety::SafetyManagerConfig& config, const std::string& device_id) { switch (config.enforcement_mode) { @@ -316,7 +316,7 @@ grpc::Status grpcStatusForSafetyReason( } GrpcStreamingSafetySession::GrpcStreamingSafetySession( - safety::SafetyCoordinator& coordinator, + safety::SafetyManager& coordinator, const GrpcRequestContext& request_context, GrpcStreamingSafetyOpen open) : coordinator_(&coordinator), @@ -469,7 +469,7 @@ std::string deterministicGrpcPayloadHash( } GrpcCommandTransaction::GrpcCommandTransaction( - safety::SafetyCoordinator& coordinator, + safety::SafetyManager& coordinator, GrpcRequestContext request_context, GrpcMethodPolicy method_policy, const google::protobuf::Message& request, @@ -1166,7 +1166,7 @@ void GrpcCommandTransaction::abandon_() noexcept grpc::Status executeRegisteredGrpcCommand( const std::shared_ptr& gateway, grpc::ServerContext* server_context, - safety::SafetyCoordinator& coordinator, + safety::SafetyManager& coordinator, const std::string& full_method_name, const google::protobuf::Message* request, google::protobuf::Message* response, @@ -1208,7 +1208,7 @@ grpc::Status executeRegisteredGrpcCommand( grpc::Status executeServerDerivedGrpcCommand( const std::shared_ptr& gateway, grpc::ServerContext* server_context, - safety::SafetyCoordinator& coordinator, + safety::SafetyManager& coordinator, const std::string& full_method_name, GrpcMethodPolicy effective_policy, const google::protobuf::Message* request, diff --git a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp b/cmvr-es/service/grpc/server/src/grpc_dexhand_service.cpp similarity index 98% rename from cmvr-es/service/grpc/src/grpc_dexhand_service.cpp rename to cmvr-es/service/grpc/server/src/grpc_dexhand_service.cpp index f2b2f2f1..0e69af20 100644 --- a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_dexhand_service.cpp @@ -15,11 +15,11 @@ #include #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" +#include "manager/control_authority_manager/include/control_authority_manager.h" +#include "service/grpc/server/include/grpc_command_transaction.h" +#include "service/grpc/server/include/media_activity_coordinator.h" +#include "service/grpc/server/include/grpc_security.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" using namespace std; using namespace cmvr::service; @@ -367,7 +367,7 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context , const cmvr::api::SetDexHandPositionsCommand_Request* request , cmvr::api::SetDexHandPositionsCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.DexHandService/SetDexHandPos", request, response, [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); @@ -415,7 +415,7 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* contex , const cmvr::api::SetDexHandAnglesCommand_Request* request , cmvr::api::SetDexHandAnglesCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.DexHandService/SetDexHandAngle", request, response, [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); @@ -477,7 +477,7 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* contex , const cmvr::api::SetDexHandForceCommand_Request* request , cmvr::api::SetDexHandForceCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.DexHandService/SetDexHandForce", request, response, [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); @@ -525,7 +525,7 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* contex , const cmvr::api::SetDexHandSpeedCommand_Request* request , cmvr::api::SetDexHandSpeedCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.DexHandService/SetDexHandSpeed", request, response, [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); @@ -573,7 +573,7 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPresetAct(grpc::ServerContext* co , const cmvr::api::SetDexHandPresetActCommand_Request* request , cmvr::api::SetDexHandPresetActCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.DexHandService/SetDexHandPresetAct", request, response, [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); diff --git a/cmvr-es/service/grpc/src/grpc_error_logging_interceptor.cpp b/cmvr-es/service/grpc/server/src/grpc_error_logging_interceptor.cpp similarity index 99% rename from cmvr-es/service/grpc/src/grpc_error_logging_interceptor.cpp rename to cmvr-es/service/grpc/server/src/grpc_error_logging_interceptor.cpp index ecbe3da9..d8a7352a 100644 --- a/cmvr-es/service/grpc/src/grpc_error_logging_interceptor.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_error_logging_interceptor.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_error_logging_interceptor.h" +#include "service/grpc/server/include/grpc_error_logging_interceptor.h" #include #include diff --git a/cmvr-es/service/grpc/src/grpc_head_service.cpp b/cmvr-es/service/grpc/server/src/grpc_head_service.cpp similarity index 98% rename from cmvr-es/service/grpc/src/grpc_head_service.cpp rename to cmvr-es/service/grpc/server/src/grpc_head_service.cpp index aa541d67..edfe5198 100644 --- a/cmvr-es/service/grpc/src/grpc_head_service.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_head_service.cpp @@ -5,10 +5,10 @@ #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 "service/grpc/server/include/grpc_command_transaction.h" +#include "service/grpc/server/include/media_activity_coordinator.h" +#include "service/grpc/server/include/grpc_security.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include #include #include @@ -65,7 +65,7 @@ grpc::Status executeHeadOperationalCommand( Operation&& operation) { return executeRegisteredGrpcCommand( - security_gateway, context, device_manager.safetyCoordinator(), + security_gateway, context, device_manager.safetyManager(), full_method_name, request, response, [&device_manager, request, response, rpc_name, operation = std::forward(operation)]( @@ -245,7 +245,7 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression( safety_open.deadline = cmvr_grpc_call_guard.context().deadline; safety_session.emplace( - dmgr_.safetyCoordinator(), + dmgr_.safetyManager(), cmvr_grpc_call_guard.context(), std::move(safety_open)); if (!safety_session->admitted()) { @@ -417,7 +417,7 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop( EmergencyStop_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.BioHeadService/EmergencyStop", request, response, [this, request, response](GrpcCommandTransaction& command) { const string dev_id = request->header().device_id(); @@ -460,7 +460,7 @@ grpc::Status gRPCMBioHeadServiceImpl::SpeakStart(grpc::ServerContext* context, c grpc::Status gRPCMBioHeadServiceImpl::SpeakStop(grpc::ServerContext* context, const cmvr::api::SpeakStop_Request* request, cmvr::api::SpeakStop_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.BioHeadService/SpeakStop", request, response, [this, request, response](GrpcCommandTransaction& command) { const string dev_id = request->header().device_id(); diff --git a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp b/cmvr-es/service/grpc/server/src/grpc_hlc_service.cpp similarity index 96% rename from cmvr-es/service/grpc/src/grpc_hlc_service.cpp rename to cmvr-es/service/grpc/server/src/grpc_hlc_service.cpp index 7051d4bf..e4e4d7f1 100644 --- a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_hlc_service.cpp @@ -14,9 +14,9 @@ #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 "service/grpc/server/include/grpc_command_transaction.h" +#include "service/grpc/server/include/grpc_security.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include "task/touch_screen_task/include/touch_screen_task.h" @@ -92,7 +92,7 @@ grpc::Status gRPCHlcServiceImpl::touch( normalized_request.mutable_header()->set_device_id(arm_id); return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.HlcService/touch", &normalized_request, response, [context, request, response, touch_task]( GrpcCommandTransaction& command) { diff --git a/cmvr-es/service/grpc/src/grpc_microphone_service.cpp b/cmvr-es/service/grpc/server/src/grpc_microphone_service.cpp similarity index 95% rename from cmvr-es/service/grpc/src/grpc_microphone_service.cpp rename to cmvr-es/service/grpc/server/src/grpc_microphone_service.cpp index 65ea67df..b18e9572 100644 --- a/cmvr-es/service/grpc/src/grpc_microphone_service.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_microphone_service.cpp @@ -1,8 +1,8 @@ #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 "manager/media_source_manager/include/device_media_source_adapter.h" +#include "service/grpc/server/include/grpc_command_transaction.h" +#include "service/grpc/server/include/media_activity_coordinator.h" +#include "service/grpc/server/include/grpc_security.h" #include #include #include @@ -84,7 +84,7 @@ grpc::Status gRPCMicroPhoneServiceImpl::GetStatus(grpc::ServerContext* context, grpc::Status gRPCMicroPhoneServiceImpl::StartRecord(grpc::ServerContext* context, const api::StartMicRecordingCommand_Request* request, api::StartMicRecordingCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.MicPhoneService/StartRecord", request, response, [this, request, response](GrpcCommandTransaction& command) { auto media_session = globalMediaActivityCoordinator().beginSession(); @@ -131,7 +131,7 @@ grpc::Status gRPCMicroPhoneServiceImpl::StartRecord(grpc::ServerContext* context grpc::Status gRPCMicroPhoneServiceImpl::StopRecord(grpc::ServerContext* context, const api::StopMicRecordingCommand_Request* request, api::StopMicRecordingCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.MicPhoneService/StopRecord", request, response, [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); @@ -154,7 +154,7 @@ grpc::Status gRPCMicroPhoneServiceImpl::StopRecord(grpc::ServerContext* context, grpc::Status gRPCMicroPhoneServiceImpl::PauseRecord(grpc::ServerContext* context, const api::PauseMicRecordingCommand_Request* request, api::PauseMicRecordingCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.MicPhoneService/PauseRecord", request, response, [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); @@ -177,7 +177,7 @@ grpc::Status gRPCMicroPhoneServiceImpl::PauseRecord(grpc::ServerContext* context grpc::Status gRPCMicroPhoneServiceImpl::ResumeRecord(grpc::ServerContext* context, const api::ResumeMicRecordingCommand_Request* request, api::ResumeMicRecordingCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.MicPhoneService/ResumeRecord", request, response, [this, request, response](GrpcCommandTransaction& command) { auto media_session = globalMediaActivityCoordinator().beginSession(); @@ -240,7 +240,7 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context return grpc::Status::OK; } - auto& media_hub = cmvr::media::globalMediaSourceHub(); + auto& media_hub = cmvr::media::globalMediaSourceManager(); const std::string track_id = cmvr::media::microphoneTrackId(dev_id); bool source_ready = false; const bool source_setup_allowed = media_session.runIfCurrent([&] { @@ -260,15 +260,15 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context } auto source_dispatch = cmvr::media::beginMediaSourceStartDispatch( - dmgr_.safetyCoordinator(), dev_id); + dmgr_.safetyManager(), dev_id); auto subscription = source_dispatch.acquired() ? media_hub.subscribe( track_id, - cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED, + cmvr::media::MediaSourceManager::StartPosition::NEXT_PUBLISHED, [context, &media_session] { return context->IsCancelled() || media_session.cancelled(); }) - : cmvr::media::MediaSourceHub::Subscription{}; + : cmvr::media::MediaSourceManager::Subscription{}; if (!subscription) { api::StreamMicAudioCommand_Feedback feedback; feedback.mutable_header()->set_success(false); @@ -356,7 +356,7 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context grpc::Status gRPCMicroPhoneServiceImpl::SetVolume(grpc::ServerContext* context, const api::SetMicPhoneVolumeCommand_Request* request, api::SetMicPhoneVolumeCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.MicPhoneService/SetVolume", request, response, [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); diff --git a/cmvr-es/service/grpc/src/grpc_motor_service.cpp b/cmvr-es/service/grpc/server/src/grpc_motor_service.cpp similarity index 99% rename from cmvr-es/service/grpc/src/grpc_motor_service.cpp rename to cmvr-es/service/grpc/server/src/grpc_motor_service.cpp index 183937ca..64b67c6c 100644 --- a/cmvr-es/service/grpc/src/grpc_motor_service.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_motor_service.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_motor_service.h" +#include "service/grpc/server/include/grpc_motor_service.h" #include #include @@ -15,9 +15,9 @@ #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" +#include "service/grpc/server/include/grpc_command_transaction.h" +#include "service/grpc/server/include/grpc_security.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { @@ -870,7 +870,7 @@ grpc::Status gRPCMotorServiceImpl::setZero( api::MotorCommandResponse* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.MotorService/setZero", request, response, [this, context, request, response](GrpcCommandTransaction& command) { return runUnaryGuarded( @@ -892,7 +892,7 @@ grpc::Status gRPCMotorServiceImpl::moveToZero( api::MotorCommandResponse* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.MotorService/moveToZero", request, response, [this, context, request, response](GrpcCommandTransaction& command) { return runUnaryGuarded( @@ -914,7 +914,7 @@ grpc::Status gRPCMotorServiceImpl::profilePosition( api::MotorCommandResponse* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.MotorService/profilePosition", request, response, [this, context, request, response](GrpcCommandTransaction& command) { return runUnaryGuarded( @@ -936,7 +936,7 @@ grpc::Status gRPCMotorServiceImpl::profileVelocity( api::MotorCommandResponse* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.MotorService/profileVelocity", request, response, [this, context, request, response](GrpcCommandTransaction& command) { return runUnaryGuarded( @@ -1004,7 +1004,7 @@ grpc::Status gRPCMotorServiceImpl::emergencyStop( api::MotorCommandResponse* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.MotorService/emergencyStop", request, response, [this, context, request, response](GrpcCommandTransaction& command) { return runUnaryGuarded( @@ -1039,7 +1039,7 @@ grpc::Status gRPCMotorServiceImpl::setEnabled( api::MotorCommandResponse* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.MotorService/setEnabled", request, response, [this, context, request, response](GrpcCommandTransaction& command) { return runUnaryGuarded( @@ -1821,7 +1821,7 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicPositionImpl( safety_open.authority_generation = generation; safety_open.deadline = request_context.deadline; GrpcStreamingSafetySession safety_session( - dmgr_.safetyCoordinator(), request_context, + dmgr_.safetyManager(), request_context, std::move(safety_open)); if (!safety_session.admitted()) { return safety_session.status(); @@ -1980,7 +1980,7 @@ grpc::Status gRPCMotorServiceImpl::streamCyclicVelocityImpl( safety_open.authority_generation = generation; safety_open.deadline = request_context.deadline; GrpcStreamingSafetySession safety_session( - dmgr_.safetyCoordinator(), request_context, + dmgr_.safetyManager(), request_context, std::move(safety_open)); if (!safety_session.admitted()) { return safety_session.status(); diff --git a/cmvr-es/service/grpc/src/grpc_recovery_audit.cpp b/cmvr-es/service/grpc/server/src/grpc_recovery_audit.cpp similarity index 98% rename from cmvr-es/service/grpc/src/grpc_recovery_audit.cpp rename to cmvr-es/service/grpc/server/src/grpc_recovery_audit.cpp index e3bd03f0..915b9188 100644 --- a/cmvr-es/service/grpc/src/grpc_recovery_audit.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_recovery_audit.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_recovery_audit.h" +#include "service/grpc/server/include/grpc_recovery_audit.h" #include #include diff --git a/cmvr-es/service/grpc/src/grpc_robot_arm_teleop_backend.cpp b/cmvr-es/service/grpc/server/src/grpc_robot_arm_teleop_backend.cpp similarity index 99% rename from cmvr-es/service/grpc/src/grpc_robot_arm_teleop_backend.cpp rename to cmvr-es/service/grpc/server/src/grpc_robot_arm_teleop_backend.cpp index d839718c..e8527717 100644 --- a/cmvr-es/service/grpc/src/grpc_robot_arm_teleop_backend.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_robot_arm_teleop_backend.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_robot_arm_teleop_backend.h" +#include "service/grpc/server/include/grpc_robot_arm_teleop_backend.h" #include #include diff --git a/cmvr-es/service/grpc/src/grpc_safety_participants.cpp b/cmvr-es/service/grpc/server/src/grpc_safety_participants.cpp similarity index 97% rename from cmvr-es/service/grpc/src/grpc_safety_participants.cpp rename to cmvr-es/service/grpc/server/src/grpc_safety_participants.cpp index e0f74fc7..a7e39b2e 100644 --- a/cmvr-es/service/grpc/src/grpc_safety_participants.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_safety_participants.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_safety_participants.h" +#include "service/grpc/server/include/grpc_safety_participants.h" #include #include @@ -12,16 +12,16 @@ #include #include -#include "manager/media_source_hub/include/device_media_source_adapter.h" -#include "manager/safety/include/safety_coordinator.h" +#include "manager/media_source_manager/include/device_media_source_adapter.h" +#include "manager/safety_manager/include/safety_manager.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" +#include "service/grpc/action/include/action_queue_executor.h" +#include "service/grpc/server/include/camera_operational_activity_registry.h" +#include "service/grpc/server/include/camera_ptz_activity_registry.h" +#include "service/grpc/server/include/media_activity_coordinator.h" +#include "service/grpc/server/include/motor_activity_coordinator.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_operation_dispatcher.h" namespace cmvr::service { @@ -966,7 +966,7 @@ std::vector mediaSourceStopOperations() const auto merge = [&unique_ids](const std::vector& ids) { unique_ids.insert(ids.begin(), ids.end()); }; - merge(media::globalMediaSourceHub().trackedSourceIds()); + merge(media::globalMediaSourceManager().trackedSourceIds()); merge(globalCameraOperationalActivityRegistry().trackedDeviceIds()); std::vector operations; @@ -976,7 +976,7 @@ std::vector mediaSourceStopOperations() "media-source:" + id, [id] { std::vector failures; - bool stopped = media::globalMediaSourceHub() + bool stopped = media::globalMediaSourceManager() .stopSourcesForDevice(id, &failures); if (!globalCameraOperationalActivityRegistry() .stopActivitiesForDevice(id, &failures)) { @@ -996,11 +996,11 @@ struct SharedParticipants { }; std::mutex shared_participants_mutex; -std::unordered_map +std::unordered_map shared_participants; void acquireSharedParticipants( - safety::SafetyCoordinator& coordinator, + safety::SafetyManager& coordinator, const std::shared_ptr& dispatcher) { std::lock_guard lock(shared_participants_mutex); @@ -1057,7 +1057,7 @@ void acquireSharedParticipants( shared_participants.emplace(&coordinator, std::move(entry)); } -void releaseSharedParticipants(safety::SafetyCoordinator& coordinator) noexcept +void releaseSharedParticipants(safety::SafetyManager& coordinator) noexcept { try { std::lock_guard lock(shared_participants_mutex); @@ -1078,7 +1078,7 @@ void releaseSharedParticipants(safety::SafetyCoordinator& coordinator) noexcept } // namespace struct GrpcSafetyParticipantRegistration::Impl { - safety::SafetyCoordinator* coordinator{nullptr}; + safety::SafetyManager* coordinator{nullptr}; std::string action_participant_id; bool shared_acquired{false}; @@ -1112,7 +1112,7 @@ GrpcSafetyParticipantRegistration::operator=( std::unique_ptr registerGrpcSafetyParticipants( - safety::SafetyCoordinator& coordinator, + safety::SafetyManager& coordinator, std::shared_ptr action_queue, std::shared_ptr stop_dispatcher) { diff --git a/cmvr-es/service/grpc/src/grpc_safety_proto.cpp b/cmvr-es/service/grpc/server/src/grpc_safety_proto.cpp similarity index 99% rename from cmvr-es/service/grpc/src/grpc_safety_proto.cpp rename to cmvr-es/service/grpc/server/src/grpc_safety_proto.cpp index 4f270c4f..fc5347da 100644 --- a/cmvr-es/service/grpc/src/grpc_safety_proto.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_safety_proto.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_safety_proto.h" +#include "service/grpc/server/include/grpc_safety_proto.h" #include diff --git a/cmvr-es/service/grpc/src/grpc_security.cpp b/cmvr-es/service/grpc/server/src/grpc_security.cpp similarity index 99% rename from cmvr-es/service/grpc/src/grpc_security.cpp rename to cmvr-es/service/grpc/server/src/grpc_security.cpp index c802037a..78593af9 100644 --- a/cmvr-es/service/grpc/src/grpc_security.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_security.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_security.h" +#include "service/grpc/server/include/grpc_security.h" #include #include diff --git a/cmvr-es/service/grpc/src/grpc_speaker_service.cpp b/cmvr-es/service/grpc/server/src/grpc_speaker_service.cpp similarity index 97% rename from cmvr-es/service/grpc/src/grpc_speaker_service.cpp rename to cmvr-es/service/grpc/server/src/grpc_speaker_service.cpp index eb113ab4..f9704fda 100644 --- a/cmvr-es/service/grpc/src/grpc_speaker_service.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_speaker_service.cpp @@ -1,7 +1,7 @@ #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 "service/grpc/server/include/grpc_command_transaction.h" +#include "service/grpc/server/include/media_activity_coordinator.h" +#include "service/grpc/server/include/grpc_security.h" #include #include #include @@ -147,7 +147,7 @@ grpc::Status gRPCSpeakerServiceImpl::GetStatus(grpc::ServerContext* context, grpc::Status gRPCSpeakerServiceImpl::PlayAudio(grpc::ServerContext* context, const api::PlayAudioCommand_Request* request, api::PlayAudioCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.SpeakerService/PlayAudio", request, response, [this, request, response](GrpcCommandTransaction& command) { auto media_session = globalMediaActivityCoordinator().beginSession(); @@ -240,7 +240,7 @@ grpc::Status gRPCSpeakerServiceImpl::StreamAudio(grpc::ServerContext* context, safety_open.deadline = cmvr_grpc_call_guard.context().deadline; safety_session.emplace( - dmgr_.safetyCoordinator(), + dmgr_.safetyManager(), cmvr_grpc_call_guard.context(), std::move(safety_open)); if (!safety_session->admitted()) { @@ -307,7 +307,7 @@ grpc::Status gRPCSpeakerServiceImpl::StreamAudio(grpc::ServerContext* context, grpc::Status gRPCSpeakerServiceImpl::StopPlayback(grpc::ServerContext* context, const api::StopSpeakerCommand_Request* request, api::StopSpeakerCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.SpeakerService/StopPlayback", request, response, [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); @@ -332,7 +332,7 @@ grpc::Status gRPCSpeakerServiceImpl::StopPlayback(grpc::ServerContext* context, grpc::Status gRPCSpeakerServiceImpl::PausePlayback(grpc::ServerContext* context, const api::PauseSpeakerCommand_Request* request, api::PauseSpeakerCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.SpeakerService/PausePlayback", request, response, [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); @@ -355,7 +355,7 @@ grpc::Status gRPCSpeakerServiceImpl::PausePlayback(grpc::ServerContext* context, grpc::Status gRPCSpeakerServiceImpl::ResumePlayback(grpc::ServerContext* context, const api::ResumeSpeakerCommand_Request* request, api::ResumeSpeakerCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.SpeakerService/ResumePlayback", request, response, [this, request, response](GrpcCommandTransaction& command) { auto media_session = globalMediaActivityCoordinator().beginSession(); @@ -396,7 +396,7 @@ grpc::Status gRPCSpeakerServiceImpl::ResumePlayback(grpc::ServerContext* context grpc::Status gRPCSpeakerServiceImpl::SetVolume(grpc::ServerContext* context, const api::SetSpeakerVolumeCommand_Request* request, api::SetSpeakerVolumeCommand_Feedback* response) { return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyCoordinator(), + security_gateway_, context, dmgr_.safetyManager(), "/cmvr.api.SpeakerService/SetVolume", request, response, [this, request, response](GrpcCommandTransaction& command) { string dev_id = request->header().device_id(); diff --git a/cmvr-es/service/grpc/src/grpc_system_service.cpp b/cmvr-es/service/grpc/server/src/grpc_system_service.cpp similarity index 97% rename from cmvr-es/service/grpc/src/grpc_system_service.cpp rename to cmvr-es/service/grpc/server/src/grpc_system_service.cpp index d3c57513..211d2f37 100644 --- a/cmvr-es/service/grpc/src/grpc_system_service.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_system_service.cpp @@ -27,20 +27,20 @@ #include "devices/gripper/abstract_gripper.h" #include "devices/microphone/abstract_microphone.h" #include "devices/speaker/abstract_speaker.h" -#include "manager/control_authority/include/control_authority_manager.h" -#include "manager/media_source_hub/include/device_media_source_adapter.h" +#include "manager/control_authority_manager/include/control_authority_manager.h" +#include "manager/media_source_manager/include/device_media_source_adapter.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/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" -#include "service/stop_all/include/stop_operation_dispatcher.h" +#include "service/grpc/action/include/action_queue_executor.h" +#include "service/grpc/server/include/camera_operational_activity_registry.h" +#include "service/grpc/server/include/camera_ptz_activity_registry.h" +#include "service/grpc/server/include/grpc_recovery_audit.h" +#include "service/grpc/server/include/grpc_safety_proto.h" +#include "service/grpc/server/include/grpc_safety_participants.h" +#include "service/grpc/server/include/grpc_security.h" +#include "service/grpc/server/include/media_activity_coordinator.h" +#include "service/grpc/server/include/motor_activity_coordinator.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_operation_dispatcher.h" using namespace cmvr::device; using namespace cmvr::service; @@ -718,7 +718,7 @@ StopOutcome stopCameraActivities( const std::shared_ptr& camera) { std::vector failures; - bool stopped = cmvr::media::globalMediaSourceHub() + bool stopped = cmvr::media::globalMediaSourceManager() .stopSourcesForDevice(device_id, &failures); if (!globalCameraPtzActivityRegistry().stopActivitiesForDevice( device_id, &failures)) { @@ -767,7 +767,7 @@ StopOutcome stopMicrophone( const std::shared_ptr& microphone) { std::vector failures; - bool stopped = cmvr::media::globalMediaSourceHub() + bool stopped = cmvr::media::globalMediaSourceManager() .stopSourcesForDevice(device_id, &failures); try { cmvr::device::MicrophoneState state{}; @@ -811,7 +811,7 @@ StopOutcome stopSpeaker( StopOutcome stopTrackedMediaActivities(const std::string& device_id) { std::vector failures; - bool stopped = cmvr::media::globalMediaSourceHub() + bool stopped = cmvr::media::globalMediaSourceManager() .stopSourcesForDevice(device_id, &failures); if (!globalCameraPtzActivityRegistry().stopActivitiesForDevice( device_id, &failures)) { @@ -1162,7 +1162,7 @@ gRPCSystemServiceImpl::gRPCSystemServiceImpl( acquireProcessStopDispatcher(stop_dispatcher_); try { safety_participant_registration_ = registerGrpcSafetyParticipants( - dmgr_.safetyCoordinator(), action_queue_, stop_dispatcher_); + dmgr_.safetyManager(), action_queue_, stop_dispatcher_); } catch (...) { releaseProcessStopDispatcher(stop_dispatcher_); throw; @@ -1217,7 +1217,7 @@ grpc::Status gRPCSystemServiceImpl::GetSystemInfo(grpc::ServerContext* context, toString(security.recovery_exposure)); response->set_grpc_insecure_non_loopback( security.insecure_non_loopback); - const auto safety = dmgr_.safetyCoordinator().snapshot(); + const auto safety = dmgr_.safetyManager().snapshot(); response->set_control_service_instance_id( safety.service_instance_id); response->set_safety_enforcement_mode( @@ -1365,7 +1365,7 @@ grpc::Status gRPCSystemServiceImpl::GetSafetyState( return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, scope.error); } - const auto snapshot = dmgr_.safetyCoordinator().snapshot(); + const auto snapshot = dmgr_.safetyManager().snapshot(); response->set_system_state( toApiSystemAdmissionState(snapshot.system_state)); response->set_safety_epoch(snapshot.safety_epoch); @@ -1486,7 +1486,7 @@ grpc::Status gRPCSystemServiceImpl::RecoverSafetyState( auto deadline = cmvr_grpc_call_guard.context().deadline; const auto configured_deadline = cmvr::safety::SafetyClock::now() + - dmgr_.safetyCoordinator().config().recovery_timeout; + dmgr_.safetyManager().config().recovery_timeout; if (deadline == cmvr::safety::SafetyClock::time_point::max() || configured_deadline < deadline) { deadline = configured_deadline; @@ -1528,7 +1528,7 @@ grpc::Status gRPCSystemServiceImpl::RecoverSafetyState( } const auto result = - dmgr_.safetyCoordinator().recover(coordinator_request); + dmgr_.safetyManager().recover(coordinator_request); response->set_recovery_id(result.recovery_id); response->set_result(toApiRecoveryResult(result.result)); response->set_previous_safety_epoch(result.previous_safety_epoch); @@ -1544,7 +1544,7 @@ grpc::Status gRPCSystemServiceImpl::RecoverSafetyState( header->set_success(success); header->set_command_id(request->recovery_id()); header->set_service_instance_id( - dmgr_.safetyCoordinator().serviceInstanceId()); + dmgr_.safetyManager().serviceInstanceId()); header->set_safety_epoch(result.current_safety_epoch); header->set_execution_state( success ? cmvr::api::COMMAND_EXECUTION_STATE_COMPLETED @@ -1596,7 +1596,7 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, grpc::StatusCode::INVALID_ARGUMENT, "StopAll request and response are required"); } - if (dmgr_.safetyCoordinator().config().enforcement_mode != + if (dmgr_.safetyManager().config().enforcement_mode != cmvr::safety::EnforcementMode::Legacy) { auto deadline = cmvr_grpc_call_guard.context().deadline; const auto configured_deadline = @@ -1616,7 +1616,7 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, if (operation_id.empty() && request->has_header()) { operation_id = request->header().command_id(); } - const auto result = dmgr_.safetyCoordinator().stopAll( + const auto result = dmgr_.safetyManager().stopAll( std::move(operation_id), deadline); response->set_operation_id(result.operation_id); response->set_previous_safety_epoch( @@ -1633,7 +1633,7 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, header->set_success(result.success); header->set_command_id(result.operation_id); header->set_service_instance_id( - dmgr_.safetyCoordinator().serviceInstanceId()); + dmgr_.safetyManager().serviceInstanceId()); header->set_safety_epoch(result.current_safety_epoch); header->set_execution_state( result.success @@ -1826,7 +1826,7 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, tracked_media_ids.insert(ids.begin(), ids.end()); }; merge_tracked_ids( - cmvr::media::globalMediaSourceHub().trackedSourceIds()); + cmvr::media::globalMediaSourceManager().trackedSourceIds()); merge_tracked_ids( globalCameraPtzActivityRegistry().trackedDeviceIds()); merge_tracked_ids( diff --git a/cmvr-es/service/grpc/src/media_activity_coordinator.cpp b/cmvr-es/service/grpc/server/src/media_activity_coordinator.cpp similarity index 99% rename from cmvr-es/service/grpc/src/media_activity_coordinator.cpp rename to cmvr-es/service/grpc/server/src/media_activity_coordinator.cpp index 8bd99123..2af439ea 100644 --- a/cmvr-es/service/grpc/src/media_activity_coordinator.cpp +++ b/cmvr-es/service/grpc/server/src/media_activity_coordinator.cpp @@ -1,7 +1,7 @@ -#include "service/grpc/include/media_activity_coordinator.h" +#include "service/grpc/server/include/media_activity_coordinator.h" #include "common/base/logging/logger.h" -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include #include diff --git a/cmvr-es/service/grpc/src/motor_activity_coordinator.cpp b/cmvr-es/service/grpc/server/src/motor_activity_coordinator.cpp similarity index 99% rename from cmvr-es/service/grpc/src/motor_activity_coordinator.cpp rename to cmvr-es/service/grpc/server/src/motor_activity_coordinator.cpp index aef287ae..9bab0d38 100644 --- a/cmvr-es/service/grpc/src/motor_activity_coordinator.cpp +++ b/cmvr-es/service/grpc/server/src/motor_activity_coordinator.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/motor_activity_coordinator.h" +#include "service/grpc/server/include/motor_activity_coordinator.h" #include #include diff --git a/cmvr-es/service/grpc/tests/camera_operational_activity_registry_test.cpp b/cmvr-es/service/grpc/server/tests/camera_operational_activity_registry_test.cpp similarity index 99% rename from cmvr-es/service/grpc/tests/camera_operational_activity_registry_test.cpp rename to cmvr-es/service/grpc/server/tests/camera_operational_activity_registry_test.cpp index 79b52385..973fdd9e 100644 --- a/cmvr-es/service/grpc/tests/camera_operational_activity_registry_test.cpp +++ b/cmvr-es/service/grpc/server/tests/camera_operational_activity_registry_test.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/camera_operational_activity_registry.h" +#include "service/grpc/server/include/camera_operational_activity_registry.h" #include #include @@ -11,7 +11,7 @@ #include -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { namespace { diff --git a/cmvr-es/service/grpc/tests/camera_ptz_activity_registry_test.cpp b/cmvr-es/service/grpc/server/tests/camera_ptz_activity_registry_test.cpp similarity index 98% rename from cmvr-es/service/grpc/tests/camera_ptz_activity_registry_test.cpp rename to cmvr-es/service/grpc/server/tests/camera_ptz_activity_registry_test.cpp index 272e3ecf..eec9ebe2 100644 --- a/cmvr-es/service/grpc/tests/camera_ptz_activity_registry_test.cpp +++ b/cmvr-es/service/grpc/server/tests/camera_ptz_activity_registry_test.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/camera_ptz_activity_registry.h" +#include "service/grpc/server/include/camera_ptz_activity_registry.h" #include #include @@ -10,7 +10,7 @@ #include -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { namespace { diff --git a/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_agv_service_test.cpp similarity index 99% rename from cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp rename to cmvr-es/service/grpc/server/tests/grpc_agv_service_test.cpp index 68f76da0..a59e2e13 100644 --- a/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp +++ b/cmvr-es/service/grpc/server/tests/grpc_agv_service_test.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_agv_service.h" +#include "service/grpc/server/include/grpc_agv_service.h" #include #include @@ -11,9 +11,9 @@ #include #include "cmvr/config/device_manager_config/device_manager_config.pb.h" -#include "manager/control_authority/include/control_authority_manager.h" +#include "manager/control_authority_manager/include/control_authority_manager.h" #include "manager/device_manager/include/device_manager.h" -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { namespace { diff --git a/cmvr-es/service/grpc/src/grpc_arm_client_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_arm_client_test.cpp similarity index 100% rename from cmvr-es/service/grpc/src/grpc_arm_client_test.cpp rename to cmvr-es/service/grpc/server/tests/grpc_arm_client_test.cpp diff --git a/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_arm_service_test.cpp similarity index 99% rename from cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp rename to cmvr-es/service/grpc/server/tests/grpc_arm_service_test.cpp index 3c923938..980d5b7f 100644 --- a/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp +++ b/cmvr-es/service/grpc/server/tests/grpc_arm_service_test.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_arm_service.h" +#include "service/grpc/server/include/grpc_arm_service.h" #include #include @@ -20,10 +20,10 @@ #include #include "cmvr/config/device_manager_config/device_manager_config.pb.h" -#include "manager/control_authority/include/control_authority_manager.h" +#include "manager/control_authority_manager/include/control_authority_manager.h" #include "manager/device_manager/include/device_manager.h" -#include "service/grpc/include/grpc_error_logging_interceptor.h" -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/server/include/grpc_error_logging_interceptor.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { namespace { @@ -599,7 +599,7 @@ protected: header->set_command_id(command_id); header->set_expected_service_instance_id( device::DeviceManager::getInstance() - .safetyCoordinator() + .safetyManager() .serviceInstanceId()); header->set_valid_for_ms(1000); request.mutable_target()->add_position(position); diff --git a/cmvr-es/service/grpc/tests/grpc_arm_teleop_service_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_arm_teleop_service_test.cpp similarity index 98% rename from cmvr-es/service/grpc/tests/grpc_arm_teleop_service_test.cpp rename to cmvr-es/service/grpc/server/tests/grpc_arm_teleop_service_test.cpp index a8394d5a..0a6be187 100644 --- a/cmvr-es/service/grpc/tests/grpc_arm_teleop_service_test.cpp +++ b/cmvr-es/service/grpc/server/tests/grpc_arm_teleop_service_test.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_arm_teleop_service.h" +#include "service/grpc/server/include/grpc_arm_teleop_service.h" #include #include @@ -17,9 +17,9 @@ #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" +#include "manager/safety_manager/include/device_safety_endpoint.h" +#include "manager/safety_manager/include/safety_manager.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { namespace { @@ -338,10 +338,10 @@ class TeleopServerHarness final { public: explicit TeleopServerHarness( std::shared_ptr backend, - safety::SafetyCoordinator* safety_coordinator = nullptr) + safety::SafetyManager* safety_manager = nullptr) : service_( std::move(backend), nullptr, nullptr, - safety_coordinator) + safety_manager) { socket_path_ = "/tmp/cmvr_arm_teleop_service_test_" + @@ -862,9 +862,9 @@ TEST(ArmTeleopServiceTest, TEST(ArmTeleopServiceTest, CoordinatorInvalidationStopsExistingSessionBeforeAnotherSetpoint) { - safety::SafetyCoordinatorConfig config; + safety::SafetyManagerConfig config; config.enforcement_mode = safety::EnforcementMode::EnforceAll; - safety::SafetyCoordinator coordinator(config); + safety::SafetyManager coordinator(config); auto endpoint = std::make_shared(); ASSERT_TRUE(coordinator.registerDevice( {endpoint->descriptor(), endpoint, {}})); diff --git a/cmvr-es/service/grpc/tests/grpc_camera_stream_policy_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_camera_stream_policy_test.cpp similarity index 96% rename from cmvr-es/service/grpc/tests/grpc_camera_stream_policy_test.cpp rename to cmvr-es/service/grpc/server/tests/grpc_camera_stream_policy_test.cpp index 1d84b942..ad450c4c 100644 --- a/cmvr-es/service/grpc/tests/grpc_camera_stream_policy_test.cpp +++ b/cmvr-es/service/grpc/server/tests/grpc_camera_stream_policy_test.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_camera_stream_policy.h" +#include "service/grpc/server/include/grpc_camera_stream_policy.h" #include #include diff --git a/cmvr-es/service/grpc/tests/grpc_command_transaction_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_command_transaction_test.cpp similarity index 95% rename from cmvr-es/service/grpc/tests/grpc_command_transaction_test.cpp rename to cmvr-es/service/grpc/server/tests/grpc_command_transaction_test.cpp index c50d7178..3ded4775 100644 --- a/cmvr-es/service/grpc/tests/grpc_command_transaction_test.cpp +++ b/cmvr-es/service/grpc/server/tests/grpc_command_transaction_test.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_command_transaction.h" +#include "service/grpc/server/include/grpc_command_transaction.h" #include #include @@ -9,7 +9,7 @@ #include #include "cmvr/api/arm_command.pb.h" -#include "manager/safety/include/device_safety_endpoint.h" +#include "manager/safety_manager/include/device_safety_endpoint.h" namespace cmvr::service { namespace { @@ -79,7 +79,7 @@ public: }; api::MoveJ_Request moveRequest( - const safety::SafetyCoordinator& coordinator, + const safety::SafetyManager& coordinator, const std::string& command_id, const double target = 0.25) { @@ -94,7 +94,7 @@ api::MoveJ_Request moveRequest( } grpc::Status executeMove( - safety::SafetyCoordinator& coordinator, + safety::SafetyManager& coordinator, const api::MoveJ_Request& request, api::MoveJ_Response& response, int& dispatches, @@ -124,7 +124,7 @@ grpc::Status executeMove( TEST(GrpcCommandTransactionTest, DeterministicHashIgnoresRetryIdentityButIncludesPayload) { - safety::SafetyCoordinator coordinator; + safety::SafetyManager coordinator; auto first = moveRequest(coordinator, "command-1", 0.25); auto retry = first; retry.mutable_header()->set_command_id("command-2"); @@ -152,7 +152,7 @@ TEST(GrpcCommandTransactionTest, TEST(GrpcCommandTransactionTest, SameIdReturnsCachedResponseWithoutRedispatch) { - safety::SafetyCoordinator coordinator; + safety::SafetyManager coordinator; auto endpoint = std::make_shared("arm"); ASSERT_TRUE(coordinator.registerDevice( {endpoint->descriptor(), endpoint, {}})); @@ -194,9 +194,9 @@ TEST(GrpcCommandTransactionTest, SameIdReturnsCachedResponseWithoutRedispatch) TEST(GrpcCommandTransactionTest, EnforcedCommandRequiresIdentityAndRunsFinalHardwareCheck) { - safety::SafetyCoordinatorConfig config; + safety::SafetyManagerConfig config; config.enforcement_mode = safety::EnforcementMode::EnforceAll; - safety::SafetyCoordinator coordinator(config); + safety::SafetyManager coordinator(config); auto endpoint = std::make_shared("arm"); ASSERT_TRUE(coordinator.registerDevice( {endpoint->descriptor(), endpoint, {}})); @@ -231,9 +231,9 @@ TEST(GrpcCommandTransactionTest, TEST(GrpcCommandTransactionTest, ExceptionAfterDispatchIsQuarantinedAndNeverRedispatched) { - safety::SafetyCoordinatorConfig config; + safety::SafetyManagerConfig config; config.enforcement_mode = safety::EnforcementMode::EnforceAll; - safety::SafetyCoordinator coordinator(config); + safety::SafetyManager coordinator(config); auto endpoint = std::make_shared("arm"); ASSERT_TRUE(coordinator.registerDevice( {endpoint->descriptor(), endpoint, {}})); @@ -269,9 +269,9 @@ TEST(GrpcCommandTransactionTest, TEST(GrpcCommandTransactionTest, ScopedDispatchChecksHardwareForEverySubmission) { - safety::SafetyCoordinatorConfig config; + safety::SafetyManagerConfig config; config.enforcement_mode = safety::EnforcementMode::EnforceAll; - safety::SafetyCoordinator coordinator(config); + safety::SafetyManager coordinator(config); auto endpoint = std::make_shared("arm"); ASSERT_TRUE(coordinator.registerDevice( {endpoint->descriptor(), endpoint, {}})); @@ -313,9 +313,9 @@ TEST(GrpcCommandTransactionTest, TEST(GrpcCommandTransactionTest, RevokedActuationPermitCannotSuppressInternalSafetyStop) { - safety::SafetyCoordinatorConfig config; + safety::SafetyManagerConfig config; config.enforcement_mode = safety::EnforcementMode::EnforceAll; - safety::SafetyCoordinator coordinator(config); + safety::SafetyManager coordinator(config); auto endpoint = std::make_shared("arm"); ASSERT_TRUE(coordinator.registerDevice( {endpoint->descriptor(), endpoint, {}})); @@ -374,9 +374,9 @@ TEST(GrpcCommandTransactionTest, TEST(GrpcCommandTransactionTest, ServerDerivedStopRemainsDispatchableWhenActuationIsQuarantined) { - safety::SafetyCoordinatorConfig config; + safety::SafetyManagerConfig config; config.enforcement_mode = safety::EnforcementMode::EnforceAll; - safety::SafetyCoordinator coordinator(config); + safety::SafetyManager coordinator(config); auto endpoint = std::make_shared("arm"); ASSERT_TRUE(coordinator.registerDevice( {endpoint->descriptor(), endpoint, {}})); diff --git a/cmvr-es/service/grpc/tests/grpc_dexhand_service_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_dexhand_service_test.cpp similarity index 97% rename from cmvr-es/service/grpc/tests/grpc_dexhand_service_test.cpp rename to cmvr-es/service/grpc/server/tests/grpc_dexhand_service_test.cpp index 6ebdb2a6..6638b27e 100644 --- a/cmvr-es/service/grpc/tests/grpc_dexhand_service_test.cpp +++ b/cmvr-es/service/grpc/server/tests/grpc_dexhand_service_test.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_dexhand_service.h" +#include "service/grpc/server/include/grpc_dexhand_service.h" #include #include @@ -18,10 +18,10 @@ #include #include "cmvr/config/device_manager_config/device_manager_config.pb.h" -#include "manager/control_authority/include/control_authority_manager.h" +#include "manager/control_authority_manager/include/control_authority_manager.h" #include "manager/device_manager/include/device_manager.h" -#include "service/grpc/include/media_activity_coordinator.h" -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/server/include/media_activity_coordinator.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { namespace { diff --git a/cmvr-es/service/grpc/tests/grpc_error_logging_interceptor_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_error_logging_interceptor_test.cpp similarity index 99% rename from cmvr-es/service/grpc/tests/grpc_error_logging_interceptor_test.cpp rename to cmvr-es/service/grpc/server/tests/grpc_error_logging_interceptor_test.cpp index 02b7adca..2287e2d2 100644 --- a/cmvr-es/service/grpc/tests/grpc_error_logging_interceptor_test.cpp +++ b/cmvr-es/service/grpc/server/tests/grpc_error_logging_interceptor_test.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_error_logging_interceptor.h" +#include "service/grpc/server/include/grpc_error_logging_interceptor.h" #include #include diff --git a/cmvr-es/service/grpc/tests/grpc_head_service_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_head_service_test.cpp similarity index 98% rename from cmvr-es/service/grpc/tests/grpc_head_service_test.cpp rename to cmvr-es/service/grpc/server/tests/grpc_head_service_test.cpp index 762dbcc7..0bef7581 100644 --- a/cmvr-es/service/grpc/tests/grpc_head_service_test.cpp +++ b/cmvr-es/service/grpc/server/tests/grpc_head_service_test.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_head_service.h" +#include "service/grpc/server/include/grpc_head_service.h" #include #include @@ -11,8 +11,8 @@ #include "cmvr/config/device_manager_config/device_manager_config.pb.h" #include "manager/device_manager/include/device_manager.h" -#include "service/grpc/include/media_activity_coordinator.h" -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/server/include/media_activity_coordinator.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { namespace { diff --git a/cmvr-es/service/grpc/src/grpc_hlc_client_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_hlc_client_test.cpp similarity index 100% rename from cmvr-es/service/grpc/src/grpc_hlc_client_test.cpp rename to cmvr-es/service/grpc/server/tests/grpc_hlc_client_test.cpp diff --git a/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_motor_service_test.cpp similarity index 99% rename from cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp rename to cmvr-es/service/grpc/server/tests/grpc_motor_service_test.cpp index da8898fa..ea369cb8 100644 --- a/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp +++ b/cmvr-es/service/grpc/server/tests/grpc_motor_service_test.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_motor_service.h" +#include "service/grpc/server/include/grpc_motor_service.h" #include #include @@ -18,8 +18,8 @@ #include "devices/motor/manager/include/motor_manager.h" #include "devices/motor/motor_protocol_interface.h" #include "manager/device_manager/include/device_manager.h" -#include "service/grpc/include/motor_activity_coordinator.h" -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/server/include/motor_activity_coordinator.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { @@ -693,7 +693,7 @@ TEST_F(MotorServiceTest, std::string::npos); EXPECT_GE(protocol_->quick_stop_count_.load(), 1); const auto safety = device::DeviceManager::getInstance() - .safetyCoordinator() + .safetyManager() .snapshot(); ASSERT_EQ(safety.devices.size(), 1U); EXPECT_EQ( diff --git a/cmvr-es/service/grpc/tests/grpc_robot_arm_teleop_backend_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_robot_arm_teleop_backend_test.cpp similarity index 99% rename from cmvr-es/service/grpc/tests/grpc_robot_arm_teleop_backend_test.cpp rename to cmvr-es/service/grpc/server/tests/grpc_robot_arm_teleop_backend_test.cpp index 7cc01f07..b320b3b4 100644 --- a/cmvr-es/service/grpc/tests/grpc_robot_arm_teleop_backend_test.cpp +++ b/cmvr-es/service/grpc/server/tests/grpc_robot_arm_teleop_backend_test.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_robot_arm_teleop_backend.h" +#include "service/grpc/server/include/grpc_robot_arm_teleop_backend.h" #include #include diff --git a/cmvr-es/service/grpc/tests/grpc_security_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_security_test.cpp similarity index 99% rename from cmvr-es/service/grpc/tests/grpc_security_test.cpp rename to cmvr-es/service/grpc/server/tests/grpc_security_test.cpp index 1a21dfe6..17b1e1f7 100644 --- a/cmvr-es/service/grpc/tests/grpc_security_test.cpp +++ b/cmvr-es/service/grpc/server/tests/grpc_security_test.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_security.h" +#include "service/grpc/server/include/grpc_security.h" #include #include diff --git a/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_system_service_test.cpp similarity index 98% rename from cmvr-es/service/grpc/tests/grpc_system_service_test.cpp rename to cmvr-es/service/grpc/server/tests/grpc_system_service_test.cpp index 7181aafd..3b3ba689 100644 --- a/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp +++ b/cmvr-es/service/grpc/server/tests/grpc_system_service_test.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/grpc_system_service.h" +#include "service/grpc/server/include/grpc_system_service.h" #include #include @@ -26,19 +26,19 @@ #include "devices/camera/abstract_camera.h" #include "devices/microphone/abstract_microphone.h" #include "devices/speaker/abstract_speaker.h" -#include "manager/control_authority/include/control_authority_manager.h" +#include "manager/control_authority_manager/include/control_authority_manager.h" #include "manager/device_manager/include/device_manager.h" -#include "manager/media_source_hub/include/device_media_source_adapter.h" +#include "manager/media_source_manager/include/device_media_source_adapter.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/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" +#include "service/grpc/action/include/action_queue_executor.h" +#include "service/grpc/server/include/camera_operational_activity_registry.h" +#include "service/grpc/server/include/camera_ptz_activity_registry.h" +#include "service/grpc/server/include/grpc_camera_service.h" +#include "service/grpc/server/include/grpc_recovery_audit.h" +#include "service/grpc/server/include/grpc_security.h" +#include "service/grpc/server/include/media_activity_coordinator.h" +#include "service/grpc/server/include/motor_activity_coordinator.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include "task/task_factory.h" namespace cmvr::service { @@ -1348,7 +1348,7 @@ protected: { task::TaskManager::destroyInstance(); blocking_stop_task.reset(); - (void)media::globalMediaSourceHub().stopAllSources(); + (void)media::globalMediaSourceManager().stopAllSources(); control::ControlAuthorityManager::instance().clear(); globalStopAllAdmissionGate().clearForTesting(); globalCameraOperationalActivityRegistry().clearForTesting(); @@ -1377,7 +1377,7 @@ protected: globalCameraPtzActivityRegistry().clearForTesting(); globalMediaActivityCoordinator().clearForTesting(); globalMotorActivityCoordinator().clearForTesting(); - (void)media::globalMediaSourceHub().stopAllSources(); + (void)media::globalMediaSourceManager().stopAllSources(); } api::GetDeviceListCommand_Feedback getDeviceList() @@ -1623,7 +1623,7 @@ TEST_F(GrpcSystemServiceTest, RecoveryIsDisabledByDefault) request.set_recovery_id("recovery-disabled"); request.mutable_scope()->set_all_devices(true); request.set_expected_safety_epoch( - manager.safetyCoordinator().snapshot().safety_epoch); + manager.safetyManager().snapshot().safety_epoch); request.set_mode(api::RecoverSafetyStateCommand::VERIFY_ONLY); request.set_reason("diagnostic verification"); api::RecoverSafetyStateCommand_Feedback response; @@ -1649,7 +1649,7 @@ TEST_F(GrpcSystemServiceTest, RecoveryRequiresDurableAuditBeforeCoordinator) request.set_recovery_id("recovery-audited"); request.mutable_scope()->set_all_devices(true); request.set_expected_safety_epoch( - manager.safetyCoordinator().snapshot().safety_epoch); + manager.safetyManager().snapshot().safety_epoch); request.set_mode(api::RecoverSafetyStateCommand::VERIFY_ONLY); request.set_reason("verify the local work cell"); request.set_timeout_ms(500); @@ -1665,7 +1665,7 @@ TEST_F(GrpcSystemServiceTest, RecoveryRequiresDurableAuditBeforeCoordinator) EXPECT_EQ(response.recovery_id(), "recovery-audited"); const auto epoch_before_failure = - manager.safetyCoordinator().snapshot().safety_epoch; + manager.safetyManager().snapshot().safety_epoch; audit->fail = true; request.set_recovery_id("recovery-audit-fails"); request.set_expected_safety_epoch(epoch_before_failure); @@ -1678,7 +1678,7 @@ TEST_F(GrpcSystemServiceTest, RecoveryRequiresDurableAuditBeforeCoordinator) response.header().reason_code(), api::COMMAND_REASON_CODE_RECOVERY_AUDIT_FAILED); EXPECT_EQ( - manager.safetyCoordinator().snapshot().safety_epoch, + manager.safetyManager().snapshot().safety_epoch, epoch_before_failure); } @@ -1782,18 +1782,18 @@ TEST_F(GrpcSystemServiceTest, { config::DeviceManagerConfig config; config.mutable_safety()->set_mode( - config::SafetyCoordinatorConfig::ENFORCE_ALL); + config::SafetyManagerConfig::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(); + manager.safetyManager().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(); + const auto safety = manager.safetyManager().snapshot(); ASSERT_EQ( safety.system_state, safety::SystemAdmissionState::Open); const auto safety_device = std::find_if( @@ -3099,17 +3099,17 @@ TEST_F(GrpcSystemServiceTest, track_config.time_base = {1, 90000}; auto descriptor = media::makeTrackDescriptor(std::move(track_config)); auto hub_stop_calls = std::make_shared>(0); - media::MediaSourceHub::SourceCallbacks callbacks; + media::MediaSourceManager::SourceCallbacks callbacks; callbacks.start = []( - const media::MediaSourceHub::FrameSink&, - const media::MediaSourceHub::CancelPredicate&) { + const media::MediaSourceManager::FrameSink&, + const media::MediaSourceManager::CancelPredicate&) { return true; }; callbacks.stop_confirmed = [hub_stop_calls] { hub_stop_calls->fetch_add(1, std::memory_order_relaxed); return true; }; - auto& hub = media::globalMediaSourceHub(); + auto& hub = media::globalMediaSourceManager(); ASSERT_TRUE(hub.registerSource( descriptor, std::move(callbacks), 2)); auto subscription = hub.subscribe(hub_track_id); diff --git a/cmvr-es/service/grpc/tests/media_activity_coordinator_test.cpp b/cmvr-es/service/grpc/server/tests/media_activity_coordinator_test.cpp similarity index 99% rename from cmvr-es/service/grpc/tests/media_activity_coordinator_test.cpp rename to cmvr-es/service/grpc/server/tests/media_activity_coordinator_test.cpp index f93155f6..17ca7db1 100644 --- a/cmvr-es/service/grpc/tests/media_activity_coordinator_test.cpp +++ b/cmvr-es/service/grpc/server/tests/media_activity_coordinator_test.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/media_activity_coordinator.h" +#include "service/grpc/server/include/media_activity_coordinator.h" #include #include diff --git a/cmvr-es/service/grpc/tests/motor_activity_coordinator_test.cpp b/cmvr-es/service/grpc/server/tests/motor_activity_coordinator_test.cpp similarity index 99% rename from cmvr-es/service/grpc/tests/motor_activity_coordinator_test.cpp rename to cmvr-es/service/grpc/server/tests/motor_activity_coordinator_test.cpp index 703f51b4..35d430f6 100644 --- a/cmvr-es/service/grpc/tests/motor_activity_coordinator_test.cpp +++ b/cmvr-es/service/grpc/server/tests/motor_activity_coordinator_test.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/include/motor_activity_coordinator.h" +#include "service/grpc/server/include/motor_activity_coordinator.h" #include #include diff --git a/cmvr-es/service/stop_all/CMakeLists.txt b/cmvr-es/service/grpc/stop_all/CMakeLists.txt similarity index 83% rename from cmvr-es/service/stop_all/CMakeLists.txt rename to cmvr-es/service/grpc/stop_all/CMakeLists.txt index a50b9cc7..d6469ad3 100644 --- a/cmvr-es/service/stop_all/CMakeLists.txt +++ b/cmvr-es/service/grpc/stop_all/CMakeLists.txt @@ -5,19 +5,19 @@ add_library(stop_all_admission_gate STATIC target_compile_features(stop_all_admission_gate PUBLIC cxx_std_17) target_include_directories(stop_all_admission_gate PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/../.. + ${CMAKE_CURRENT_SOURCE_DIR}/../../.. ) add_library(cmvr_es::stop_all_admission_gate ALIAS stop_all_admission_gate) add_library(camera_operational_activity_registry STATIC - ../grpc/src/camera_operational_activity_registry.cpp + ../server/src/camera_operational_activity_registry.cpp ) target_compile_features(camera_operational_activity_registry PUBLIC cxx_std_17) target_include_directories(camera_operational_activity_registry PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/../.. + ${CMAKE_CURRENT_SOURCE_DIR}/../../.. ) target_link_libraries(camera_operational_activity_registry PUBLIC diff --git a/cmvr-es/service/stop_all/include/deferred_stop_operation.h b/cmvr-es/service/grpc/stop_all/include/deferred_stop_operation.h similarity index 100% rename from cmvr-es/service/stop_all/include/deferred_stop_operation.h rename to cmvr-es/service/grpc/stop_all/include/deferred_stop_operation.h diff --git a/cmvr-es/service/stop_all/include/stop_all_admission_gate.h b/cmvr-es/service/grpc/stop_all/include/stop_all_admission_gate.h similarity index 100% rename from cmvr-es/service/stop_all/include/stop_all_admission_gate.h rename to cmvr-es/service/grpc/stop_all/include/stop_all_admission_gate.h diff --git a/cmvr-es/service/stop_all/include/stop_operation_dispatcher.h b/cmvr-es/service/grpc/stop_all/include/stop_operation_dispatcher.h similarity index 100% rename from cmvr-es/service/stop_all/include/stop_operation_dispatcher.h rename to cmvr-es/service/grpc/stop_all/include/stop_operation_dispatcher.h diff --git a/cmvr-es/service/stop_all/src/stop_all_admission_gate.cpp b/cmvr-es/service/grpc/stop_all/src/stop_all_admission_gate.cpp similarity index 97% rename from cmvr-es/service/stop_all/src/stop_all_admission_gate.cpp rename to cmvr-es/service/grpc/stop_all/src/stop_all_admission_gate.cpp index 8920a732..cdee9a90 100644 --- a/cmvr-es/service/stop_all/src/stop_all_admission_gate.cpp +++ b/cmvr-es/service/grpc/stop_all/src/stop_all_admission_gate.cpp @@ -1,4 +1,4 @@ -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include diff --git a/cmvr-es/service/stop_all/src/stop_operation_dispatcher.cpp b/cmvr-es/service/grpc/stop_all/src/stop_operation_dispatcher.cpp similarity index 98% rename from cmvr-es/service/stop_all/src/stop_operation_dispatcher.cpp rename to cmvr-es/service/grpc/stop_all/src/stop_operation_dispatcher.cpp index f53b68c2..38bfb0cf 100644 --- a/cmvr-es/service/stop_all/src/stop_operation_dispatcher.cpp +++ b/cmvr-es/service/grpc/stop_all/src/stop_operation_dispatcher.cpp @@ -1,4 +1,4 @@ -#include "service/stop_all/include/stop_operation_dispatcher.h" +#include "service/grpc/stop_all/include/stop_operation_dispatcher.h" #include #include diff --git a/cmvr-es/service/stop_all/tests/stop_all_admission_gate_test.cpp b/cmvr-es/service/grpc/stop_all/tests/stop_all_admission_gate_test.cpp similarity index 97% rename from cmvr-es/service/stop_all/tests/stop_all_admission_gate_test.cpp rename to cmvr-es/service/grpc/stop_all/tests/stop_all_admission_gate_test.cpp index 14300844..21990915 100644 --- a/cmvr-es/service/stop_all/tests/stop_all_admission_gate_test.cpp +++ b/cmvr-es/service/grpc/stop_all/tests/stop_all_admission_gate_test.cpp @@ -1,4 +1,4 @@ -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include diff --git a/cmvr-es/service/stop_all/tests/stop_operation_dispatcher_test.cpp b/cmvr-es/service/grpc/stop_all/tests/stop_operation_dispatcher_test.cpp similarity index 99% rename from cmvr-es/service/stop_all/tests/stop_operation_dispatcher_test.cpp rename to cmvr-es/service/grpc/stop_all/tests/stop_operation_dispatcher_test.cpp index aabb337b..562cd05b 100644 --- a/cmvr-es/service/stop_all/tests/stop_operation_dispatcher_test.cpp +++ b/cmvr-es/service/grpc/stop_all/tests/stop_operation_dispatcher_test.cpp @@ -1,4 +1,4 @@ -#include "service/stop_all/include/stop_operation_dispatcher.h" +#include "service/grpc/stop_all/include/stop_operation_dispatcher.h" #include diff --git a/cmvr-es/service/quic_edge/CMakeLists.txt b/cmvr-es/service/quic_edge/CMakeLists.txt index a6562f83..3efed1be 100644 --- a/cmvr-es/service/quic_edge/CMakeLists.txt +++ b/cmvr-es/service/quic_edge/CMakeLists.txt @@ -14,7 +14,7 @@ target_include_directories(quic_edge_service PUBLIC ${PROJECT_SOURCE_DIR}/cmvr-e target_link_libraries(quic_edge_service PUBLIC cmvr_es::proto - cmvr_es::media_source_hub + cmvr_es::media_source_manager PRIVATE cmvr_es::device_media_source_adapter cmvr_es::device_manager @@ -29,7 +29,7 @@ if(BUILD_TESTING) target_compile_features(quic_edge_protocol_test PRIVATE cxx_std_17) target_link_libraries(quic_edge_protocol_test PRIVATE cmvr_es::quic_edge_service - cmvr_es::media_source_hub + cmvr_es::media_source_manager cmvr_es::stop_all_admission_gate Threads::Threads ) diff --git a/cmvr-es/service/quic_edge/include/quic_edge_service.h b/cmvr-es/service/quic_edge/include/quic_edge_service.h index 23eae61b..9b86af96 100644 --- a/cmvr-es/service/quic_edge/include/quic_edge_service.h +++ b/cmvr-es/service/quic_edge/include/quic_edge_service.h @@ -15,7 +15,7 @@ #include "cmvr/config/quic_edge_config/quic_edge_config.pb.h" #include "devices/device_types.h" -#include "manager/media_source_hub/include/media_source_hub.h" +#include "manager/media_source_manager/include/media_source_manager.h" #include "service/quic_edge/include/control_framing.h" #include "service/quic_edge/include/datagram_packetizer.h" #include "service/quic_edge/include/quic_transport.h" @@ -76,7 +76,7 @@ public: explicit QuicEdgeService(config::QuicEdgeConfig config); QuicEdgeService(config::QuicEdgeConfig config, std::unique_ptr transport, - media::MediaSourceHub& media_hub, + media::MediaSourceManager& media_hub, DeviceSnapshotProvider device_snapshot_provider = {}); ~QuicEdgeService(); @@ -106,7 +106,7 @@ private: struct ActiveTrack { config::QuicEdgeTrackConfig config; std::string source_track_id; - media::MediaSourceHub::Subscription subscription; + media::MediaSourceManager::Subscription subscription; std::optional last_description; bool waiting_for_keyframe{false}; bool keyframe_requested{false}; @@ -157,7 +157,7 @@ private: config::QuicEdgeConfig config_; std::unique_ptr transport_; - media::MediaSourceHub* media_hub_{nullptr}; + media::MediaSourceManager* media_hub_{nullptr}; bool using_global_media_hub_{false}; SourceRegistrar source_registrar_; DeviceSnapshotProvider device_snapshot_provider_; diff --git a/cmvr-es/service/quic_edge/include/quic_edge_types.h b/cmvr-es/service/quic_edge/include/quic_edge_types.h index 36fda822..cabfdd36 100644 --- a/cmvr-es/service/quic_edge/include/quic_edge_types.h +++ b/cmvr-es/service/quic_edge/include/quic_edge_types.h @@ -22,7 +22,7 @@ enum DatagramFlag : std::uint16_t { }; // All integer fields are serialized in network byte order. codec_generation -// is a compact token for the full 64-bit MediaSourceHub descriptor generation +// is a compact token for the full 64-bit MediaSourceManager descriptor generation // announced on the reliable control stream. struct DatagramHeader { std::uint8_t protocol_version{kProtocolVersion}; diff --git a/cmvr-es/service/quic_edge/src/quic_edge_device_adapter.cpp b/cmvr-es/service/quic_edge/src/quic_edge_device_adapter.cpp index 856535dc..0e1ba0fc 100644 --- a/cmvr-es/service/quic_edge/src/quic_edge_device_adapter.cpp +++ b/cmvr-es/service/quic_edge/src/quic_edge_device_adapter.cpp @@ -3,14 +3,14 @@ #include #include "manager/device_manager/include/device_manager.h" -#include "manager/media_source_hub/include/device_media_source_adapter.h" +#include "manager/media_source_manager/include/device_media_source_adapter.h" namespace cmvr::quic_edge { QuicEdgeService::QuicEdgeService(config::QuicEdgeConfig config) : config_(std::move(config)), transport_(createDefaultQuicTransport(config_.datagram_send_queue_depth())), - media_hub_(&media::globalMediaSourceHub()), + media_hub_(&media::globalMediaSourceManager()), using_global_media_hub_(true), device_snapshot_provider_([] { return device::DeviceManager::getInstance().snapshot(); @@ -57,7 +57,7 @@ QuicEdgeService::QuicEdgeService(config::QuicEdgeConfig config) } if (!registered && !media_hub_->hasSource(source_track_id)) { if (error) { - *error = "failed to register MediaSourceHub source: " + + *error = "failed to register MediaSourceManager source: " + source_track_id; } return false; 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 0cdc02e2..2ea62c79 100644 --- a/cmvr-es/service/quic_edge/src/quic_edge_service.cpp +++ b/cmvr-es/service/quic_edge/src/quic_edge_service.cpp @@ -25,7 +25,7 @@ #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" +#include "manager/media_source_manager/include/device_media_source_adapter.h" namespace cmvr::quic_edge { namespace { @@ -423,7 +423,7 @@ const char* toString(const QuicEdgeServiceState state) QuicEdgeService::QuicEdgeService(config::QuicEdgeConfig config, std::unique_ptr transport, - media::MediaSourceHub& media_hub, + media::MediaSourceManager& media_hub, DeviceSnapshotProvider device_snapshot_provider) : config_(std::move(config)), transport_(std::move(transport)), @@ -598,7 +598,7 @@ bool QuicEdgeService::initialize(std::string* error) return false; } if (!transport_ || !media_hub_) { - const std::string message = "QUIC edge transport or MediaSourceHub is null"; + const std::string message = "QUIC edge transport or MediaSourceManager is null"; setState(QuicEdgeServiceState::FAILED, message); setError(error, message); return false; @@ -1453,18 +1453,18 @@ void QuicEdgeService::refreshMediaTracks( safety::DispatchGuard source_dispatch; if (using_global_media_hub_) { source_dispatch = media::beginMediaSourceStartDispatch( - device::DeviceManager::getInstance().safetyCoordinator(), + device::DeviceManager::getInstance().safetyManager(), track_config.device_id()); if (!source_dispatch.acquired()) { recordMediaError( - "MediaSourceHub safety admission rejected: " + + "MediaSourceManager safety admission rejected: " + track.source_track_id); continue; } } track.subscription = media_hub_->subscribe( track.source_track_id, - media::MediaSourceHub::StartPosition::LATEST_AVAILABLE, + media::MediaSourceManager::StartPosition::LATEST_AVAILABLE, [this, activity_generation] { std::lock_guard lock(mutex_); return stop_requested_ || media_stop_requested_ || @@ -1472,7 +1472,7 @@ void QuicEdgeService::refreshMediaTracks( }); if (!track.subscription.valid()) { recordMediaError( - "MediaSourceHub source unavailable: " + track.source_track_id); + "MediaSourceManager source unavailable: " + track.source_track_id); continue; } track.waiting_for_keyframe = @@ -1500,14 +1500,14 @@ bool QuicEdgeService::ensureSourceRegistered( { if (media_hub_->hasSource(source_track_id)) return true; if (!using_global_media_hub_ || !source_registrar_) { - setError(error, "MediaSourceHub source unavailable: " + source_track_id); + setError(error, "MediaSourceManager source unavailable: " + source_track_id); return false; } const bool registered = source_registrar_(track, source_track_id, error); if (!registered && !media_hub_->hasSource(source_track_id)) { if (!error || error->empty()) { setError(error, - "failed to register MediaSourceHub source: " + source_track_id); + "failed to register MediaSourceManager source: " + source_track_id); } return false; } @@ -1638,7 +1638,7 @@ bool QuicEdgeService::processTrack(ActiveTrack* track, if (!read) { if (!track->subscription.valid()) { recordMediaError( - "MediaSourceHub subscription stopped: " + track->source_track_id); + "MediaSourceManager subscription stopped: " + track->source_track_id); } return true; } @@ -1647,7 +1647,7 @@ bool QuicEdgeService::processTrack(ActiveTrack* track, if (!frame || !frame->descriptor || frame->descriptor->id != track->source_track_id || frame->descriptor->kind != expectedKind(track->config)) { - recordMediaError("MediaSourceHub returned an invalid or mismatched frame"); + recordMediaError("MediaSourceManager returned an invalid or mismatched frame"); track->next_frame_discontinuous = true; return true; } @@ -1667,7 +1667,7 @@ bool QuicEdgeService::processTrack(ActiveTrack* track, !track->last_description || *track->last_description != description; if (track->last_description && descriptor_changed && description.codec_generation < track->last_description->codec_generation) { - recordMediaError("MediaSourceHub descriptor generation regressed"); + recordMediaError("MediaSourceManager descriptor generation regressed"); track->next_frame_discontinuous = true; return true; } diff --git a/cmvr-es/service/quic_edge/tests/quic_edge_protocol_test.cpp b/cmvr-es/service/quic_edge/tests/quic_edge_protocol_test.cpp index f607d16f..b652a904 100644 --- a/cmvr-es/service/quic_edge/tests/quic_edge_protocol_test.cpp +++ b/cmvr-es/service/quic_edge/tests/quic_edge_protocol_test.cpp @@ -14,11 +14,11 @@ #include #include "cmvr/quic_edge/v1/quic_edge.pb.h" -#include "manager/media_source_hub/include/media_source_hub.h" +#include "manager/media_source_manager/include/media_source_manager.h" #include "service/quic_edge/include/control_framing.h" #include "service/quic_edge/include/datagram_packetizer.h" #include "service/quic_edge/include/quic_edge_service.h" -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" namespace { @@ -483,14 +483,14 @@ bool testPacketizer() bool testServiceWithSharedHub() { const std::string track_id = "camera-test/video/color"; - media::MediaSourceHub hub; - media::MediaSourceHub::FrameSink sink; + media::MediaSourceManager hub; + media::MediaSourceManager::FrameSink sink; std::mutex sink_mutex; std::atomic source_started{false}; std::atomic keyframe_requests{0}; - media::MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [&](const media::MediaSourceHub::FrameSink& value, - const media::MediaSourceHub::CancelPredicate&) { + media::MediaSourceManager::SourceCallbacks callbacks; + callbacks.start = [&](const media::MediaSourceManager::FrameSink& value, + const media::MediaSourceManager::CancelPredicate&) { std::lock_guard lock(sink_mutex); sink = value; source_started.store(true); @@ -528,7 +528,7 @@ bool testServiceWithSharedHub() frame_config.sequence = 1; frame_config.capture_time_ns = 1000000; frame_config.key_frame = true; - media::MediaSourceHub::FrameSink publisher; + media::MediaSourceManager::FrameSink publisher; { std::lock_guard lock(sink_mutex); publisher = sink; @@ -581,15 +581,15 @@ bool testMediaActivityInterruptPreservesPresenceAndResumes() { const std::string track_id = "camera-stop-all/video/color"; service::StopAllAdmissionGate admission; - media::MediaSourceHub hub(&admission); - media::MediaSourceHub::FrameSink sink; + media::MediaSourceManager hub(&admission); + media::MediaSourceManager::FrameSink sink; std::mutex sink_mutex; std::atomic source_started{false}; std::atomic source_starts{0U}; std::atomic source_stops{0U}; - media::MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [&](const media::MediaSourceHub::FrameSink& value, - const media::MediaSourceHub::CancelPredicate&) { + media::MediaSourceManager::SourceCallbacks callbacks; + callbacks.start = [&](const media::MediaSourceManager::FrameSink& value, + const media::MediaSourceManager::CancelPredicate&) { { std::lock_guard lock(sink_mutex); sink = value; @@ -620,7 +620,7 @@ bool testMediaActivityInterruptPreservesPresenceAndResumes() })); auto publish = [&](const std::uint64_t sequence) { - media::MediaSourceHub::FrameSink publisher; + media::MediaSourceManager::FrameSink publisher; { std::lock_guard lock(sink_mutex); publisher = sink; @@ -680,12 +680,12 @@ bool testMediaActivityInterruptCancelsStartingSubscription() { const std::string track_id = "slow-stop-all/video/color"; service::StopAllAdmissionGate admission; - media::MediaSourceHub hub(&admission); + media::MediaSourceManager hub(&admission); std::atomic start_entered{false}; std::atomic start_cancelled{false}; - media::MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [&](const media::MediaSourceHub::FrameSink&, - const media::MediaSourceHub::CancelPredicate& cancelled) { + media::MediaSourceManager::SourceCallbacks callbacks; + callbacks.start = [&](const media::MediaSourceManager::FrameSink&, + const media::MediaSourceManager::CancelPredicate& cancelled) { start_entered.store(true); while (!cancelled()) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); @@ -727,7 +727,7 @@ bool testMediaActivityInterruptCancelsStartingSubscription() bool testMissingInjectedSourceRetriesSafely() { - media::MediaSourceHub hub; + media::MediaSourceManager hub; auto transport = std::make_unique(); FakeTransport* transport_view = transport.get(); quic_edge::QuicEdgeService service( @@ -746,7 +746,7 @@ bool testMissingInjectedSourceRetriesSafely() bool testPresenceOnlyWithoutMedia() { - media::MediaSourceHub hub; + media::MediaSourceManager hub; auto transport = std::make_unique(); FakeTransport* transport_view = transport.get(); quic_edge::QuicEdgeService service( @@ -809,7 +809,7 @@ bool testDeviceManagerSnapshotInHeartbeat() failed.status_updated_at_unix_ms = 303U; snapshot.devices.push_back(failed); - media::MediaSourceHub hub; + media::MediaSourceManager hub; auto transport = std::make_unique(); FakeTransport* transport_view = transport.get(); quic_edge::QuicEdgeService service( @@ -943,7 +943,7 @@ bool testAllDeviceKindAndStateMappings() snapshot.devices.push_back(std::move(row)); } - media::MediaSourceHub hub; + media::MediaSourceManager hub; auto transport = std::make_unique(); FakeTransport* transport_view = transport.get(); quic_edge::QuicEdgeService service( @@ -974,7 +974,7 @@ bool testAllDeviceKindAndStateMappings() bool testConfiguredHeartbeatIntervalWithoutGatewayOverride() { - media::MediaSourceHub hub; + media::MediaSourceManager hub; auto transport = std::make_unique( true, true, true, 0U, 0U); FakeTransport* transport_view = transport.get(); @@ -1001,7 +1001,7 @@ bool testConfiguredHeartbeatIntervalWithoutGatewayOverride() bool testSnapshotLatencyDoesNotConsumeAckDeadline() { - media::MediaSourceHub hub; + media::MediaSourceManager hub; auto transport = std::make_unique( true, true, true, 0U, 250U, 70U); FakeTransport* transport_view = transport.get(); @@ -1026,7 +1026,7 @@ bool testSnapshotLatencyDoesNotConsumeAckDeadline() bool testHeartbeatTimeoutReconnectsWithoutTaskFailure() { - media::MediaSourceHub hub; + media::MediaSourceManager hub; auto transport = std::make_unique(true, false); FakeTransport* transport_view = transport.get(); auto config = validPresenceOnlyConfig(); @@ -1046,7 +1046,7 @@ bool testHeartbeatTimeoutReconnectsWithoutTaskFailure() bool testRegistrationRejectionBacksOff() { - media::MediaSourceHub hub; + media::MediaSourceManager hub; auto transport = std::make_unique(false, true); FakeTransport* transport_view = transport.get(); auto config = validPresenceOnlyConfig(); @@ -1074,7 +1074,7 @@ bool testRegistrationRejectionBacksOff() bool testHeartbeatAckRequiresSessionId() { - media::MediaSourceHub hub; + media::MediaSourceManager hub; auto transport = std::make_unique(true, true, false); FakeTransport* transport_view = transport.get(); quic_edge::QuicEdgeService service( @@ -1095,13 +1095,13 @@ bool testHeartbeatAckRequiresSessionId() bool testSlowMediaStartDoesNotBlockHeartbeat() { const std::string track_id = "slow-camera/video/color"; - media::MediaSourceHub hub; + media::MediaSourceManager hub; std::atomic start_entered{false}; std::atomic start_exited{false}; std::atomic release_start{false}; - media::MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [&](const media::MediaSourceHub::FrameSink&, - const media::MediaSourceHub::CancelPredicate& cancelled) { + media::MediaSourceManager::SourceCallbacks callbacks; + callbacks.start = [&](const media::MediaSourceManager::FrameSink&, + const media::MediaSourceManager::CancelPredicate& cancelled) { start_entered.store(true); while (!release_start.load() && !cancelled()) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); @@ -1148,7 +1148,7 @@ bool testSlowMediaStartDoesNotBlockHeartbeat() bool testLifecycleStateGuards() { - media::MediaSourceHub hub; + media::MediaSourceManager hub; { auto transport = std::make_unique(); quic_edge::QuicEdgeService service( @@ -1180,7 +1180,7 @@ bool testLifecycleStateGuards() bool testRobotIdIsRequired() { - media::MediaSourceHub hub; + media::MediaSourceManager hub; auto config = validPresenceOnlyConfig(); config.clear_robot_id(); auto transport = std::make_unique(); diff --git a/cmvr-es/task/CMakeLists.txt b/cmvr-es/task/CMakeLists.txt index d9952ab0..05d0896d 100644 --- a/cmvr-es/task/CMakeLists.txt +++ b/cmvr-es/task/CMakeLists.txt @@ -12,7 +12,7 @@ target_link_libraries(task cmvr_es::ik_solver cmvr_es::base_motion cmvr_es::self_collision_checker - cmvr_es::control_authority + cmvr_es::control_authority_manager PRIVATE cmvr_es::device_manager cmvr_es::stop_all_admission_gate 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 4356d607..c7799fc2 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 @@ -11,21 +11,21 @@ #include "common/config/config_files.h" #include "devices/arm/robot_arm.h" #include "manager/device_manager/include/device_manager.h" -#include "service/grpc/include/grpc_agv_service.h" -#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" -#include "service/grpc/include/grpc_head_service.h" -#include "service/grpc/include/grpc_hlc_service.h" -#include "service/grpc/include/grpc_microphone_service.h" -#include "service/grpc/include/grpc_motor_service.h" -#include "service/grpc/include/grpc_speaker_service.h" -#include "service/grpc/include/grpc_system_service.h" +#include "service/grpc/server/include/grpc_agv_service.h" +#include "service/grpc/server/include/grpc_arm_service.h" +#include "service/grpc/server/include/grpc_arm_teleop_service.h" +#include "service/grpc/server/include/grpc_robot_arm_teleop_backend.h" +#include "service/grpc/server/include/grpc_recovery_audit.h" +#include "service/grpc/server/include/grpc_security.h" +#include "service/grpc/server/include/grpc_camera_service.h" +#include "service/grpc/server/include/grpc_dexhand_service.h" +#include "service/grpc/server/include/grpc_error_logging_interceptor.h" +#include "service/grpc/server/include/grpc_head_service.h" +#include "service/grpc/server/include/grpc_hlc_service.h" +#include "service/grpc/server/include/grpc_microphone_service.h" +#include "service/grpc/server/include/grpc_motor_service.h" +#include "service/grpc/server/include/grpc_speaker_service.h" +#include "service/grpc/server/include/grpc_system_service.h" #include "task/task_factory.h" namespace cmvr::task { @@ -136,7 +136,7 @@ bool GrpcServerTask::start() : service::makeDisabledArmTeleopBackend(), nullptr, security_gateway_, - &device::DeviceManager::getInstance().safetyCoordinator()); + &device::DeviceManager::getInstance().safetyManager()); motor_service_ = std::make_unique(security_gateway_); agv_service_ = diff --git a/cmvr-es/task/quic_edge_task/src/quic_edge_task.cpp b/cmvr-es/task/quic_edge_task/src/quic_edge_task.cpp index a37bb855..fe5d2293 100644 --- a/cmvr-es/task/quic_edge_task/src/quic_edge_task.cpp +++ b/cmvr-es/task/quic_edge_task/src/quic_edge_task.cpp @@ -8,7 +8,7 @@ #include "cmvr/config/task_manager_config/task_manager_config.pb.h" #include "common/base/logging/logger.h" -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include "common/config/config_files.h" #include "manager/device_manager/include/device_manager.h" #include "task/task_factory.h" diff --git a/cmvr-es/task/quic_edge_task/tests/quic_edge_task_test.cpp b/cmvr-es/task/quic_edge_task/tests/quic_edge_task_test.cpp index 99a05f00..06e6124c 100644 --- a/cmvr-es/task/quic_edge_task/tests/quic_edge_task_test.cpp +++ b/cmvr-es/task/quic_edge_task/tests/quic_edge_task_test.cpp @@ -12,9 +12,9 @@ #include #include "cmvr/quic_edge/v1/quic_edge.pb.h" -#include "manager/media_source_hub/include/media_source_hub.h" +#include "manager/media_source_manager/include/media_source_manager.h" #include "service/quic_edge/include/control_framing.h" -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include "task/quic_edge_task/include/quic_edge_task.h" namespace { @@ -299,7 +299,7 @@ int main() admission_task.stop(); admission.clearForTesting(); - cmvr::media::MediaSourceHub media_hub; + cmvr::media::MediaSourceManager media_hub; std::vector transports; std::vector services; std::vector expanded_configs; 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 4198c9e6..e94cabf1 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 @@ -19,7 +19,7 @@ #include "devices/camera/abstract_camera.h" #include "devices/dexhand/abstract_dexhand.h" #include "devices/arm/robot_arm.h" -#include "manager/control_authority/include/control_authority_manager.h" +#include "manager/control_authority_manager/include/control_authority_manager.h" #include "task/task.h" #include "algorithms/perception/apriltag/include/apriltag_perception.h" #include "algorithms/perception/apriltag/include/tag_relative_target_3d.h" 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 f8ef3ecc..f02cd579 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 @@ -11,8 +11,8 @@ #include #include "manager/device_manager/include/device_manager.h" -#include "service/grpc/include/camera_operational_activity_registry.h" -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/server/include/camera_operational_activity_registry.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include "task/touch_screen_task/include/touch_screen_task.h" namespace cmvr::task { 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 7ec8dc77..85572300 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 @@ -12,8 +12,8 @@ #include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h" #include "cmvr/config/touch_screen_algorithm_config.pb.h" #include "manager/device_manager/include/device_manager.h" -#include "service/grpc/include/camera_operational_activity_registry.h" -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/server/include/camera_operational_activity_registry.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include namespace cmvr::task { diff --git a/cmvr-es/task/ume_teleop_task/include/ume_teleop_task.h b/cmvr-es/task/ume_teleop_task/include/ume_teleop_task.h index 9886e6ff..14353b8d 100644 --- a/cmvr-es/task/ume_teleop_task/include/ume_teleop_task.h +++ b/cmvr-es/task/ume_teleop_task/include/ume_teleop_task.h @@ -12,7 +12,7 @@ #include #include "cmvr/config/ume_teleop_config/ume_teleop_config.pb.h" -#include "service/arm_teleop_client/include/grpc_arm_teleop_client.h" +#include "service/grpc/client/include/grpc_arm_teleop_client.h" #include "task/task.h" namespace cmvr::task { diff --git a/cmvr-es/task/ume_teleop_task/src/ume_teleop_task.cpp b/cmvr-es/task/ume_teleop_task/src/ume_teleop_task.cpp index 4caec47d..c793f074 100644 --- a/cmvr-es/task/ume_teleop_task/src/ume_teleop_task.cpp +++ b/cmvr-es/task/ume_teleop_task/src/ume_teleop_task.cpp @@ -11,7 +11,7 @@ #include "cmvr/config/task_manager_config/task_manager_config.pb.h" #include "common/base/logging/logger.h" -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include "common/config/config_files.h" #include "task/task_factory.h" diff --git a/cmvr-es/task/ume_teleop_task/tests/ume_teleop_task_test.cpp b/cmvr-es/task/ume_teleop_task/tests/ume_teleop_task_test.cpp index a075ffcc..67766831 100644 --- a/cmvr-es/task/ume_teleop_task/tests/ume_teleop_task_test.cpp +++ b/cmvr-es/task/ume_teleop_task/tests/ume_teleop_task_test.cpp @@ -13,8 +13,8 @@ #include #include "cmvr/api/arm_teleop_v1.grpc.pb.h" -#include "service/arm_teleop_client/include/grpc_arm_teleop_client.h" -#include "service/stop_all/include/stop_all_admission_gate.h" +#include "service/grpc/client/include/grpc_arm_teleop_client.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include "task/ume_teleop_task/include/ume_teleop_task.h" namespace { diff --git a/docs/device_safety_control_plane_architecture.md b/docs/device_safety_control_plane_architecture.md index 693e53a5..bf49c37e 100644 --- a/docs/device_safety_control_plane_architecture.md +++ b/docs/device_safety_control_plane_architecture.md @@ -19,7 +19,7 @@ | --- | --- | --- | --- | | 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 日志评审 | +| 2 | 软件完成 | DeviceManager 持有 SafetyManager、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 和真机台架 | @@ -38,7 +38,7 @@ AUBO 另有一条设备内硬件语义：真实硬件急停曾有效、随后输 本设计采用以下核心决策： 1. `DeviceManager` 继续负责设备注册和生命周期，并持有一个独立、可测试的 - `SafetyCoordinator`。状态机代码不直接堆入 `DeviceManager`。 + `SafetyManager`。状态机代码不直接堆入 `DeviceManager`。 2. Service 不再自行组合 StopAll gate、设备状态和控制权判断。所有会改变设备或 活动状态的命令必须声明 `CommandIntent`，通过统一准入获得短生命周期的 `AdmissionPermit`。 @@ -58,7 +58,7 @@ AUBO 另有一条设备内硬件语义：真实硬件急停曾有效、随后输 10. 认证关闭时由服务端注入固定的 `anonymous` principal，不能接受客户端自报身份或角色； `RecoverSafetyState` 默认禁用，只有本机受限模式可以显式开放。 11. 传输加密、身份认证和方法授权是三个正交层。后续增加静态 Token、JWT 或 mTLS 时， - 只能替换安全网关组件，不修改设备、`SafetyCoordinator` 或业务 Proto。 + 只能替换安全网关组件，不修改设备、`SafetyManager` 或业务 Proto。 12. 该软件控制面不替代独立物理急停，也不声明 SIL、PL 或其他功能安全等级。 ## 2. 改造前基础与需要保留的行为 @@ -67,7 +67,7 @@ AUBO 另有一条设备内硬件语义：真实硬件急停曾有效、随后输 | 当前能力 | 目标用法 | | --- | --- | -| `ControlAuthorityManager` 的 lease、generation、dispatch fence、quarantine | 作为 `SafetyCoordinator` 内部控制权组件 | +| `ControlAuthorityManager` 的 lease、generation、dispatch fence、quarantine | 作为 `SafetyManager` 内部控制权组件 | | `StopAllAdmissionGate` 的并发 round 和失败后 fail-closed | 作为阶段 2、3 的兼容参与者，最终由统一状态机接管 | | `ActionQueueExecutor` 的 `action_id`、service instance 和精确 retired ID 账本 | 作为普通命令幂等账本的语义模板 | | Motor、Media、Camera activity coordinator | 先包装为 `SafetyParticipant`，最后逐步合并 | @@ -91,9 +91,9 @@ AUBO 另有一条设备内硬件语义：真实硬件急停曾有效、随后输 当前实现锚点： - 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`； +- SystemService StopAll：`cmvr-es/service/grpc/server/src/grpc_system_service.cpp`； +- 全局 gate：`cmvr-es/service/grpc/stop_all/include/stop_all_admission_gate.h`； +- 控制权：`cmvr-es/manager/control_authority_manager/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`。 @@ -132,7 +132,7 @@ AUBO 另有一条设备内硬件语义：真实硬件急停曾有效、随后输 6. 相同 ID、不同语义 payload 必须返回冲突，不能覆盖旧记录。 7. 硬件结果不确定时保存 `OUTCOME_UNKNOWN`，重试只能查询该结果，不能再次下发。 8. 恢复成功只表示软件准入可重新评估，不表示设备被上电、使能、解除急停或自动运动。 -9. `SafetyCoordinator` 持有内部锁时不得调用设备、网络或可能阻塞的 participant 方法。 +9. `SafetyManager` 持有内部锁时不得调用设备、网络或可能阻塞的 participant 方法。 10. 驱动最终安全检查失败时，即使已经获得 permit，也不能下发设备命令。 11. 进程重启后，控制设备在新鲜状态确认完成前不能自动恢复到可控制状态。 12. 所有安全状态转换都增加 epoch、产生事件，并记录明确 reason code。 @@ -262,7 +262,7 @@ flowchart LR Client["gRPC / QUIC / Local Task"] --> Gateway["Request Context Gateway
限制 可选认证 授权策略 审计"] Gateway --> Adapter["Typed Service Adapter
声明 CommandIntent"] Adapter --> Ledger["CommandLedger
幂等和结果"] - Adapter --> Coordinator["DeviceManager::SafetyCoordinator"] + Adapter --> Coordinator["DeviceManager::SafetyManager"] Coordinator --> Policy["SensorPolicy / ControlPolicy"] Coordinator --> Authority["ControlAuthorityManager"] Coordinator --> Participants["SafetyParticipant Registry"] @@ -276,7 +276,7 @@ flowchart LR ### 5.1 建议目录 ```text -cmvr-es/manager/safety/ +cmvr-es/manager/safety_manager/ include/safety_types.h include/safety_reason.h include/device_safety_endpoint.h @@ -285,7 +285,7 @@ cmvr-es/manager/safety/ include/command_admission_controller.h include/command_ledger.h include/safety_operation_orchestrator.h - include/safety_coordinator.h + include/safety_manager.h src/... tests/... @@ -299,7 +299,7 @@ cmvr-es/service/grpc/security/ tests/... ``` -新增 CMake target：`cmvr_es::safety_coordinator`。它可以依赖通用类型和 +新增 CMake target：`cmvr_es::safety_manager`。它可以依赖通用类型和 `ControlAuthorityManager`，但不能依赖 gRPC、具体设备后端或厂商 SDK。 ### 5.2 `DeviceManager` 的职责变化 @@ -307,8 +307,8 @@ cmvr-es/service/grpc/security/ `DeviceManager` 增加： ```cpp -SafetyCoordinator& safetyCoordinator() noexcept; -const SafetyCoordinator& safetyCoordinator() const noexcept; +SafetyManager& safetyManager() noexcept; +const SafetyManager& safetyManager() const noexcept; ``` 它负责： @@ -332,7 +332,7 @@ const SafetyCoordinator& safetyCoordinator() const noexcept; ```cpp DeviceManager& -SafetyCoordinator& +SafetyManager& CommandLedger& GrpcSecurityGateway& ``` @@ -353,7 +353,7 @@ gRPC 管理面仍能启动。目标 Runtime 启动顺序为： ```text load config and logging - -> construct DeviceManager core and SafetyCoordinator + -> construct DeviceManager core and SafetyManager -> validate selected gRPC security profile and start management-plane services -> initialize/start devices -> reconcile required control snapshots @@ -361,7 +361,7 @@ load config and logging -> Open with device-level Blocked, or global Latched ``` -- 配置损坏、所选安全 Profile 的身份材料无效或 SafetyCoordinator 核心构造失败仍使进程启动失败； +- 配置损坏、所选安全 Profile 的身份材料无效或 SafetyManager 核心构造失败仍使进程启动失败； - `AuthenticationMode::Disabled` 是显式兼容模式，不伪装成“已认证”；如果使用非 loopback 明文监听，启动日志、GetSystemInfo 和指标必须持续暴露该风险； - 单个设备 create/init/start 失败记录在 inventory，并使相关资源 Blocked； @@ -497,7 +497,7 @@ ACK 或写入总线分别可能具有不同强度，不能统一解释成“运 3. CommandLedger 按 effective principal 预留 ID，并计算语义 payload hash；Disabled 模式使用 服务端固定的 `anonymous` namespace。 4. 若存在同 ID 记录：相同 hash 返回/等待原结果；不同 hash 返回冲突。 -5. Service 使用固定 `CommandDescriptor` 调用 `SafetyCoordinator::admit()`。 +5. Service 使用固定 `CommandDescriptor` 调用 `SafetyManager::admit()`。 6. Coordinator 在短锁内读取 global state、safety epoch、device slot 和 cached snapshot。 7. Policy 判断意图、freshness、硬件事实、设备状态和调用角色。 8. Control intent 获取或校验 `ControlAuthorityManager` lease。 @@ -615,7 +615,7 @@ struct DeviceSafetyRegistration { - 配置要求严格启动时，可以直接使 Runtime 初始化失败。 新增全新 DeviceKind 仍可能需要修改现有 DeviceFactory、配置 Proto 和对外业务 API；本设计 -保证的是不再修改 SafetyCoordinator、StopAll 和 Recover 的设备类型分支。 +保证的是不再修改 SafetyManager、StopAll 和 Recover 的设备类型分支。 ### 10.2 Endpoint @@ -667,7 +667,7 @@ public: }; ``` -participant 可以代表设备，也可以代表 ActionQueue、MediaSourceHub、Motor session registry 等 +participant 可以代表设备，也可以代表 ActionQueue、MediaSourceManager、Motor session registry 等 跨设备活动域。StopAll 不再知道具体 C++ 设备类型。 ## 11. CommandLedger 与结果语义 @@ -1046,7 +1046,7 @@ public: 但不能依赖 thread-local 在 interceptor 和 handler 之间传递身份；同步、异步和 callback RPC 都必须 具有明确的 per-call 所有权。 -`SafetyCoordinator` 不依赖 gRPC 类型。Service 只把从 `RequestContext` 派生的稳定 +`SafetyManager` 不依赖 gRPC 类型。Service 只把从 `RequestContext` 派生的稳定 `CommandActor`/capability 传给准入和 ledger；驱动层完全不可见认证方式。 ### 16.3 部署 Profile @@ -1155,7 +1155,7 @@ Profile 决定。 以下内容不应因认证升级而修改： - 设备命令 request/feedback Proto； -- `DeviceManager`、`SafetyCoordinator`、Sensor/Control policy； +- `DeviceManager`、`SafetyManager`、Sensor/Control policy； - `DeviceSafetyEndpoint`、`SafetyParticipant` 和厂商驱动； - handler 内的命令准入、StopAll 或 Recover 业务分支。 @@ -1165,12 +1165,12 @@ Profile 决定。 ## 17. 配置设计 -### 17.1 SafetyCoordinatorConfig +### 17.1 SafetyManagerConfig 建议在 `DeviceManagerConfig` 中增加： ```protobuf -message SafetyCoordinatorConfig { +message SafetyManagerConfig { enum EnforcementMode { ENFORCEMENT_MODE_UNSPECIFIED = 0; LEGACY = 1; @@ -1295,7 +1295,7 @@ capability manifest/SystemInfo。 | 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 capture | Sensor | opened、streaming、worker generation | 复用 MediaSourceManager；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 始终允许；不视为机械执行器 | @@ -1314,7 +1314,7 @@ Aubo JSON `get_di/get_do` 可以归类 Observe；`set_do` 必须归类 Configure | --- | --- | --- | | 0 | RequestContext、AuthN/AuthZ/Audit 扩展边界 | 否，显式保持现有兼容行为 | | 1 | 类型、快照、错误、幂等契约 | 否，旧 gate 仍权威 | -| 2 | SafetyCoordinator shadow | 否，只比较决策 | +| 2 | SafetyManager shadow | 否，只比较决策 | | 3 | 高风险设备逐个 enforce | 仅改变选中设备 | | 4 | 泛化 StopAll、正式 Recover | 改变系统安全事务 | | 5 | EnforceAll、真机签字、移除旧路径 | 全量切换 | @@ -1386,7 +1386,7 @@ Aubo JSON `get_di/get_do` 可以归类 Observe；`set_do` 必须归类 Configure #### 实施项 -1. 新建 `manager/safety` target 和 `safety_types.h`。 +1. 新建 `manager/safety_manager` target 和 `safety_types.h`。 2. 定义 CommandIntent、SafetyCondition、TriState、SafetyBlocker、SafetySnapshot。 3. 定义 DeviceSafetyDescriptor、Endpoint、Participant 接口。 4. 在 DeviceManager 中建立 `SafetySnapshotStore`，Manager snapshot 只读缓存。 @@ -1420,7 +1420,7 @@ Aubo JSON `get_di/get_do` 可以归类 Observe；`set_do` 必须归类 Configure 新 Proto 字段不可删除或复用；可以停止使用新字段，但必须保留 wire schema。SnapshotStore 可以退回仅诊断模式，不影响旧 gate。 -### 阶段 2：SafetyCoordinator 影子运行 +### 阶段 2：SafetyManager 影子运行 #### 目标 @@ -1428,7 +1428,7 @@ Aubo JSON `get_di/get_do` 可以归类 Observe；`set_do` 必须归类 Configure #### 实施项 -1. DeviceManager 构造并持有 SafetyCoordinator。 +1. DeviceManager 构造并持有 SafetyManager。 2. 在阶段 0 的 GrpcMethodPolicyRegistry 中补齐 CommandIntent，并引入 typed CommandDescriptor。 3. gRPC service 增加可注入构造函数；GrpcServerTask 统一传入 coordinator/ledger，沿用既有 GrpcSecurityGateway。 @@ -1476,7 +1476,7 @@ Aubo JSON `get_di/get_do` 可以归类 Observe；`set_do` 必须归类 Configure #### 目标 让 Arm、Teleoperation、AGV、Motor、DexHand control 和 Camera PTZ 的普通命令由 -SafetyCoordinator 权威准入，并统一幂等与执行结果语义。 +SafetyManager 权威准入，并统一幂等与执行结果语义。 #### 迁移顺序 @@ -1527,7 +1527,7 @@ SafetyCoordinator 权威准入，并统一幂等与执行结果语义。 - 每类命令都有结构化结果，Internal 不再承载所有业务错误； - 旧 gate 只作为兼容 deny，不再能单独 reopen 新 Coordinator； - 设备级 OutcomeUnknown 可通过 GetSafetyState 定位并进入恢复流程； -- 新控制设备接入安全层不需要修改 SafetyCoordinator switch。 +- 新控制设备接入安全层不需要修改 SafetyManager switch。 #### 回滚边界 @@ -1761,7 +1761,7 @@ Coordinator 保存有界内存事件环，并将关键事件写入持久审计 4. Safety types、snapshot store 和 tests； 5. CommandHeader/reason code additive Proto； 6. CommandLedger； -7. SafetyCoordinator shadow 和 GetSafetyState； +7. SafetyManager shadow 和 GetSafetyState； 8. legacy participant adapters； 9. Arm + ArmTeleop migration； 10. AGV migration； 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 6c2e1ad0..ff60aac7 100644 --- a/protos/cmvr/config/device_manager_config/device_manager_config.proto +++ b/protos/cmvr/config/device_manager_config/device_manager_config.proto @@ -1,7 +1,7 @@ syntax = "proto3"; package cmvr.config; -message SafetyCoordinatorConfig { +message SafetyManagerConfig { enum EnforcementMode { ENFORCEMENT_MODE_UNSPECIFIED = 0; LEGACY = 1; @@ -58,7 +58,7 @@ message DeviceManagerConfig { string description = 3; repeated DeviceConfigEntry devices = 4; bool init_all_motors_when_no_active_joints = 20; - SafetyCoordinatorConfig safety = 21; + SafetyManagerConfig safety = 21; } message DeviceManagerRootConfig { DeviceManagerConfig device_manager = 1; diff --git a/protos/cmvr/config/quic_edge_config/quic_edge_config.proto b/protos/cmvr/config/quic_edge_config/quic_edge_config.proto index 8e01ef93..8a26f1d8 100644 --- a/protos/cmvr/config/quic_edge_config/quic_edge_config.proto +++ b/protos/cmvr/config/quic_edge_config/quic_edge_config.proto @@ -36,7 +36,7 @@ message QuicEdgeTrackConfig { // Zero uses QuicEdgeConfig.maximum_frame_bytes. uint32 max_frame_bytes = 5; - // Protocol-neutral MediaSourceHub track ID. When empty, the service derives + // Protocol-neutral MediaSourceManager track ID. When empty, the service derives // the default ID from source_kind and device_id (for example // "right_hand_cam/video/color"). The Hub's descriptor is the authority for // codec metadata and descriptor generation. diff --git a/protos/cmvr/quic_edge/v1/README.md b/protos/cmvr/quic_edge/v1/README.md index f34377e8..5874f64f 100644 --- a/protos/cmvr/quic_edge/v1/README.md +++ b/protos/cmvr/quic_edge/v1/README.md @@ -70,7 +70,7 @@ MsQuic/TLS/UDP and implements the v1 control and DATAGRAM receiver. With the strict build above, CTest registers: - `cmvr_quic_msquic_e2e_test`, which connects the production edge service and - feeds synthetic H.264 video plus AAC audio through `MediaSourceHub`, then + feeds synthetic H.264 video plus AAC audio through `MediaSourceManager`, then verifies registration, heartbeat, descriptors, DATAGRAMs and frame reassembly; - `cmvr_es_quic_process_smoke_test`, which starts the actual `cmvr_es` @@ -216,7 +216,7 @@ heartbeat therefore continue while a media source is slow or unavailable; the reliable send path serializes heartbeat and media metadata so envelope sequence order remains strict. -`MediaSourceHub` is protocol-neutral and gives each adapter an independent, +`MediaSourceManager` is protocol-neutral and gives each adapter an independent, single-consumer subscription cursor. Its source-start callback receives a cancellation predicate and must check it around potentially blocking device startup. QUIC reconnect/stop and gRPC client cancellation can therefore abandon diff --git a/protos/cmvr/quic_edge/v1/quic_edge.proto b/protos/cmvr/quic_edge/v1/quic_edge.proto index 48f3a321..6003fd14 100644 --- a/protos/cmvr/quic_edge/v1/quic_edge.proto +++ b/protos/cmvr/quic_edge/v1/quic_edge.proto @@ -197,7 +197,7 @@ message MediaTrackDescriptor { string codec = 4; uint64 codec_generation = 5; - // The exact MediaSourceHub track and the 32-bit token repeated in every + // The exact MediaSourceManager track and the 32-bit token repeated in every // DATAGRAM header. The full generation remains on the reliable stream. string source_track_id = 6; uint32 codec_generation_token = 7; diff --git a/test/e2e/CMakeLists.txt b/test/e2e/CMakeLists.txt index 88649bd6..15b4b148 100644 --- a/test/e2e/CMakeLists.txt +++ b/test/e2e/CMakeLists.txt @@ -9,9 +9,9 @@ if(NOT TARGET cmvr_es::quic_test_gateway) endif() if(NOT TARGET cmvr_es::quic_edge_service OR - NOT TARGET cmvr_es::media_source_hub) + NOT TARGET cmvr_es::media_source_manager) message(FATAL_ERROR - "Real QUIC E2E requires the production QUIC service and MediaSourceHub") + "Real QUIC E2E requires the production QUIC service and MediaSourceManager") endif() add_executable(cmvr_quic_msquic_e2e_test @@ -22,7 +22,7 @@ target_link_libraries(cmvr_quic_msquic_e2e_test PRIVATE cmvr_es::quic_test_gateway cmvr_es::quic_edge_service - cmvr_es::media_source_hub + cmvr_es::media_source_manager ) if(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang") diff --git a/test/e2e/README.md b/test/e2e/README.md index e0e4e7ba..e88e8e84 100644 --- a/test/e2e/README.md +++ b/test/e2e/README.md @@ -11,7 +11,7 @@ fake transport，也不要求连接物理设备。 | `cmvr_quic_msquic_e2e_test` | 测试进程内同时运行 Gateway 和生产 `QuicEdgeService` | TLS/ALPN、注册、DeviceManager 合成快照、至少两次心跳 ACK、H.264/AAC descriptor 精确字段、真实 DATAGRAM、分片、序列号、flags、长度与载荷哈希 | | `cmvr_es_quic_process_smoke_test` | 分别启动测试 Gateway 和真实 `cmvr_es` 子进程 | 临时配置加载、`QuicEdgeTask` 工厂和生命周期、节点注册、IP、禁用设备过滤、已启用设备创建失败上报、本地心跳周期、至少两次心跳 ACK、SIGTERM 安全退出 | -第一项向生产 `MediaSourceHub` 注册两个有界 synthetic source： +第一项向生产 `MediaSourceManager` 注册两个有界 synthetic source： - 2500 字节的 H.264 Annex B IDR 视频帧，用于覆盖 DATAGRAM 分片； - 带 ADTS header 的 AAC-LC 48 kHz 双声道音频帧。 diff --git a/test/e2e/quic_msquic_e2e_test.cpp b/test/e2e/quic_msquic_e2e_test.cpp index daf9ec50..c967dcec 100644 --- a/test/e2e/quic_msquic_e2e_test.cpp +++ b/test/e2e/quic_msquic_e2e_test.cpp @@ -15,7 +15,7 @@ #include #include "common/media/media_frame.h" -#include "manager/media_source_hub/include/media_source_hub.h" +#include "manager/media_source_manager/include/media_source_manager.h" #include "service/quic_edge/include/quic_edge_service.h" #include "service/quic_edge/include/quic_transport.h" @@ -26,7 +26,7 @@ using cmvr::media::Codec; using cmvr::media::MediaFrame; using cmvr::media::MediaFramePtr; using cmvr::media::MediaKind; -using cmvr::media::MediaSourceHub; +using cmvr::media::MediaSourceManager; using cmvr::media::PayloadFormat; using cmvr::media::TrackDescriptor; using cmvr::media::TrackDescriptorPtr; @@ -171,12 +171,12 @@ public: { } - MediaSourceHub::SourceCallbacks callbacks(const bool video) + MediaSourceManager::SourceCallbacks callbacks(const bool video) { - MediaSourceHub::SourceCallbacks callbacks; + MediaSourceManager::SourceCallbacks callbacks; callbacks.start = [this]( - const MediaSourceHub::FrameSink& sink, - const MediaSourceHub::CancelPredicate& cancelled) { + const MediaSourceManager::FrameSink& sink, + const MediaSourceManager::CancelPredicate& cancelled) { if (!sink || (cancelled && cancelled())) return false; std::lock_guard lock(mutex_); sink_ = sink; @@ -203,7 +203,7 @@ public: const bool key_frame, const bool discontinuity = false) { - MediaSourceHub::FrameSink sink; + MediaSourceManager::FrameSink sink; { std::lock_guard lock(mutex_); if (!running_ || !sink_) return false; @@ -234,7 +234,7 @@ public: private: TrackDescriptorPtr descriptor_; mutable std::mutex mutex_; - MediaSourceHub::FrameSink sink_; + MediaSourceManager::FrameSink sink_; bool running_{false}; std::uint64_t key_frame_requests_{0U}; }; @@ -443,12 +443,12 @@ int run(const Arguments& arguments) const TrackDescriptorPtr audio_descriptor = makeAudioDescriptor(); SyntheticSource video(video_descriptor); SyntheticSource audio(audio_descriptor); - MediaSourceHub hub; + MediaSourceManager hub; if (!hub.registerSource( video_descriptor, video.callbacks(true), 8U) || !hub.registerSource( audio_descriptor, audio.callbacks(false), 8U)) { - std::cerr << "failed to register synthetic MediaSourceHub tracks\n"; + std::cerr << "failed to register synthetic MediaSourceManager tracks\n"; return 1; } diff --git a/test/quic_gateway/README.md b/test/quic_gateway/README.md index fbcdc294..713cc054 100644 --- a/test/quic_gateway/README.md +++ b/test/quic_gateway/README.md @@ -287,7 +287,7 @@ frames_completed >= 1 关闭全部设备时，实际 `output/bin/cmvr_es` 可以完整验证 TLS、ALPN、控制 stream、 注册、IP 上报、心跳和重连，但不会产生音视频。 -要在无硬件环境验证媒体 DATAGRAM，测试侧还需要 synthetic MediaSourceHub producer +要在无硬件环境验证媒体 DATAGRAM，测试侧还需要 synthetic MediaSourceManager producer 或测试专用 fake camera/microphone。该 Gateway 已具备媒体接收和重组能力，但不会 伪造 Edge 发出的媒体。 From 1b3cd55505a71517167330a3f9d61d9d64d1ad11 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Mon, 17 Aug 2026 12:16:20 +0800 Subject: [PATCH 8/8] feat(aubo): auto recover after hardware estop release --- cmvr-es/config/devices/arm/aubo_arm.pb.txt | 1 + cmvr-es/devices/arm/aubo_arm/README.md | 17 +- cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp | 313 +++++++++++++++--- .../devices/arm/aubo_arm/aubo_safety_state.h | 16 +- .../aubo_arm/tests/aubo_safety_state_test.cpp | 22 +- ...evice_safety_control_plane_architecture.md | 10 +- .../cmvr/config/arm_config/arm_config.proto | 3 + 7 files changed, 309 insertions(+), 73 deletions(-) diff --git a/cmvr-es/config/devices/arm/aubo_arm.pb.txt b/cmvr-es/config/devices/arm/aubo_arm.pb.txt index 551f31df..cd8f3803 100644 --- a/cmvr-es/config/devices/arm/aubo_arm.pb.txt +++ b/cmvr-es/config/devices/arm/aubo_arm.pb.txt @@ -17,6 +17,7 @@ arm { tool_frame: "tool0" username: "aubo" password: "123456" + auto_power_on_after_hardware_estop_release: true } } } diff --git a/cmvr-es/devices/arm/aubo_arm/README.md b/cmvr-es/devices/arm/aubo_arm/README.md index 669aa42d..f895c6cd 100644 --- a/cmvr-es/devices/arm/aubo_arm/README.md +++ b/cmvr-es/devices/arm/aubo_arm/README.md @@ -108,16 +108,21 @@ cmake --install build 所有 Move、Speed、Servo 和程序启动请求均按不安全状态拒绝； - 硬件急停会立即使当前运动 generation 失效，并在急停输入有效期间保持锁存。 检测到硬件急停输入消失且控制器重新报告 `Normal`/`ReducedMode` 后，后端应 - 自动执行安全恢复确认；防护停机和 Safety Fault/Violation 仍保持显式恢复语义； + 自动执行 `poweron()` 和 `startup()`，恢复到 `Running` 后再完成安全确认并开放新的 + gRPC 控制指令；防护停机和 Safety Fault/Violation 仍保持显式恢复语义； - `emergencyStop()` 使用独立的 `SoftwareEmergencyStop` 锁存。即使软件急停在真实 硬件急停有效期间触发，后续硬件采样也不能覆盖该锁存，释放硬件急停开关不会 自动清除软件急停；它只能通过显式安全恢复流程解除； - 锁存后会终止直接运动与程序、关闭 servo 模式并清理控制器轨迹。硬件急停 - 自动恢复只有在确认 `ExecId == -1`、普通队列和轨迹队列均为空、运行时已停止 - 且机械臂稳定后才能解除锁存；如果自动确认失败，则继续保持 fail-closed， - 并允许通过 `torqueOn`/`clearFault`/`unlockProtectiveStop` 显式重试恢复； -- 恢复流程不会调用 `resume`、`arbitraryResume`、`startMove`，也不会重新提交 - 急停前的目标、速度、servo 指令或程序； + 自动恢复先上电到 `Idle`，在刹车释放前清理 runtime、servo 和轨迹队列，再执行 + `startup()`；到达 `Running` 后还会再次确认 `ExecId == -1`、普通队列和轨迹队列 + 均为空、运行时已停止且机械臂稳定，全部成立后才能解除锁存； +- 当前 AUBO 配置通过 `auto_power_on_after_hardware_estop_release: true` 显式启用自动 + 上电。自动确认失败时继续保持 fail-closed，并允许通过 `torqueOn`/`clearFault`/ + `unlockProtectiveStop` 显式重试；本轮释放期间收到 `stopMotion()` 或 `torqueOff()` + 会取消自动上电，显式停止始终优先； +- 恢复流程只调用 `poweron()` 和 `startup()`，不会调用 `resume`、`arbitraryResume`、 + `startMove`，也不会重新提交急停前的目标、速度、servo 指令或程序； - AUBO SDK 未在本地文档中保证急停期间 `clearPath` 的可用性，也未说明释放 急停开关后的控制器恢复时序。因此自动恢复必须在释放后再次清队列并完成上述 安全确认；无法确认时不得解除锁存。“释放开关后零位移”的最终保证仍需真机 diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index d92619e6..4c0da356 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -175,11 +175,13 @@ public: } bool completeHardwareEmergencyStop( + const bool robot_running, const bool controller_idle, const bool cancellation_confirmed) { if (!state_->completeHardwareEmergencyStopRecovery( - token_, controller_idle, cancellation_confirmed)) { + token_, robot_running, controller_idle, + cancellation_confirmed)) { return false; } completed_ = true; @@ -392,6 +394,7 @@ struct AuboSafetyMonitor final { static_cast(RuntimeState::Stopped)}; std::atomic emergency_stop_source{-1}; std::atomic hardware_emergency_stop_latched{false}; + std::atomic automatic_recovery_suppressed{false}; std::atomic servo_mode_select{0}; std::atomic last_sample_ns{0}; std::atomic cancellation_confirmed{true}; @@ -403,6 +406,8 @@ struct AuboSafetyMonitor final { std::condition_variable wait_cv; std::mutex termination_mutex; std::recursive_mutex command_rpc_mutex; + bool auto_power_on_after_hardware_estop_release{false}; + std::function on_hardware_estop_auto_recovered; std::string arm_id; }; @@ -466,7 +471,11 @@ void publishSafetySample( monitor->last_sample_ns.store(monotonicNowNs()); if (emergency_stop_source != 0) { - monitor->hardware_emergency_stop_latched.store(true); + const bool first_sample_for_event = + !monitor->hardware_emergency_stop_latched.exchange(true); + if (first_sample_for_event) { + monitor->automatic_recovery_suppressed.store(false); + } } else if (!current.latched) { monitor->hardware_emergency_stop_latched.store(false); } @@ -568,6 +577,8 @@ bool enforceControllerTermination( const std::shared_ptr& rpc_client, const std::shared_ptr& monitor) { + std::unique_lock command_rpc_lock( + monitor->command_rpc_mutex); std::unique_lock termination_lock(monitor->termination_mutex); monitor->motion_state->cancelActiveForSafety(); auto stop_request = monitor->motion_state->beginStop(); @@ -854,6 +865,185 @@ bool controllerStillQuiescent( RuntimeState::Stopped; } +bool hardwareEmergencyStopRecoveryCurrent( + const std::shared_ptr& monitor, + const aubo_internal::RecoveryToken token) +{ + const auto snapshot = monitor->safety_state->snapshot(); + return token.valid() && !monitor->stop_requested.load() && + !monitor->automatic_recovery_suppressed.load() && + monitor->emergency_stop_source.load() == 0 && snapshot.latched && + snapshot.recovery_in_progress && snapshot.epoch == token.epoch && + !snapshot.software_emergency_stop_latched && + snapshot.latched_reason == + aubo_internal::SafetyCondition::RobotEmergencyStop && + aubo_internal::isMotionSafe(snapshot.observed); +} + +bool waitForHardwareEmergencyStopRecoveryMode( + const RobotInterfacePtr& robot_interface, + const std::shared_ptr& monitor, + const aubo_internal::RecoveryToken token, + const RobotModeType target_mode) +{ + const auto deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(20); + while (std::chrono::steady_clock::now() < deadline) { + if (!hardwareEmergencyStopRecoveryCurrent(monitor, token)) { + return false; + } + if (robot_interface->getRobotState()->getRobotModeType() == + target_mode) { + return true; + } + if (monitorWait(monitor, std::chrono::milliseconds(100))) { + return false; + } + } + return false; +} + +bool autoPowerOnAfterHardwareEmergencyStop( + const std::shared_ptr& rpc_client, + const std::shared_ptr& monitor, + const RobotInterfacePtr& robot_interface) +{ + std::unique_lock command_rpc_lock( + monitor->command_rpc_mutex); + refreshSafetySample(rpc_client, monitor, robot_interface); + const auto snapshot = monitor->safety_state->snapshot(); + if (!aubo_internal::shouldAutoRecoverHardwareEmergencyStop( + snapshot, + monitor->hardware_emergency_stop_latched.load(), + monitor->emergency_stop_source.load(), + monitor->auto_power_on_after_hardware_estop_release, + monitor->automatic_recovery_suppressed.load())) { + return false; + } + + const auto token = monitor->safety_state->beginRecovery(snapshot.epoch); + if (!token.has_value()) { + return false; + } + SafetyRecoveryGuard recovery{monitor->safety_state, *token}; + const auto fail = [&monitor](const std::string& detail) { + CMVR_LOG(WARNING) + << "[AuboArm] hardware emergency-stop automatic power-on " + "failed, id=" + << monitor->arm_id << ", detail=" << detail; + return false; + }; + + try { + cancelForSafetyTransition(monitor); + if (!hardwareEmergencyStopRecoveryCurrent(monitor, *token)) { + return fail("recovery was cancelled before controller setup"); + } + + double mass = 0.0; + std::vector cog(3, 0.0); + std::vector aom(3, 0.0); + std::vector inertia(6, 0.0); + const int payload_ret = robot_interface->getRobotConfig()->setPayload( + mass, cog, aom, inertia); + if (payload_ret != arcs::common_interface::AUBO_OK) { + return fail("setPayload ret=" + std::to_string(payload_ret)); + } + if (!hardwareEmergencyStopRecoveryCurrent(monitor, *token)) { + return fail("recovery was cancelled after payload setup"); + } + + auto current_mode = + robot_interface->getRobotState()->getRobotModeType(); + if (current_mode != RobotModeType::Running && + current_mode != RobotModeType::Idle) { + const int power_on_ret = + robot_interface->getRobotManage()->poweron(); + if (power_on_ret != arcs::common_interface::AUBO_OK) { + return fail("poweron ret=" + std::to_string(power_on_ret)); + } + if (!waitForHardwareEmergencyStopRecoveryMode( + robot_interface, monitor, *token, + RobotModeType::Idle)) { + return fail("Idle was not reached after poweron"); + } + current_mode = RobotModeType::Idle; + } + + refreshSafetySample(rpc_client, monitor, robot_interface); + if (!hardwareEmergencyStopRecoveryCurrent(monitor, *token)) { + return fail("safety state changed before brake release"); + } + + const bool cleanup_ok = current_mode == RobotModeType::Running + ? enforceControllerTermination(rpc_client, monitor) + : prepareControllerForStartup(rpc_client, monitor); + if (!cleanup_ok) { + return fail("old runtime, servo, or path state could not be cleared"); + } + if (!hardwareEmergencyStopRecoveryCurrent(monitor, *token)) { + return fail("safety state changed during pre-startup cleanup"); + } + + if (current_mode != RobotModeType::Running) { + const int startup_ret = + robot_interface->getRobotManage()->startup(); + if (startup_ret != arcs::common_interface::AUBO_OK) { + return fail("startup ret=" + std::to_string(startup_ret)); + } + if (!waitForHardwareEmergencyStopRecoveryMode( + robot_interface, monitor, *token, + RobotModeType::Running)) { + return fail("Running was not reached after startup"); + } + } + + refreshSafetySample(rpc_client, monitor, robot_interface); + if (!hardwareEmergencyStopRecoveryCurrent(monitor, *token) || + monitor->robot_mode.load() != + static_cast(RobotModeType::Running)) { + return fail("controller safety changed during startup"); + } + + // Startup is allowed to energize the arm, but it must not revive an + // old controller operation. Terminate once more in Running mode and + // require a fresh empty/steady observation before reopening commands. + cancelForSafetyTransition(monitor); + if (!enforceControllerTermination(rpc_client, monitor)) { + return fail("post-startup controller quiescence was not confirmed"); + } + refreshSafetySample(rpc_client, monitor, robot_interface); + const bool robot_running = + monitor->robot_mode.load() == + static_cast(RobotModeType::Running); + const bool controller_idle = + robot_running && + hardwareEmergencyStopRecoveryCurrent(monitor, *token) && + controllerStillQuiescent(rpc_client, robot_interface); + if (!recovery.completeHardwareEmergencyStop( + robot_running, + controller_idle, + monitor->cancellation_confirmed.load())) { + return fail("safety epoch changed before recovery commit"); + } + + monitor->hardware_emergency_stop_latched.store(false); + monitor->automatic_recovery_suppressed.store(false); + if (monitor->on_hardware_estop_auto_recovered) { + monitor->on_hardware_estop_auto_recovered(); + } + CMVR_LOG(INFO) + << "[AuboArm] hardware emergency-stop release automatically " + "powered on and enabled, id=" + << monitor->arm_id; + return true; + } catch (const std::exception& error) { + return fail(error.what()); + } catch (...) { + return fail("unknown exception"); + } +} + void runSafetyMonitor( const std::shared_ptr& monitor, const std::string& ip, @@ -896,44 +1086,15 @@ void runSafetyMonitor( monitor ->hardware_emergency_stop_latched .load(), - monitor->emergency_stop_source.load()); + monitor->emergency_stop_source.load(), + monitor + ->auto_power_on_after_hardware_estop_release, + monitor + ->automatic_recovery_suppressed + .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; - } - } + (void)autoPowerOnAfterHardwareEmergencyStop( + rpc_client, monitor, robot_interface); safety = monitor->safety_state->snapshot(); } @@ -1996,6 +2157,8 @@ Result AuboArm::torqueOn( } emergency_stopped_.store(false); servo_mode_.store(false); + monitor->hardware_emergency_stop_latched.store(false); + monitor->automatic_recovery_suppressed.store(false); if (const auto cancelled = cancellation_result()) { return *cancelled; } @@ -2021,22 +2184,44 @@ Result AuboArm::torqueOn( Result AuboArm::torqueOff() { - const auto ready = ensureConnected_("torqueOff"); - if (!ready.ok()) { - return ready; + std::shared_ptr rpc_client; + std::shared_ptr monitor; + { + std::lock_guard lock(mutex_); + const auto ready = ensureConnected_("torqueOff"); + if (!ready.ok()) { + return ready; + } + rpc_client = sdk_->rpc_client; + monitor = sdk_->safety_monitor; + if (monitor) { + monitor->automatic_recovery_suppressed.store(true); + } } try { - const auto robot_names = sdk_->rpc_client->getRobotNames(); - if (robot_names.empty()) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); + std::unique_lock command_rpc_lock; + if (monitor) { + command_rpc_lock = std::unique_lock( + monitor->command_rpc_mutex); } - auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); - if (!robot_interface) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); + Result interface_result; + auto robot_interface = getPrimaryRobotInterface( + rpc_client, "torqueOff", interface_result); + if (!interface_result.ok()) { + return interface_result; } - robot_interface->getRobotManage()->poweroff(); - if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::PowerOff)) { + const int power_off_ret = + robot_interface->getRobotManage()->poweroff(); + if (power_off_ret != arcs::common_interface::AUBO_OK) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOff failed: poweroff ret=" + + std::to_string(power_off_ret)); + } + if (!waitForRobotMode( + robot_interface, + arcs::common_interface::RobotModeType::PowerOff)) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOff failed: timeout waiting for PowerOff"); } return Result::success(); @@ -2582,6 +2767,13 @@ Result AuboArm::stopMotion_( } const auto motion_state = sdk_->motion_state; + const auto monitor = sdk_->safety_monitor; + std::unique_lock command_rpc_lock; + if (monitor) { + monitor->automatic_recovery_suppressed.store(true); + command_rpc_lock = std::unique_lock( + monitor->command_rpc_mutex); + } aubo_internal::MotionKind forced_kind = aubo_internal::MotionKind::None; if (requested_kind == MotionStopKind::Joint) { @@ -3025,6 +3217,14 @@ Result AuboArm::connect(const std::string& ip, const int port) sdk_state->safety_monitor->motion_state = sdk_state->motion_state; sdk_state->safety_monitor->arm_id = id_; + sdk_state->safety_monitor + ->auto_power_on_after_hardware_estop_release = + vendor_cfg_.auto_power_on_after_hardware_estop_release(); + sdk_state->safety_monitor->on_hardware_estop_auto_recovered = + [this] { + busy_.store(false); + servo_mode_.store(false); + }; publishSafetySample( sdk_state->safety_monitor, robot_interface->getRobotState()->getSafetyModeType(), @@ -3709,12 +3909,21 @@ Result AuboArm::ensureMotionReady_( condition == aubo_internal::SafetyCondition::Violation) { code = ArmErrorCode::RobotInFault; } + std::string recovery_instruction = + "; clear the hardware condition and perform explicit recovery"; + if (condition == + aubo_internal::SafetyCondition::RobotEmergencyStop && + monitor->auto_power_on_after_hardware_estop_release && + !monitor->automatic_recovery_suppressed.load()) { + recovery_instruction = + "; release the physical emergency stop and wait for " + "automatic power-on recovery"; + } return Result::failure( code, "[AuboArm] " + context + " rejected: hardware safety latch is " + - safetyConditionName(condition) + - "; clear the hardware condition and perform explicit recovery"); + safetyConditionName(condition) + recovery_instruction); } if (monitor->robot_mode.load() != 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 54b5e750..b11fb94d 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h @@ -81,9 +81,12 @@ struct SafetySnapshot { inline bool shouldAutoRecoverHardwareEmergencyStop( const SafetySnapshot& snapshot, const bool hardware_emergency_stop_was_observed, - const int current_emergency_stop_source) noexcept + const int current_emergency_stop_source, + const bool auto_power_on_enabled, + const bool automatic_recovery_suppressed) noexcept { - return hardware_emergency_stop_was_observed && snapshot.latched && + return auto_power_on_enabled && !automatic_recovery_suppressed && + hardware_emergency_stop_was_observed && snapshot.latched && !snapshot.recovery_in_progress && !snapshot.software_emergency_stop_latched && snapshot.latched_reason == SafetyCondition::RobotEmergencyStop && @@ -175,10 +178,13 @@ public: 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. + // The caller may clear the physical E-stop latch only after it has powered + // the controller, released the brakes, and then re-confirmed an empty, + // steady controller in Running mode. This never authorizes replaying the + // old target, runtime program, or servo session. bool completeHardwareEmergencyStopRecovery( const RecoveryToken token, + const bool robot_running, const bool controller_idle, const bool cancellation_confirmed) { @@ -187,7 +193,7 @@ public: !recovery_in_progress_ || software_emergency_stop_latched_ || latched_reason_ != SafetyCondition::RobotEmergencyStop || - !isMotionSafe(observed_) || !controller_idle || + !isMotionSafe(observed_) || !robot_running || !controller_idle || !cancellation_confirmed) { return false; } 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 8ae17007..716d2b00 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 @@ -54,9 +54,13 @@ int main() CHECK_TRUE(state.snapshot().latched); CHECK_TRUE(!state.tryPermit().has_value()); CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop( - state.snapshot(), false, 0)); + state.snapshot(), false, 0, true, false)); CHECK_TRUE(shouldAutoRecoverHardwareEmergencyStop( - state.snapshot(), true, 0)); + state.snapshot(), true, 0, true, false)); + CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop( + state.snapshot(), true, 0, false, false)); + CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop( + state.snapshot(), true, 0, true, true)); const auto recovery = state.beginRecovery(state.snapshot().epoch); CHECK_TRUE(recovery.has_value()); @@ -65,8 +69,14 @@ int main() const auto retry = state.beginRecovery(state.snapshot().epoch); CHECK_TRUE(retry.has_value()); + // Automatic release is not committed at Idle/PowerOn. The controller + // must have completed startup and reached Running first. + CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery( + *retry, false, true, true)); + CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery( + *retry, true, false, true)); CHECK_TRUE(state.completeHardwareEmergencyStopRecovery( - *retry, true, true)); + *retry, true, true, true)); const auto recovered_permit = state.tryPermit(); CHECK_TRUE(recovered_permit.has_value()); CHECK_TRUE(state.validate(*recovered_permit)); @@ -80,7 +90,7 @@ int main() state.observe(SafetyCondition::RobotEmergencyStop); state.observe(SafetyCondition::Normal); CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop( - state.snapshot(), true, 0)); + state.snapshot(), true, 0, true, false)); CHECK_TRUE(state.snapshot().software_emergency_stop_latched); CHECK_TRUE(state.snapshot().latched_reason == SafetyCondition::SoftwareEmergencyStop); @@ -88,7 +98,7 @@ int main() state.snapshot().epoch); CHECK_TRUE(software_recovery.has_value()); CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery( - *software_recovery, true, true)); + *software_recovery, true, true, true)); state.failRecovery(*software_recovery); const auto explicit_software_recovery = state.beginRecovery( state.snapshot().epoch); @@ -104,7 +114,7 @@ int main() state.snapshot().epoch); CHECK_TRUE(stale_recovery.has_value()); CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery( - *stale_recovery, true, true)); + *stale_recovery, true, true, true)); state.failRecovery(*stale_recovery); const auto explicit_recovery = state.beginRecovery( state.snapshot().epoch); diff --git a/docs/device_safety_control_plane_architecture.md b/docs/device_safety_control_plane_architecture.md index bf49c37e..d6675cd9 100644 --- a/docs/device_safety_control_plane_architecture.md +++ b/docs/device_safety_control_plane_architecture.md @@ -29,9 +29,10 @@ `RECOVERY_LOCAL_ONLY`、服务端确认实际 peer 为 loopback/Unix socket 且持久审计可写时才可开放。 AUBO 另有一条设备内硬件语义：真实硬件急停曾有效、随后输入消失且控制器重新报告 -`Normal/ReducedMode` 时，驱动会在重新清理队列并确认 quiescent 后自动解除该硬件锁存； +`Normal/ReducedMode` 时，驱动会自动上电到 `Idle`、清理旧队列、执行 `startup()`，并在 +`Running` 下再次确认 quiescent 后解除该硬件锁存，使新的 gRPC 指令可以重新准入； 软件 `emergencyStop()` 使用独立 `SoftwareEmergencyStop` 锁存，即使它与硬件急停重叠也绝不被 -硬件输入释放自动清除。自动流程不上电、不 resume、不重放旧目标。 +硬件输入释放自动清除。自动流程不 resume、不重放旧目标；显式 Stop/PowerOff 会取消本轮自动上电。 ## 1. 决策摘要 @@ -131,7 +132,8 @@ AUBO 另有一条设备内硬件语义：真实硬件急停曾有效、随后输 共享服务端生成的 `anonymous` principal，因此 command ID 必须在整个匿名部署内唯一。 6. 相同 ID、不同语义 payload 必须返回冲突，不能覆盖旧记录。 7. 硬件结果不确定时保存 `OUTCOME_UNKNOWN`，重试只能查询该结果，不能再次下发。 -8. 恢复成功只表示软件准入可重新评估，不表示设备被上电、使能、解除急停或自动运动。 +8. 通用 `RecoverSafetyState` 成功只表示软件准入可重新评估，不表示设备被上电、使能、 + 解除急停或自动运动。AUBO 物理急停释放后的自动上电是独立、显式配置的设备内策略。 9. `SafetyManager` 持有内部锁时不得调用设备、网络或可能阻塞的 participant 方法。 10. 驱动最终安全检查失败时，即使已经获得 permit，也不能下发设备命令。 11. 进程重启后，控制设备在新鲜状态确认完成前不能自动恢复到可控制状态。 @@ -1286,7 +1288,7 @@ capability manifest/SystemInfo。 | 设备/入口 | 策略 | 关键安全事实 | Stop/恢复要点 | | --- | --- | --- | --- | -| Aubo Arm | Control | connected、robot mode、exec/queue、power、硬件/软件 EStop、protective stop、fault | 硬件 EStop 释放后仅在 Normal/Reduced、队列清空和 quiescent 确认后自动恢复；软件 EStop 独立锁存；无法确认 exec 时 OutcomeUnknown | +| Aubo Arm | Control | connected、robot mode、exec/queue、power、硬件/软件 EStop、protective stop、fault | 硬件 EStop 释放后在 Normal/Reduced 下自动 poweron/startup，Running 且队列清空、quiescent 后才重新准入；显式 Stop/PowerOff 优先；软件 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 不做网络调用 | diff --git a/protos/cmvr/config/arm_config/arm_config.proto b/protos/cmvr/config/arm_config/arm_config.proto index a97be752..b717e89d 100644 --- a/protos/cmvr/config/arm_config/arm_config.proto +++ b/protos/cmvr/config/arm_config/arm_config.proto @@ -48,6 +48,9 @@ message VendorRobotArmBackendConfig { string tool_frame = 8; string username = 9; string password = 10; + // AUBO only. Automatic energization after a physical E-stop is deliberately + // opt-in because releasing brakes changes the hardware energy state. + optional bool auto_power_on_after_hardware_estop_release = 11; } enum DamiaoMotorModel {