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