diff --git a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt index a3351b74..aee5d72a 100644 --- a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt @@ -67,6 +67,19 @@ if(BUILD_TESTING) ) set_tests_properties(aubo_arm_motion_result_test PROPERTIES TIMEOUT 10) + add_executable(aubo_motion_state_test + tests/aubo_motion_state_test.cpp + ) + target_include_directories(aubo_motion_state_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + add_test( + NAME aubo_motion_state_test + COMMAND aubo_motion_state_test + ) + set_tests_properties(aubo_motion_state_test PROPERTIES TIMEOUT 10) + add_executable(aubo_arm_json_command_test tests/aubo_arm_json_command_test.cpp ) diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index 00261397..e2936689 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -1,6 +1,7 @@ #include "devices/arm/aubo_arm/aubo_arm.h" #include "devices/arm/aubo_arm/aubo_motion_result.h" +#include "devices/arm/aubo_arm/aubo_motion_state.h" #include #include @@ -18,11 +19,133 @@ namespace cmvr::device { namespace { -struct BusyGuard { - std::atomic& busy; - ~BusyGuard() { busy.store(false); } +class MotionOwnerGuard final { +public: + MotionOwnerGuard( + aubo_internal::MotionState& state, + std::atomic& busy, + const aubo_internal::MotionToken token) + : state_(state), busy_(busy), token_(token) + { + } + + ~MotionOwnerGuard() noexcept + { + try { + if (requires_settlement_ && !settled_) { + state_.failMotion(token_); + } else { + state_.finish(token_, finish_mode_); + } + busy_.store(state_.busy()); + } catch (...) { + // A failed state lock must not terminate an RPC unwind. Preserve + // the conservative externally visible state instead. + busy_.store(true); + } + } + + MotionOwnerGuard(const MotionOwnerGuard&) = delete; + MotionOwnerGuard& operator=(const MotionOwnerGuard&) = delete; + + void requireExplicitSettlement() noexcept + { + requires_settlement_ = true; + } + + void settle() noexcept { settled_ = true; } + + void clearOnFinish() noexcept + { + finish_mode_ = aubo_internal::MotionFinishMode::Clear; + } + + void retainKind() noexcept + { + finish_mode_ = aubo_internal::MotionFinishMode::Retain; + } + +private: + aubo_internal::MotionState& state_; + std::atomic& busy_; + aubo_internal::MotionToken token_; + aubo_internal::MotionFinishMode finish_mode_{ + aubo_internal::MotionFinishMode::RestorePrevious}; + bool requires_settlement_{false}; + bool settled_{false}; }; +class StopStateGuard final { +public: + StopStateGuard( + aubo_internal::MotionState& state, + std::atomic& busy) + : state_(state), busy_(busy) + { + } + + ~StopStateGuard() noexcept + { + if (!completed_) { + try { + state_.failStop(); + } catch (...) { + // Keep the facade fail-closed even if state cleanup fails. + } + busy_.store(true); + } + } + + StopStateGuard(const StopStateGuard&) = delete; + StopStateGuard& operator=(const StopStateGuard&) = delete; + + bool complete() + { + if (!state_.completeStop()) { + return false; + } + busy_.store(false); + completed_ = true; + return true; + } + +private: + aubo_internal::MotionState& state_; + std::atomic& busy_; + bool completed_{false}; +}; + +const char* motionStartFailure( + const aubo_internal::MotionStartStatus status) noexcept +{ + switch (status) { + case aubo_internal::MotionStartStatus::Invalid: + return "invalid motion type"; + case aubo_internal::MotionStartStatus::Busy: + return "another motion is active"; + case aubo_internal::MotionStartStatus::Stopping: + return "a stop operation is in progress"; + case aubo_internal::MotionStartStatus::Blocked: + return "the previous stop did not complete; retry stopMotion or reconnect"; + case aubo_internal::MotionStartStatus::Started: + break; + } + return "unknown motion state"; +} + +const char* motionKindName(const aubo_internal::MotionKind kind) noexcept +{ + switch (kind) { + case aubo_internal::MotionKind::Joint: + return "joint"; + case aubo_internal::MotionKind::Linear: + return "linear"; + case aubo_internal::MotionKind::None: + break; + } + return "unknown"; +} + std::vector defaultJointNames(const std::size_t dof) { std::vector names; @@ -182,21 +305,40 @@ bool waitForRobotMode(const RobotInterfacePtr& robot_interface, return false; } -int waitArrival(const RobotInterfacePtr& robot_interface) +template +aubo_internal::MotionWaitResult waitArrival( + const RobotInterfacePtr& robot_interface, + IsCancelled&& is_cancelled) { int retry_count = 0; + if (is_cancelled()) { + return aubo_internal::MotionWaitResult::Cancelled; + } int exec_id = robot_interface->getMotionControl()->getExecId(); while (exec_id == -1 && retry_count++ < 5) { + if (is_cancelled()) { + return aubo_internal::MotionWaitResult::Cancelled; + } std::this_thread::sleep_for(std::chrono::milliseconds(50)); + if (is_cancelled()) { + return aubo_internal::MotionWaitResult::Cancelled; + } exec_id = robot_interface->getMotionControl()->getExecId(); } if (exec_id == -1) { - return -1; + return is_cancelled() + ? aubo_internal::MotionWaitResult::Cancelled + : aubo_internal::MotionWaitResult::Failed; } while (robot_interface->getMotionControl()->getExecId() != -1) { + if (is_cancelled()) { + return aubo_internal::MotionWaitResult::Cancelled; + } std::this_thread::sleep_for(std::chrono::milliseconds(50)); } - return 0; + return is_cancelled() + ? aubo_internal::MotionWaitResult::Cancelled + : aubo_internal::MotionWaitResult::Completed; } bool waitServoModeSelect(const RobotInterfacePtr& robot_interface, const int mode) @@ -228,6 +370,8 @@ CartesianPose poseFromVector(const std::vector& values) struct AuboArm::SdkState { std::shared_ptr rpc_client; + std::shared_ptr motion_state{ + std::make_shared()}; }; AuboArm::AuboArm(const config::RobotArmConfig& cfg) @@ -423,7 +567,7 @@ ArmState AuboArm::getRobotState() const state.connected = connected_.load(); state.powered_on = state.connected; state.brake_released = state.connected; - state.moving = busy_.load(); + state.moving = busy(); state.robot_mode = getRobotMode(); state.safety_mode = getSafetyMode(); state.control_mode = getControlMode(); @@ -515,7 +659,16 @@ RobotMode AuboArm::getRobotMode() const if (emergency_stopped_) { return RobotMode::Stopped; } - return busy_.load() ? RobotMode::Running : RobotMode::Idle; + return busy() ? RobotMode::Running : RobotMode::Idle; +} + +bool AuboArm::busy() const +{ + std::lock_guard lock(mutex_); + if (sdk_) { + return sdk_->motion_state->busy(); + } + return false; } Result AuboArm::torqueOn() @@ -612,37 +765,60 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o if (!validDof_(target.position.size(), error)) { return Result::failure(ArmErrorCode::InvalidDof, error); } - const auto ready = ensureConnected_("moveJ"); - if (!ready.ok()) { - return ready; - } - if (busy_.exchange(true)) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); - } - BusyGuard busy_guard{busy_}; - try { - const auto robot_names = sdk_->rpc_client->getRobotNames(); + std::unique_lock submit_lock(mutex_); + const auto locked_ready = ensureConnected_("moveJ"); + if (!locked_ready.ok()) { + return locked_ready; + } + const auto rpc_client = sdk_->rpc_client; + const auto motion_state = sdk_->motion_state; + const auto motion = motion_state->begin( + aubo_internal::MotionKind::Joint); + if (!motion.started()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] moveJ rejected: " + + std::string(motionStartFailure(motion.status)) + + ", id=" + id_); + } + busy_.store(true); + MotionOwnerGuard motion_owner{ + *motion_state, busy_, motion.token}; + + const auto robot_names = rpc_client->getRobotNames(); if (robot_names.empty()) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); } - auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + auto robot_interface = rpc_client->getRobotInterface(robot_names.front()); if (!robot_interface) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); } auto motion_control = robot_interface->getMotionControl(); motion_control->setSpeedFraction(speed_scaling_); + motion_owner.requireExplicitSettlement(); const int ret = motion_control->moveJoint( target.position, options.acceleration > 0.0 ? options.acceleration : 0.5, options.velocity > 0.0 ? options.velocity : 0.5, options.blend_radius, 0); + if (ret == arcs::common_interface::AUBO_OK) { + submit_lock.unlock(); + } else { + motion_owner.settle(); + } const auto outcome = aubo_internal::resolveMotionCommand( ret, arcs::common_interface::AUBO_OK, arcs::common_interface::AUBO_REQUEST_IGNORE, - [&robot_interface]() { return waitArrival(robot_interface); }); + [&robot_interface, motion_state, token = motion.token]() { + return waitArrival( + robot_interface, + [motion_state, token]() { + return motion_state->cancelled(token); + }); + }); switch (outcome) { case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion: CMVR_LOG(DEBUG) << "[AuboArm] moveJ completed without motion: sdk ret=" @@ -650,7 +826,14 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o << arcs::common_interface::returnValue2Str(ret) << ")"; return Result::success(); case aubo_internal::MotionCommandOutcome::CompletedAfterMotion: + motion_owner.clearOnFinish(); + motion_owner.settle(); return Result::success(); + case aubo_internal::MotionCommandOutcome::Cancelled: + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveJ stopped by stopMotion"); case aubo_internal::MotionCommandOutcome::SubmitFailed: return Result::failure( ArmErrorCode::CommandFailed, @@ -671,18 +854,30 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration if (!validDof_(velocity.velocity.size(), error)) { return Result::failure(ArmErrorCode::InvalidDof, error); } - const auto ready = ensureConnected_("speedJ"); - if (!ready.ok()) { - return ready; - } - if (busy_.exchange(true)) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); - } - BusyGuard busy_guard{busy_}; - try { + std::unique_lock submit_lock(mutex_); + const auto locked_ready = ensureConnected_("speedJ"); + if (!locked_ready.ok()) { + return locked_ready; + } + const auto rpc_client = sdk_->rpc_client; + const auto motion_state = sdk_->motion_state; + const auto motion = motion_state->begin( + aubo_internal::MotionKind::Joint, true); + if (!motion.started()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] speedJ rejected: " + + std::string(motionStartFailure(motion.status)) + + ", id=" + id_); + } + busy_.store(true); + MotionOwnerGuard motion_owner{ + *motion_state, busy_, motion.token}; + Result interface_result; - auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "speedJ", interface_result); + auto robot_interface = getPrimaryRobotInterface( + rpc_client, "speedJ", interface_result); if (!interface_result.ok()) { return interface_result; } @@ -690,14 +885,31 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_); const double resolved_acceleration = acceleration > 0.0 ? acceleration : 1.5; const double resolved_duration = duration > 0.0 ? duration : 100.0; + motion_owner.requireExplicitSettlement(); + submit_lock.unlock(); + if (motion_state->cancelled(motion.token)) { + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] speedJ stopped by stopMotion before submission"); + } const int ret = robot_interface->getMotionControl()->speedJoint( velocity.velocity, resolved_acceleration, resolved_duration); + if (motion_state->cancelled(motion.token)) { + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] speedJ stopped by stopMotion"); + } if (ret != 0) { + motion_owner.settle(); return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] speedJ failed: ret=" + std::to_string(ret)); } + motion_owner.retainKind(); + motion_owner.settle(); return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] speedJ failed: ") + e.what()); @@ -706,46 +918,38 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration Result AuboArm::stopJ(double acceleration) { - if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { - return Result::success(); - } - try { - Result interface_result; - auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "stopJ", interface_result); - if (!interface_result.ok()) { - return interface_result; - } - const double resolved_acceleration = acceleration > 0.0 ? acceleration : 31.0; - const int ret = robot_interface->getMotionControl()->stopJoint(resolved_acceleration); - busy_.store(false); - if (ret != 0) { - return Result::failure(ArmErrorCode::CommandFailed, - "[AuboArm] stopJ failed: ret=" + std::to_string(ret)); - } - return Result::success(); - } catch (const std::exception& e) { - return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopJ failed: ") + e.what()); - } + return stopMotion_(MotionStopKind::Joint, acceleration); } Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame) { (void)frame; - const auto ready = ensureConnected_("moveL"); - if (!ready.ok()) { - return ready; - } - if (busy_.exchange(true)) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); - } - BusyGuard busy_guard{busy_}; - try { - const auto robot_names = sdk_->rpc_client->getRobotNames(); + std::unique_lock submit_lock(mutex_); + const auto locked_ready = ensureConnected_("moveL"); + if (!locked_ready.ok()) { + return locked_ready; + } + const auto rpc_client = sdk_->rpc_client; + const auto motion_state = sdk_->motion_state; + const auto motion = motion_state->begin( + aubo_internal::MotionKind::Linear); + if (!motion.started()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] moveL rejected: " + + std::string(motionStartFailure(motion.status)) + + ", id=" + id_); + } + busy_.store(true); + MotionOwnerGuard motion_owner{ + *motion_state, busy_, motion.token}; + + const auto robot_names = rpc_client->getRobotNames(); if (robot_names.empty()) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); } - auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + auto robot_interface = rpc_client->getRobotInterface(robot_names.front()); if (!robot_interface) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); } @@ -754,17 +958,29 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, std::vector tcp_offset(6, 0.0); robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); std::vector pose{target.x, target.y, target.z, target.rx, target.ry, target.rz}; + motion_owner.requireExplicitSettlement(); const int ret = motion_control->moveLine( pose, options.acceleration > 0.0 ? options.acceleration : 0.5, options.velocity > 0.0 ? options.velocity : 0.25, options.blend_radius, 0); + if (ret == arcs::common_interface::AUBO_OK) { + submit_lock.unlock(); + } else { + motion_owner.settle(); + } const auto outcome = aubo_internal::resolveMotionCommand( ret, arcs::common_interface::AUBO_OK, arcs::common_interface::AUBO_REQUEST_IGNORE, - [&robot_interface]() { return waitArrival(robot_interface); }); + [&robot_interface, motion_state, token = motion.token]() { + return waitArrival( + robot_interface, + [motion_state, token]() { + return motion_state->cancelled(token); + }); + }); switch (outcome) { case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion: CMVR_LOG(DEBUG) << "[AuboArm] moveL completed without motion: sdk ret=" @@ -772,7 +988,14 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, << arcs::common_interface::returnValue2Str(ret) << ")"; return Result::success(); case aubo_internal::MotionCommandOutcome::CompletedAfterMotion: + motion_owner.clearOnFinish(); + motion_owner.settle(); return Result::success(); + case aubo_internal::MotionCommandOutcome::Cancelled: + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveL stopped by stopMotion"); case aubo_internal::MotionCommandOutcome::SubmitFailed: return Result::failure( ArmErrorCode::CommandFailed, @@ -789,18 +1012,30 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame) { - const auto ready = ensureConnected_("speedL"); - if (!ready.ok()) { - return ready; - } - if (busy_.exchange(true)) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); - } - BusyGuard busy_guard{busy_}; - try { + std::unique_lock submit_lock(mutex_); + const auto locked_ready = ensureConnected_("speedL"); + if (!locked_ready.ok()) { + return locked_ready; + } + const auto rpc_client = sdk_->rpc_client; + const auto motion_state = sdk_->motion_state; + const auto motion = motion_state->begin( + aubo_internal::MotionKind::Linear, true); + if (!motion.started()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] speedL rejected: " + + std::string(motionStartFailure(motion.status)) + + ", id=" + id_); + } + busy_.store(true); + MotionOwnerGuard motion_owner{ + *motion_state, busy_, motion.token}; + Result interface_result; - auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "speedL", interface_result); + auto robot_interface = getPrimaryRobotInterface( + rpc_client, "speedL", interface_result); if (!interface_result.ok()) { return interface_result; } @@ -820,8 +1055,10 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d tool_frame[0] = 0.0; tool_frame[1] = 0.0; tool_frame[2] = 0.0; - line_speed = sdk_->rpc_client->getMath()->poseTrans(tool_frame, line_speed); - angular_speed = sdk_->rpc_client->getMath()->poseTrans(tool_frame, angular_speed); + line_speed = rpc_client->getMath()->poseTrans( + tool_frame, line_speed); + angular_speed = rpc_client->getMath()->poseTrans( + tool_frame, angular_speed); } else if (frame == FrameType::User) { return Result::failure(ArmErrorCode::UnsupportedCommand, "[AuboArm] speedL User frame requires a configured user coordinate frame"); @@ -838,14 +1075,31 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d const double resolved_acceleration = acceleration > 0.0 ? acceleration : 1.2; const double resolved_duration = duration > 0.0 ? duration : 100.0; + motion_owner.requireExplicitSettlement(); + submit_lock.unlock(); + if (motion_state->cancelled(motion.token)) { + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] speedL stopped by stopMotion before submission"); + } const int ret = robot_interface->getMotionControl()->speedLine( speed, resolved_acceleration, resolved_duration); + if (motion_state->cancelled(motion.token)) { + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] speedL stopped by stopMotion"); + } if (ret != 0) { + motion_owner.settle(); return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] speedL failed: ret=" + std::to_string(ret)); } + motion_owner.retainKind(); + motion_owner.settle(); return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] speedL failed: ") + e.what()); @@ -854,44 +1108,165 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d Result AuboArm::stopL(std::optional acceleration) { - if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { - return Result::success(); - } - try { - Result interface_result; - auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "stopL", interface_result); - if (!interface_result.ok()) { - return interface_result; - } - const double resolved_acceleration = - acceleration.has_value() && *acceleration > 0.0 ? *acceleration : 10.0; - const int ret = robot_interface->getMotionControl()->stopLine(resolved_acceleration, resolved_acceleration); - busy_.store(false); - if (ret != 0) { - return Result::failure(ArmErrorCode::CommandFailed, - "[AuboArm] stopL failed: ret=" + std::to_string(ret)); - } - return Result::success(); - } catch (const std::exception& e) { - return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopL failed: ") + e.what()); - } + return stopMotion_( + MotionStopKind::Linear, + acceleration.has_value() ? *acceleration : 0.0); } Result AuboArm::stopMotion() { - if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { + return stopMotion_(MotionStopKind::Automatic, 0.0); +} + +Result AuboArm::stopMotion_( + const MotionStopKind requested_kind, + const double acceleration) +{ + if (!connected_.load()) { return Result::success(); } + try { - const auto robot_names = sdk_->rpc_client->getRobotNames(); - if (robot_names.empty()) { + std::unique_lock submit_lock(mutex_); + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { return Result::success(); } - auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); - if (robot_interface) { - robot_interface->getMotionControl()->stopMove(true, true); + + const auto motion_state = sdk_->motion_state; + aubo_internal::MotionKind forced_kind = + aubo_internal::MotionKind::None; + if (requested_kind == MotionStopKind::Joint) { + forced_kind = aubo_internal::MotionKind::Joint; + } else if (requested_kind == MotionStopKind::Linear) { + forced_kind = aubo_internal::MotionKind::Linear; + } + + const auto stop_request = + motion_state->beginStop(forced_kind); + if (!stop_request.started()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] stopMotion rejected: another stop operation is in progress"); + } + busy_.store(true); + StopStateGuard stop_state_guard{ + *motion_state, busy_}; + + Result interface_result; + auto robot_interface = getPrimaryRobotInterface( + sdk_->rpc_client, "stopMotion", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + + auto motion_control = robot_interface->getMotionControl(); + auto robot_state = robot_interface->getRobotState(); + int last_exec_id = motion_control->getExecId(); + bool last_steady = robot_state->isSteady(); + const bool requires_vendor_stop = + stop_request.tracked_motion || + last_exec_id != -1 || + !last_steady; + + if (requires_vendor_stop && + stop_request.kind == aubo_internal::MotionKind::None) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] stopMotion failed: controller is moving but the " + "active direct-motion type is unknown"); + } + + const auto issue_vendor_stop = [&]() -> Result { + int ret = 0; + if (stop_request.kind == aubo_internal::MotionKind::Joint) { + const double resolved_acceleration = + acceleration > 0.0 ? acceleration : 31.0; + ret = motion_control->stopJoint(resolved_acceleration); + } else { + const double resolved_acceleration = + acceleration > 0.0 ? acceleration : 10.0; + ret = motion_control->stopLine( + resolved_acceleration, resolved_acceleration); + } + if (ret != arcs::common_interface::AUBO_OK) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] stopMotion failed: " + + std::string(motionKindName(stop_request.kind)) + + " stop sdk ret=" + std::to_string(ret) + + " (" + + arcs::common_interface::returnValue2Str(ret) + + ")"); + } + return Result::success(); + }; + + if (requires_vendor_stop) { + const auto stop_result = issue_vendor_stop(); + if (!stop_result.ok()) { + return stop_result; + } + } + + constexpr auto kStopTimeout = std::chrono::seconds(5); + constexpr auto kPollInterval = std::chrono::milliseconds(50); + constexpr int kStableSamples = 3; + const auto deadline = + std::chrono::steady_clock::now() + kStopTimeout; + int stable_samples = 0; + bool idle_since_stop = last_exec_id == -1 && last_steady; + bool owner_active = + motion_state->ownerActive(stop_request.active_token); + while (std::chrono::steady_clock::now() < deadline) { + last_exec_id = motion_control->getExecId(); + last_steady = robot_state->isSteady(); + owner_active = + motion_state->ownerActive(stop_request.active_token); + const bool physically_idle = + last_exec_id == -1 && last_steady; + if (!physically_idle && idle_since_stop) { + if (stop_request.kind == + aubo_internal::MotionKind::None) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] stopMotion failed: motion started " + "after an idle observation but its type is unknown"); + } + const auto stop_result = issue_vendor_stop(); + if (!stop_result.ok()) { + return stop_result; + } + idle_since_stop = false; + } + if (physically_idle && !owner_active) { + if (++stable_samples >= kStableSamples) { + break; + } + } else { + stable_samples = 0; + } + if (physically_idle) { + idle_since_stop = true; + } + std::this_thread::sleep_for(kPollInterval); + } + if (stable_samples < kStableSamples) { + return Result::failure( + ArmErrorCode::Timeout, + "[AuboArm] stopMotion failed: timeout waiting for " + + std::string(motionKindName(stop_request.kind)) + + " motion to stop, generation=" + + std::to_string(stop_request.active_token.generation) + + ", exec_id=" + std::to_string(last_exec_id) + + ", steady=" + (last_steady ? "true" : "false") + + ", owner_active=" + + (owner_active ? "true" : "false")); + } + if (!stop_state_guard.complete()) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] stopMotion failed: cancelled motion handler is still active"); } - busy_.store(false); return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopMotion failed: ") + e.what()); @@ -1226,7 +1601,6 @@ Result AuboArm::stopProgram() } try { const int ret = sdk_->rpc_client->getRuntimeMachine()->abort(); - busy_.store(false); if (ret != 0) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] stopProgram failed: ret=" + std::to_string(ret)); diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h index 28a19663..6f05af2d 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h @@ -80,12 +80,19 @@ public: CartesianPose fk(const std::string& base_link, const std::string& ee_link) override; CartesianPose fk(bool is_tcp = true) override; CartesianVelocity getSpeedLCommandTwistBase() const override { return {}; } - bool busy() const override { return busy_.load(); } + bool busy() const override; private: + enum class MotionStopKind { + Automatic, + Joint, + Linear, + }; + Result unsupported_(const std::string& name) const; bool validDof_(std::size_t size, std::string& error) const; Result ensureConnected_(const std::string& context) const; + Result stopMotion_(MotionStopKind kind, double acceleration); struct SdkState; diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h b/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h index d2cba859..d1c92fb0 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h @@ -6,10 +6,17 @@ namespace cmvr::device::aubo_internal { enum class MotionCommandOutcome { CompletedWithoutMotion, CompletedAfterMotion, + Cancelled, SubmitFailed, CompletionFailed, }; +enum class MotionWaitResult { + Completed, + Cancelled, + Failed, +}; + template MotionCommandOutcome resolveMotionCommand( const int return_code, @@ -23,7 +30,11 @@ MotionCommandOutcome resolveMotionCommand( if (return_code != success_code) { return MotionCommandOutcome::SubmitFailed; } - if (wait_for_completion() != 0) { + const auto wait_result = wait_for_completion(); + if (wait_result == MotionWaitResult::Cancelled) { + return MotionCommandOutcome::Cancelled; + } + if (wait_result != MotionWaitResult::Completed) { return MotionCommandOutcome::CompletionFailed; } return MotionCommandOutcome::CompletedAfterMotion; diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h b/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h new file mode 100644 index 00000000..76409c90 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h @@ -0,0 +1,265 @@ +#ifndef CMVR_ES_AUBO_MOTION_STATE_H +#define CMVR_ES_AUBO_MOTION_STATE_H + +#include +#include +#include +#include +#include + +namespace cmvr::device::aubo_internal { + +enum class MotionKind { + None, + Joint, + Linear, +}; + +struct MotionToken { + std::uint64_t generation{0}; + MotionKind kind{MotionKind::None}; + + bool valid() const noexcept + { + return generation != 0 && kind != MotionKind::None; + } +}; + +enum class MotionStartStatus { + Started, + Invalid, + Busy, + Stopping, + Blocked, +}; + +struct MotionStartResult { + MotionStartStatus status{MotionStartStatus::Busy}; + MotionToken token; + + bool started() const noexcept + { + return status == MotionStartStatus::Started; + } +}; + +enum class MotionFinishMode { + RestorePrevious, + Clear, + Retain, +}; + +enum class StopStartStatus { + Started, + AlreadyStopping, +}; + +struct StopRequest { + StopStartStatus status{StopStartStatus::AlreadyStopping}; + MotionKind kind{MotionKind::None}; + MotionToken active_token; + bool tracked_motion{false}; + + bool started() const noexcept + { + return status == StopStartStatus::Started; + } +}; + +// Tracks one direct AUBO motion owner. MoveJ/MoveL submissions are serialized +// through the vendor call. Speed calls release the outer mutex before their +// potentially blocking SDK call, so the generation cancellation below also +// closes the stop-vs-speed-submission race. +class MotionState final { +public: + MotionStartResult begin( + const MotionKind kind, + const bool replace_retained_same_kind = false) + { + std::lock_guard lock(mutex_); + if (kind == MotionKind::None) { + return {MotionStartStatus::Invalid, {}}; + } + if (stop_in_progress_) { + return {MotionStartStatus::Stopping, {}}; + } + if (blocked_) { + return {MotionStartStatus::Blocked, {}}; + } + if (owner_active_) { + return {MotionStartStatus::Busy, {}}; + } + if (last_kind_ != MotionKind::None && + (!replace_retained_same_kind || last_kind_ != kind)) { + return {MotionStartStatus::Busy, {}}; + } + + MotionToken token{++next_generation_, kind}; + owner_active_ = true; + active_token_ = token; + previous_kind_ = last_kind_; + return {MotionStartStatus::Started, token}; + } + + void finish( + const MotionToken& token, + const MotionFinishMode mode = MotionFinishMode::RestorePrevious) + { + std::lock_guard lock(mutex_); + if (!owner_active_ || + active_token_.generation != token.generation) { + return; + } + + owner_active_ = false; + active_token_ = {}; + if (!stop_in_progress_ && !blocked_) { + if (mode == MotionFinishMode::Retain) { + last_kind_ = token.kind; + } else if (mode == MotionFinishMode::Clear) { + last_kind_ = MotionKind::None; + } else { + last_kind_ = previous_kind_; + } + } + previous_kind_ = MotionKind::None; + owner_finished_cv_.notify_all(); + } + + void failMotion(const MotionToken& token) + { + std::lock_guard lock(mutex_); + if (!owner_active_ || + active_token_.generation != token.generation) { + return; + } + owner_active_ = false; + active_token_ = {}; + last_kind_ = token.kind; + previous_kind_ = MotionKind::None; + blocked_ = true; + owner_finished_cv_.notify_all(); + } + + StopRequest beginStop( + const MotionKind requested_kind = MotionKind::None) + { + std::lock_guard lock(mutex_); + if (stop_in_progress_) { + return {}; + } + + stop_in_progress_ = true; + const MotionToken active = owner_active_ + ? active_token_ + : MotionToken{}; + // A successful speedJoint/speedLine call may keep the controller in + // velocity mode after the SDK function returns, even when the target + // velocity is zero and the robot currently reports steady. Retain that + // motion kind until a typed stop has been acknowledged. + const bool tracked_motion = + active.valid() || last_kind_ != MotionKind::None; + if (active.valid()) { + cancelled_generation_ = std::max( + cancelled_generation_, active.generation); + } + MotionKind kind = MotionKind::None; + if (active.valid()) { + kind = active.kind; + } else if (last_kind_ != MotionKind::None) { + kind = last_kind_; + } else if (requested_kind != MotionKind::None) { + kind = requested_kind; + } else { + kind = last_kind_; + } + if (kind != MotionKind::None) { + last_kind_ = kind; + } + return { + StopStartStatus::Started, + kind, + active, + tracked_motion}; + } + + bool cancelled(const MotionToken& token) const + { + std::lock_guard lock(mutex_); + return token.valid() && + token.generation <= cancelled_generation_; + } + + bool waitForOwnerExit( + const MotionToken& token, + const std::chrono::milliseconds timeout) + { + if (!token.valid()) { + return true; + } + std::unique_lock lock(mutex_); + return owner_finished_cv_.wait_for( + lock, + timeout, + [this, &token]() { + return !owner_active_ || + active_token_.generation != token.generation; + }); + } + + bool ownerActive(const MotionToken& token) const + { + if (!token.valid()) { + return false; + } + std::lock_guard lock(mutex_); + return owner_active_ && + active_token_.generation == token.generation; + } + + bool completeStop() + { + std::lock_guard lock(mutex_); + if (owner_active_) { + return false; + } + stop_in_progress_ = false; + blocked_ = false; + active_token_ = {}; + last_kind_ = MotionKind::None; + previous_kind_ = MotionKind::None; + owner_finished_cv_.notify_all(); + return true; + } + + void failStop() + { + std::lock_guard lock(mutex_); + stop_in_progress_ = false; + blocked_ = true; + owner_finished_cv_.notify_all(); + } + + bool busy() const + { + std::lock_guard lock(mutex_); + return owner_active_ || stop_in_progress_ || blocked_ || + last_kind_ != MotionKind::None; + } + +private: + mutable std::mutex mutex_; + std::condition_variable owner_finished_cv_; + std::uint64_t next_generation_{0}; + std::uint64_t cancelled_generation_{0}; + MotionToken active_token_; + MotionKind last_kind_{MotionKind::None}; + MotionKind previous_kind_{MotionKind::None}; + bool owner_active_{false}; + bool stop_in_progress_{false}; + bool blocked_{false}; +}; + +} // namespace cmvr::device::aubo_internal + +#endif // CMVR_ES_AUBO_MOTION_STATE_H diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp index da2c16ee..fb1656e5 100644 --- a/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp +++ b/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp @@ -18,6 +18,7 @@ namespace { int main() { using cmvr::device::aubo_internal::MotionCommandOutcome; + using cmvr::device::aubo_internal::MotionWaitResult; using cmvr::device::aubo_internal::resolveMotionCommand; constexpr int success_code = 0; @@ -26,7 +27,7 @@ int main() int wait_calls = 0; const auto wait_succeeded = [&wait_calls]() { ++wait_calls; - return 0; + return MotionWaitResult::Completed; }; CHECK_TRUE(resolveMotionCommand( success_code, @@ -57,7 +58,7 @@ int main() wait_calls = 0; const auto wait_failed = [&wait_calls]() { ++wait_calls; - return -1; + return MotionWaitResult::Failed; }; CHECK_TRUE(resolveMotionCommand( success_code, @@ -66,5 +67,17 @@ int main() wait_failed) == MotionCommandOutcome::CompletionFailed); CHECK_TRUE(wait_calls == 1); + wait_calls = 0; + const auto wait_cancelled = [&wait_calls]() { + ++wait_calls; + return MotionWaitResult::Cancelled; + }; + CHECK_TRUE(resolveMotionCommand( + success_code, + success_code, + request_ignore_code, + wait_cancelled) == MotionCommandOutcome::Cancelled); + CHECK_TRUE(wait_calls == 1); + return 0; } diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp new file mode 100644 index 00000000..3bb12777 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp @@ -0,0 +1,127 @@ +#include "devices/arm/aubo_arm/aubo_motion_state.h" + +#include +#include + +namespace { + +#define CHECK_TRUE(condition) \ + do { \ + if (!(condition)) { \ + std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \ + << #condition << std::endl; \ + return 1; \ + } \ + } while (false) + +} // namespace + +int main() +{ + using namespace cmvr::device::aubo_internal; + + MotionState state; + CHECK_TRUE(state.begin(MotionKind::None).status == + MotionStartStatus::Invalid); + const auto joint = state.begin(MotionKind::Joint); + CHECK_TRUE(joint.started()); + CHECK_TRUE(state.busy()); + CHECK_TRUE(state.begin(MotionKind::Linear).status == + MotionStartStatus::Busy); + + const auto stop_joint = state.beginStop(); + CHECK_TRUE(stop_joint.started()); + CHECK_TRUE(stop_joint.kind == MotionKind::Joint); + CHECK_TRUE(stop_joint.active_token.generation == + joint.token.generation); + CHECK_TRUE(stop_joint.tracked_motion); + CHECK_TRUE(state.cancelled(joint.token)); + CHECK_TRUE(state.beginStop().status == + StopStartStatus::AlreadyStopping); + CHECK_TRUE(state.begin(MotionKind::Linear).status == + MotionStartStatus::Stopping); + CHECK_TRUE(!state.waitForOwnerExit( + joint.token, std::chrono::milliseconds(1))); + CHECK_TRUE(!state.completeStop()); + + state.finish(joint.token); + CHECK_TRUE(state.waitForOwnerExit( + joint.token, std::chrono::milliseconds(1))); + CHECK_TRUE(state.completeStop()); + CHECK_TRUE(!state.busy()); + + const auto linear = state.begin(MotionKind::Linear); + CHECK_TRUE(linear.started()); + CHECK_TRUE(!state.cancelled(linear.token)); + // A delayed guard from the cancelled command must not release a newer one. + state.finish(joint.token); + CHECK_TRUE(state.busy()); + state.finish(linear.token, MotionFinishMode::Clear); + CHECK_TRUE(!state.busy()); + + const auto speed_joint = state.begin(MotionKind::Joint); + CHECK_TRUE(speed_joint.started()); + state.finish(speed_joint.token, MotionFinishMode::Retain); + CHECK_TRUE(state.busy()); + CHECK_TRUE(state.begin(MotionKind::Linear).status == + MotionStartStatus::Busy); + const auto rejected_speed_update = + state.begin(MotionKind::Joint, true); + CHECK_TRUE(rejected_speed_update.started()); + state.finish( + rejected_speed_update.token, + MotionFinishMode::RestorePrevious); + const auto stop_speed = state.beginStop(); + CHECK_TRUE(stop_speed.started()); + CHECK_TRUE(stop_speed.kind == MotionKind::Joint); + CHECK_TRUE(stop_speed.tracked_motion); + CHECK_TRUE(state.completeStop()); + CHECK_TRUE(!state.busy()); + + const auto idle_stop = state.beginStop(); + CHECK_TRUE(idle_stop.kind == MotionKind::None); + CHECK_TRUE(!idle_stop.tracked_motion); + state.failStop(); + const auto retry_idle_stop = state.beginStop(); + CHECK_TRUE(retry_idle_stop.kind == MotionKind::None); + CHECK_TRUE(!retry_idle_stop.tracked_motion); + CHECK_TRUE(state.completeStop()); + + const auto mismatched_stop_motion = state.begin(MotionKind::Linear); + CHECK_TRUE(mismatched_stop_motion.started()); + const auto mismatched_stop = state.beginStop(MotionKind::Joint); + CHECK_TRUE(mismatched_stop.kind == MotionKind::Linear); + state.finish(mismatched_stop_motion.token); + CHECK_TRUE(state.completeStop()); + + const auto uncertain_motion = state.begin(MotionKind::Joint); + CHECK_TRUE(uncertain_motion.started()); + state.failMotion(uncertain_motion.token); + CHECK_TRUE(state.busy()); + CHECK_TRUE(state.begin(MotionKind::Linear).status == + MotionStartStatus::Blocked); + const auto stop_uncertain = state.beginStop(); + CHECK_TRUE(stop_uncertain.kind == MotionKind::Joint); + CHECK_TRUE(stop_uncertain.tracked_motion); + CHECK_TRUE(state.completeStop()); + + const auto failed_stop_motion = state.begin(MotionKind::Linear); + CHECK_TRUE(failed_stop_motion.started()); + const auto failed_stop = state.beginStop(); + CHECK_TRUE(failed_stop.kind == MotionKind::Linear); + state.failStop(); + CHECK_TRUE(state.busy()); + CHECK_TRUE(state.begin(MotionKind::Joint).status == + MotionStartStatus::Blocked); + state.finish(failed_stop_motion.token); + + const auto retry = state.beginStop(); + CHECK_TRUE(retry.started()); + CHECK_TRUE(retry.kind == MotionKind::Linear); + CHECK_TRUE(state.completeStop()); + const auto recovered = state.begin(MotionKind::Joint); + CHECK_TRUE(recovered.started()); + state.finish(recovered.token, MotionFinishMode::Clear); + + return 0; +} 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 4e29f24f..b3547c2f 100644 --- a/cmvr-es/manager/control_authority/include/control_authority_manager.h +++ b/cmvr-es/manager/control_authority/include/control_authority_manager.h @@ -41,6 +41,14 @@ public: const std::string& resource_id, const std::string& owner_id, Duration ttl); + + // Atomically invalidates a normal control lease and joins a safety + // barrier. Each safety caller receives an independent token; normal + // control remains blocked until the last safety token is released. + ControlAcquireResult preemptAcquire( + const std::string& resource_id, + const std::string& owner_id, + Duration ttl); bool renew(const ControlLeaseToken& token, Duration ttl); bool validate(const ControlLeaseToken& token); void release(const ControlLeaseToken& token) noexcept; @@ -60,6 +68,8 @@ private: std::string owner_id; std::uint64_t generation{0}; std::chrono::steady_clock::time_point deadline; + bool preemptible{true}; + std::unordered_map safety_holders; }; bool expired_(const Entry& entry) const noexcept; 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 e52d3353..53facdd8 100644 --- a/cmvr-es/manager/control_authority/src/control_authority_manager.cpp +++ b/cmvr-es/manager/control_authority/src/control_authority_manager.cpp @@ -42,7 +42,50 @@ ControlAcquireResult ControlAuthorityManager::tryAcquire( Entry{ owner_id, token.generation, - std::chrono::steady_clock::now() + ttl}); + std::chrono::steady_clock::now() + ttl, + true, + {}}); + return {true, std::move(token), {}}; +} + +ControlAcquireResult ControlAuthorityManager::preemptAcquire( + const std::string& resource_id, + const std::string& owner_id, + const Duration ttl) +{ + if (resource_id.empty() || owner_id.empty() || + ttl <= Duration::zero()) { + return {false, {}, "invalid control barrier request"}; + } + + 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), {}}; + } + entries_.erase(existing); + } + + ControlLeaseToken token; + 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::time_point::max(), + false, + {{token.generation, owner_id}}}); return {true, std::move(token), {}}; } @@ -55,15 +98,22 @@ bool ControlAuthorityManager::renew( } std::lock_guard lock(mutex_); const auto found = entries_.find(token.resource_id); - if (found == entries_.end() || - expired_(found->second) || - found->second.owner_id != token.owner_id || - found->second.generation != token.generation) { + if (found == entries_.end() || expired_(found->second)) { if (found != entries_.end() && expired_(found->second)) { 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.owner_id != token.owner_id || + found->second.generation != token.generation) { + return false; + } found->second.deadline = std::chrono::steady_clock::now() + ttl; return true; @@ -84,6 +134,12 @@ bool ControlAuthorityManager::validate( 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; + } return found->second.owner_id == token.owner_id && found->second.generation == token.generation; } @@ -97,9 +153,22 @@ void ControlAuthorityManager::release( try { std::lock_guard lock(mutex_); const auto found = entries_.find(token.resource_id); - if (found != entries_.end() && - found->second.owner_id == token.owner_id && - found->second.generation == token.generation) { + 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) { + return; + } + found->second.safety_holders.erase(holder); + if (found->second.safety_holders.empty()) { + entries_.erase(found); + } + } else if (found->second.owner_id == token.owner_id && + found->second.generation == token.generation) { entries_.erase(found); } } catch (...) { 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 b6b2b965..855d4853 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 @@ -61,6 +61,38 @@ TEST_F(ControlAuthorityManagerTest, StaleGenerationCannotReleaseNewLease) EXPECT_TRUE(manager.validate(current.token)); } +TEST_F(ControlAuthorityManagerTest, + SafetyBarrierAtomicallyPreemptsControlAndRejectsOtherOwners) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 100ms); + ASSERT_TRUE(control.acquired); + + const auto barrier = manager.preemptAcquire( + "right_arm", "stop-operation", 100ms); + ASSERT_TRUE(barrier.acquired) << barrier.detail; + EXPECT_FALSE(manager.validate(control.token)); + EXPECT_TRUE(manager.validate(barrier.token)); + + const auto move_during_stop = + manager.tryAcquire("right_arm", "new-move", 100ms); + EXPECT_FALSE(move_during_stop.acquired); + const auto second_stop = manager.preemptAcquire( + "right_arm", "second-stop", 100ms); + ASSERT_TRUE(second_stop.acquired) << second_stop.detail; + + manager.release(control.token); + EXPECT_TRUE(manager.validate(barrier.token)); + manager.release(barrier.token); + EXPECT_TRUE(manager.validate(second_stop.token)); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "new-move", 100ms) + .acquired); + manager.release(second_stop.token); + EXPECT_FALSE(manager.isLeased("right_arm")); +} + TEST_F(ControlAuthorityManagerTest, ExpiryAndRenewUseMonotonicLocalTime) { auto& manager = ControlAuthorityManager::instance(); diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index b1567f00..a8fb5622 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -126,11 +126,16 @@ grpc::Status setDeviceNotFound(Response* response, const std::string& device_id) grpc::Status setControlLeaseConflict( api::CommandHeader_Feedback* response, - const std::string& device_id) + const std::string& device_id, + const std::string& detail) { - const std::string message = + std::string message = "RobotArm control is leased by another active control operation: " + device_id; + if (!detail.empty()) { + CMVR_LOG(WARNING) << "[gRPCArmServiceImpl] control lease conflict, id=" + << device_id << ", detail=" << detail; + } fillFeedback(response, false, message); return grpc::Status( grpc::StatusCode::FAILED_PRECONDITION, message); @@ -139,17 +144,19 @@ grpc::Status setControlLeaseConflict( template grpc::Status setControlLeaseConflict( Response* response, - const std::string& device_id) + const std::string& device_id, + const std::string& detail) { return setControlLeaseConflict( - response->mutable_header(), device_id); + response->mutable_header(), device_id, detail); } class ScopedUnaryControlLease final { public: ScopedUnaryControlLease( const std::string& device_id, - const char* operation) + const char* operation, + const bool preemptive = false) : manager_(control::ControlAuthorityManager::instance()) { static std::atomic sequence{0}; @@ -159,14 +166,15 @@ public: sequence.fetch_add( 1U, std::memory_order_relaxed) + 1U); - auto acquired = manager_.tryAcquire( - device_id, - owner, - std::chrono::duration_cast< - control::ControlAuthorityManager::Duration>( - std::chrono::hours(24))); + 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); acquired_ = acquired.acquired; token_ = std::move(acquired.token); + detail_ = std::move(acquired.detail); } ~ScopedUnaryControlLease() @@ -175,10 +183,12 @@ public: } bool acquired() const noexcept { return acquired_; } + const std::string& detail() const noexcept { return detail_; } private: control::ControlAuthorityManager& manager_; control::ControlLeaseToken token_; + std::string detail_; bool acquired_{false}; }; @@ -199,10 +209,12 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } - // A safety-disable command preempts any network teleoperation lease. - // The teleoperation executor must fail its next renew before it can - // dispatch another setpoint. - control::ControlAuthorityManager::instance().revoke(device_id); + ScopedUnaryControlLease control_barrier( + device_id, "torqueOff", true); + if (!control_barrier.acquired()) { + return setControlLeaseConflict( + response, device_id, control_barrier.detail()); + } const auto result = arm->torqueOff(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { @@ -228,7 +240,8 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "torqueOn"); if (!control_lease.acquired()) { - return setControlLeaseConflict(response, device_id); + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } const auto result = arm->torqueOn(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); @@ -255,7 +268,8 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "moveJ"); if (!control_lease.acquired()) { - return setControlLeaseConflict(response, device_id); + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } const auto result = arm->moveJ(toJointPositionCommand(request->target()), toMotionOptions(request->options())); @@ -283,7 +297,8 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "moveL"); if (!control_lease.acquired()) { - return setControlLeaseConflict(response, device_id); + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } const auto result = arm->moveL(toCartesianPose(request->target()), toMotionOptions(request->options()), @@ -312,7 +327,8 @@ grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "speedJ"); if (!control_lease.acquired()) { - return setControlLeaseConflict(response, device_id); + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } const auto result = arm->speedJ(toJointVelocityCommand(request->velocity()), request->acceleration(), @@ -343,7 +359,8 @@ grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "speedL"); if (!control_lease.acquired()) { - return setControlLeaseConflict(response, device_id); + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } const auto result = arm->speedL(toCartesianVelocity(request->velocity()), request->acceleration(), @@ -375,7 +392,8 @@ grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "servoJ"); if (!control_lease.acquired()) { - return setControlLeaseConflict(response, device_id); + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } const auto result = arm->servoJ(toJointPositionCommand(request->target())); if (result.ok()) { @@ -399,7 +417,12 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } - control::ControlAuthorityManager::instance().revoke(device_id); + ScopedUnaryControlLease control_barrier( + device_id, "stopMotion", true); + if (!control_barrier.acquired()) { + return setControlLeaseConflict( + response, device_id, control_barrier.detail()); + } const auto result = arm->stopMotion(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { @@ -480,7 +503,8 @@ grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "calibrateZeroQ"); if (!control_lease.acquired()) { - return setControlLeaseConflict(response, device_id); + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } const auto result = arm->calibrateZeroQ(request->joint_name()); if (result.ok()) { @@ -557,7 +581,8 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context, ScopedUnaryControlLease control_lease( device_id, "clearFault"); if (!control_lease.acquired()) { - return setControlLeaseConflict(response, device_id); + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } const auto result = arm->clearFault(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); diff --git a/cmvr-es/service/grpc/src/grpc_system_service.cpp b/cmvr-es/service/grpc/src/grpc_system_service.cpp index 908f4c91..87c592cc 100644 --- a/cmvr-es/service/grpc/src/grpc_system_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_system_service.cpp @@ -4,16 +4,65 @@ #include "../include/grpc_system_service.h" +#include #include #include +#include +#include +#include #include "common/base/logging/logger.h" +#include "manager/control_authority/include/control_authority_manager.h" using namespace cmvr::device; using namespace cmvr::service; namespace { +class ScopedControlBarrierSet final { +public: + ~ScopedControlBarrierSet() + { + auto& authority = + cmvr::control::ControlAuthorityManager::instance(); + for (const auto& token : tokens_) { + authority.release(token); + } + } + + bool acquire(const std::string& device_id, std::string& detail) + { + static std::atomic sequence{0}; + const std::string owner = + "grpc-system:stop-all:" + + std::to_string( + sequence.fetch_add(1U, std::memory_order_relaxed) + 1U); + const auto result = + cmvr::control::ControlAuthorityManager::instance() + .preemptAcquire( + device_id, + owner, + std::chrono::duration_cast< + cmvr::control::ControlAuthorityManager::Duration>( + std::chrono::hours(24))); + if (!result.acquired) { + detail = result.detail; + return false; + } + try { + tokens_.push_back(result.token); + } catch (...) { + cmvr::control::ControlAuthorityManager::instance().release( + result.token); + throw; + } + return true; + } + +private: + std::vector tokens_; +}; + std::uint64_t unixTimeMs() noexcept { const auto elapsed = std::chrono::duration_cast( @@ -239,6 +288,22 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) { try { + const auto snapshot = dmgr_.snapshot(); + ScopedControlBarrierSet control_barriers; + for (const auto& device : snapshot.devices) { + if (device.kind == cmvr::device::DeviceKind::Arm) { + std::string detail; + if (!control_barriers.acquire(device.id, detail)) { + CMVR_LOG(WARNING) + << "[gRPCSystemServiceImpl] (StopAll): failed to " + "acquire arm safety barrier, id=" + << device.id << ", detail=" << detail; + throw std::runtime_error( + "StopAll could not acquire the RobotArm safety " + "barrier: " + device.id); + } + } + } dmgr_.stop(); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); 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 57c9d060..f5b2c46f 100644 --- a/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp @@ -1,6 +1,10 @@ #include "service/grpc/include/grpc_arm_service.h" +#include +#include +#include #include +#include #include #include #include @@ -11,6 +15,7 @@ #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" namespace cmvr::service { @@ -57,7 +62,12 @@ public: } device::Result torqueOn() override { return device::Result::success(); } - device::Result torqueOff() override { return device::Result::success(); } + device::Result torqueOff() override + { + std::lock_guard lock(motion_mutex_); + ++torque_off_calls_; + return device::Result::success(); + } device::Result calibrateZeroQ(const std::string&) override { return device::Result::success(); @@ -82,7 +92,7 @@ public: device::Result moveJ(const device::JointPositionCommand&, const device::MotionOptions&) override { - return device::Result::success(); + return enterMotion("moveJ", move_j_calls_); } device::Result speedJ(const device::JointVelocityCommand&, double, @@ -99,7 +109,7 @@ public: const device::MotionOptions&, device::FrameType = device::FrameType::Base) override { - return device::Result::success(); + return enterMotion("moveL", move_l_calls_); } device::Result speedL( const device::CartesianVelocity&, @@ -115,9 +125,101 @@ public: } device::Result stopMotion() override { + std::unique_lock lock(motion_mutex_); + ++stop_motion_calls_; + if (block_next_stop_) { + block_next_stop_ = false; + blocking_stop_started_ = true; + stop_started_cv_.notify_all(); + stop_release_cv_.wait( + lock, + [this]() { return release_blocking_stop_; }); + } return device::Result::success(); } + void blockNextMotion() + { + std::lock_guard lock(motion_mutex_); + block_next_motion_ = true; + blocking_motion_started_ = false; + release_blocking_motion_ = false; + blocking_motion_name_.clear(); + } + + bool waitForBlockingMotion( + const std::string& operation, + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(motion_mutex_); + return motion_started_cv_.wait_for( + lock, + timeout, + [this, &operation]() { + return blocking_motion_started_ && + blocking_motion_name_ == operation; + }); + } + + void releaseBlockingMotion() + { + { + std::lock_guard lock(motion_mutex_); + release_blocking_motion_ = true; + } + motion_release_cv_.notify_all(); + } + + void blockNextStopMotion() + { + std::lock_guard lock(motion_mutex_); + block_next_stop_ = true; + blocking_stop_started_ = false; + release_blocking_stop_ = false; + } + + bool waitForBlockingStop(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(motion_mutex_); + return stop_started_cv_.wait_for( + lock, + timeout, + [this]() { return blocking_stop_started_; }); + } + + void releaseBlockingStop() + { + { + std::lock_guard lock(motion_mutex_); + release_blocking_stop_ = true; + } + stop_release_cv_.notify_all(); + } + + int moveJCalls() const + { + std::lock_guard lock(motion_mutex_); + return move_j_calls_; + } + + int moveLCalls() const + { + std::lock_guard lock(motion_mutex_); + return move_l_calls_; + } + + int stopMotionCalls() const + { + std::lock_guard lock(motion_mutex_); + return stop_motion_calls_; + } + + int torqueOffCalls() const + { + std::lock_guard lock(motion_mutex_); + return torque_off_calls_; + } + device::Result startServoMode(const device::ServoOptions&) override { return device::Result::success(); @@ -217,6 +319,42 @@ public: bool next_success{true}; std::string next_response_json; std::string last_request_json; + +private: + device::Result enterMotion(const char* operation, int& call_count) + { + std::unique_lock lock(motion_mutex_); + ++call_count; + if (!block_next_motion_) { + return device::Result::success(); + } + + block_next_motion_ = false; + blocking_motion_started_ = true; + blocking_motion_name_ = operation; + motion_started_cv_.notify_all(); + motion_release_cv_.wait( + lock, + [this]() { return release_blocking_motion_; }); + return device::Result::success(); + } + + mutable std::mutex motion_mutex_; + std::condition_variable motion_started_cv_; + std::condition_variable motion_release_cv_; + std::condition_variable stop_started_cv_; + std::condition_variable stop_release_cv_; + bool block_next_motion_{false}; + bool blocking_motion_started_{false}; + bool release_blocking_motion_{false}; + bool block_next_stop_{false}; + bool blocking_stop_started_{false}; + bool release_blocking_stop_{false}; + std::string blocking_motion_name_; + int move_j_calls_{0}; + int move_l_calls_{0}; + int stop_motion_calls_{0}; + int torque_off_calls_{0}; }; class JsonCommandNonArmDevice final : public device::AbstractDevice { @@ -243,6 +381,7 @@ class GrpcArmServiceTest : public ::testing::Test { protected: void SetUp() override { + control::ControlAuthorityManager::instance().clear(); device::DeviceManager::destroyInstance(); config::DeviceManagerConfig config; auto& manager = device::DeviceManager::getInstance(config); @@ -263,6 +402,7 @@ protected: aubo_arm_.reset(); left_arm_.reset(); device::DeviceManager::destroyInstance(); + control::ControlAuthorityManager::instance().clear(); } grpc::Status execute(const std::string& device_id, @@ -276,6 +416,60 @@ protected: return service_->ExecuteJsonCommand(&context, &request, &response); } + struct MoveOutcome { + grpc::Status status; + bool response_success{false}; + std::string response_error; + }; + + MoveOutcome moveJ(const std::string& device_id) + { + api::MoveJ_Request request; + request.mutable_header()->set_device_id(device_id); + request.mutable_target()->add_position(0.1); + api::MoveJ_Response response; + grpc::ServerContext context; + auto status = service_->moveJ(&context, &request, &response); + return { + std::move(status), + response.header().success(), + response.header().error_message()}; + } + + MoveOutcome moveL(const std::string& device_id) + { + api::MoveL_Request request; + request.mutable_header()->set_device_id(device_id); + request.mutable_target()->set_x(0.1); + api::MoveL_Response response; + grpc::ServerContext context; + auto status = service_->moveL(&context, &request, &response); + return { + std::move(status), + response.header().success(), + response.header().error_message()}; + } + + grpc::Status stopMotion( + const std::string& device_id, + api::CommandHeader_Feedback& response) + { + api::CommandHeader_Request request; + request.set_device_id(device_id); + grpc::ServerContext context; + return service_->stopMotion(&context, &request, &response); + } + + grpc::Status torqueOff( + const std::string& device_id, + api::CommandHeader_Feedback& response) + { + api::CommandHeader_Request request; + request.set_device_id(device_id); + grpc::ServerContext context; + return service_->torqueOff(&context, &request, &response); + } + std::shared_ptr left_arm_; std::shared_ptr aubo_arm_; std::shared_ptr non_arm_; @@ -374,5 +568,125 @@ TEST_F(GrpcArmServiceTest, EXPECT_EQ(left_arm_->execute_calls, 0); } +TEST_F(GrpcArmServiceTest, + StopMotionRevokesBlockedMoveJLeaseBeforeMoveLReturns) +{ + aubo_arm_->blockNextMotion(); + auto blocked_move = std::async( + std::launch::async, + [this]() { return moveJ("aubo_arm"); }); + + const bool move_started = aubo_arm_->waitForBlockingMotion( + "moveJ", std::chrono::seconds(2)); + + MoveOutcome conflict; + MoveOutcome during_stop; + api::CommandHeader_Feedback torque_off_response; + grpc::Status torque_off_status; + api::CommandHeader_Feedback stop_response; + grpc::Status stop_status; + MoveOutcome resumed_move; + bool stop_started = false; + std::future blocked_stop; + if (move_started) { + conflict = moveL("aubo_arm"); + aubo_arm_->blockNextStopMotion(); + blocked_stop = std::async( + std::launch::async, + [this, &stop_response]() { + return stopMotion("aubo_arm", stop_response); + }); + stop_started = aubo_arm_->waitForBlockingStop( + std::chrono::seconds(2)); + if (stop_started) { + torque_off_status = torqueOff( + "aubo_arm", torque_off_response); + during_stop = moveL("aubo_arm"); + } + aubo_arm_->releaseBlockingStop(); + stop_status = blocked_stop.get(); + resumed_move = moveL("aubo_arm"); + } + + // Keep the original RPC active until after the replacement MoveL has + // attempted to acquire control. This models a driver whose stopped motion + // takes time to unwind and guards the lease hand-off itself. + aubo_arm_->releaseBlockingMotion(); + const auto original_move = blocked_move.get(); + + ASSERT_TRUE(move_started); + ASSERT_TRUE(stop_started); + EXPECT_EQ(conflict.status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_EQ(conflict.response_error, conflict.status.error_message()); + EXPECT_EQ(during_stop.status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_EQ(during_stop.response_error, + during_stop.status.error_message()); + EXPECT_TRUE(torque_off_status.ok()) + << torque_off_status.error_message(); + EXPECT_TRUE(torque_off_response.success()) + << torque_off_response.error_message(); + EXPECT_TRUE(stop_status.ok()) << stop_status.error_message(); + EXPECT_TRUE(stop_response.success()) + << stop_response.error_message(); + EXPECT_TRUE(resumed_move.status.ok()) + << resumed_move.status.error_message(); + EXPECT_TRUE(resumed_move.response_success) + << resumed_move.response_error; + EXPECT_TRUE(original_move.status.ok()) + << original_move.status.error_message(); + EXPECT_TRUE(original_move.response_success) + << 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); +} + +TEST_F(GrpcArmServiceTest, + StopMotionRevokesBlockedMoveLLeaseBeforeMoveJReturns) +{ + aubo_arm_->blockNextMotion(); + auto blocked_move = std::async( + std::launch::async, + [this]() { return moveL("aubo_arm"); }); + + const bool move_started = aubo_arm_->waitForBlockingMotion( + "moveL", std::chrono::seconds(2)); + + MoveOutcome conflict; + api::CommandHeader_Feedback stop_response; + grpc::Status stop_status; + MoveOutcome resumed_move; + if (move_started) { + conflict = moveJ("aubo_arm"); + stop_status = stopMotion("aubo_arm", stop_response); + resumed_move = moveJ("aubo_arm"); + } + + aubo_arm_->releaseBlockingMotion(); + const auto original_move = blocked_move.get(); + + ASSERT_TRUE(move_started); + EXPECT_EQ(conflict.status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_EQ(conflict.response_error, conflict.status.error_message()); + EXPECT_TRUE(stop_status.ok()) << stop_status.error_message(); + EXPECT_TRUE(stop_response.success()) + << stop_response.error_message(); + EXPECT_TRUE(resumed_move.status.ok()) + << resumed_move.status.error_message(); + EXPECT_TRUE(resumed_move.response_success) + << resumed_move.response_error; + EXPECT_TRUE(original_move.status.ok()) + << original_move.status.error_message(); + EXPECT_TRUE(original_move.response_success) + << original_move.response_error; + EXPECT_EQ(aubo_arm_->moveJCalls(), 1); + EXPECT_EQ(aubo_arm_->moveLCalls(), 1); + EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1); +} + } // 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 8642a5b4..22eac5e4 100644 --- a/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp @@ -2,8 +2,11 @@ #include #include +#include #include +#include #include +#include #include #include #include @@ -12,6 +15,7 @@ #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" namespace cmvr::service { @@ -36,11 +40,58 @@ public: { return health_; } + bool stop() override + { + std::unique_lock lock(stop_mutex_); + ++stop_calls_; + if (block_next_stop_) { + block_next_stop_ = false; + stop_started_ = true; + stop_started_cv_.notify_all(); + stop_release_cv_.wait( + lock, + [this]() { return release_stop_; }); + } + return true; + } + int stopCalls() const + { + std::lock_guard lock(stop_mutex_); + return stop_calls_; + } + void blockNextStop() + { + std::lock_guard lock(stop_mutex_); + block_next_stop_ = true; + stop_started_ = false; + release_stop_ = false; + } + bool waitForStop(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(stop_mutex_); + return stop_started_cv_.wait_for( + lock, timeout, [this]() { return stop_started_; }); + } + void releaseStop() + { + { + std::lock_guard lock(stop_mutex_); + release_stop_ = true; + } + stop_release_cv_.notify_all(); + } private: device::DeviceKind kind_; std::string type_name_; device::DeviceHealthSnapshot health_; + mutable std::mutex stop_mutex_; + std::condition_variable stop_started_cv_; + std::condition_variable stop_release_cv_; + int stop_calls_{0}; + bool block_next_stop_{false}; + bool stop_started_{false}; + bool release_stop_{false}; }; std::uint64_t currentUnixTimeMs() @@ -66,6 +117,7 @@ class GrpcSystemServiceTest : public ::testing::Test { protected: void SetUp() override { + control::ControlAuthorityManager::instance().clear(); device::DeviceManager::destroyInstance(); } @@ -74,6 +126,7 @@ protected: service_.reset(); owned_devices_.clear(); device::DeviceManager::destroyInstance(); + control::ControlAuthorityManager::instance().clear(); } api::GetDeviceListCommand_Feedback getDeviceList() @@ -287,5 +340,74 @@ TEST_F(GrpcSystemServiceTest, MapsEveryKnownDeviceKind) } } +TEST_F(GrpcSystemServiceTest, + StopAllStopsRegisteredDevicesAndRevokesOnlyArmLease) +{ + 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& authority = control::ControlAuthorityManager::instance(); + const auto arm_lease = authority.tryAcquire( + arm->id(), "arm-session", std::chrono::seconds(30)); + const auto camera_lease = authority.tryAcquire( + camera->id(), "camera-session", std::chrono::seconds(30)); + const auto existing_stop_barrier = authority.preemptAcquire( + already_stopping_arm->id(), + "existing-stop", + std::chrono::seconds(30)); + ASSERT_TRUE(arm_lease.acquired) << arm_lease.detail; + ASSERT_TRUE(camera_lease.acquired) << camera_lease.detail; + ASSERT_TRUE(existing_stop_barrier.acquired) + << existing_stop_barrier.detail; + ASSERT_TRUE(authority.validate(arm_lease.token)); + 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(); + + 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_FALSE(authority.validate(arm_lease.token)); + EXPECT_FALSE(authority.isLeased(arm->id())); + EXPECT_TRUE(authority.validate(camera_lease.token)); + EXPECT_TRUE(authority.isLeased(camera->id())); + EXPECT_TRUE(authority.validate(existing_stop_barrier.token)); + EXPECT_TRUE(authority.isLeased(already_stopping_arm->id())); + const auto move_after_stop = authority.tryAcquire( + arm->id(), "move-after-stop", std::chrono::seconds(30)); + EXPECT_TRUE(move_after_stop.acquired) << move_after_stop.detail; + authority.release(move_after_stop.token); + authority.release(existing_stop_barrier.token); +} + } // namespace } // namespace cmvr::service