From 55912abad015544d9cd596944ae3e34b78cb7a2a Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 14 Aug 2026 11:58:17 +0800 Subject: [PATCH] fix(stop-all): cancel blocked arm startup safely --- cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp | 314 ++++++++++++++++-- cmvr-es/devices/arm/aubo_arm/aubo_arm.h | 3 + .../devices/arm/aubo_arm/aubo_motion_state.h | 103 +++++- .../arm/aubo_arm/aubo_torque_on_result.h | 50 +++ .../tests/aubo_arm_motion_result_test.cpp | 75 +++++ .../aubo_arm/tests/aubo_motion_state_test.cpp | 59 +++- cmvr-es/devices/arm/robot_arm.h | 23 ++ .../grpc/include/grpc_system_service.h | 14 +- cmvr-es/service/grpc/src/grpc_arm_service.cpp | 37 ++- .../service/grpc/src/grpc_system_service.cpp | 192 ++++++++++- .../grpc/tests/grpc_arm_service_test.cpp | 277 ++++++++++++++- .../grpc/tests/grpc_system_service_test.cpp | 184 ++++++++++ .../include/stop_operation_dispatcher.h | 10 +- .../src/stop_operation_dispatcher.cpp | 32 +- .../tests/stop_operation_dispatcher_test.cpp | 82 +++++ 15 files changed, 1377 insertions(+), 78 deletions(-) create mode 100644 cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index 2842c9af..63bbd9e6 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -3,6 +3,7 @@ #include "devices/arm/aubo_arm/aubo_motion_result.h" #include "devices/arm/aubo_arm/aubo_motion_state.h" #include "devices/arm/aubo_arm/aubo_safety_state.h" +#include "devices/arm/aubo_arm/aubo_torque_on_result.h" #include #include @@ -547,9 +548,30 @@ bool enforceControllerTermination( { std::unique_lock termination_lock(monitor->termination_mutex); monitor->motion_state->cancelActiveForSafety(); - const auto stop_request = monitor->motion_state->beginStop(); + auto stop_request = monitor->motion_state->beginStop(); if (!stop_request.started()) { - return monitor->cancellation_confirmed.load(); + constexpr auto kExistingStopTimeout = std::chrono::seconds(6); + const auto existing_result = + monitor->motion_state->waitForStopCompletion( + stop_request, + std::chrono::duration_cast( + kExistingStopTimeout)); + if (existing_result == aubo_internal::StopWaitStatus::Timeout) { + monitor->cancellation_confirmed.store(false); + return false; + } + + // A regular stopMotion confirms direct-motion idle, while safety + // termination additionally verifies runtime, servo and controller + // queues. Re-enter the stop state and perform that stronger check. A + // failed existing stop is retried here as well, while MotionState + // remains fail-closed between the two attempts. + monitor->motion_state->cancelActiveForSafety(); + stop_request = monitor->motion_state->beginStop(); + if (!stop_request.started()) { + monitor->cancellation_confirmed.store(false); + return false; + } } const auto fail = [&monitor]() { @@ -1010,18 +1032,38 @@ RobotInterfacePtr getPrimaryRobotInterface(const std::shared_ptr& cancellation_requested) { const auto start_time = std::chrono::steady_clock::now(); while (std::chrono::steady_clock::now() - start_time < std::chrono::seconds(20)) { + if (cancellationRequested(cancellation_requested)) { + return RobotModeWaitResult::Cancelled; + } const auto current_mode = robot_interface->getRobotState()->getRobotModeType(); if (current_mode == target_mode) { - return true; + return RobotModeWaitResult::Reached; } std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - return false; + return cancellationRequested(cancellation_requested) + ? RobotModeWaitResult::Cancelled + : RobotModeWaitResult::Timeout; +} + +bool waitForRobotMode(const RobotInterfacePtr& robot_interface, + const RobotModeType& target_mode) +{ + return waitForRobotMode(robot_interface, target_mode, {}) == + RobotModeWaitResult::Reached; } template @@ -1487,6 +1529,12 @@ bool AuboArm::busy() const } Result AuboArm::torqueOn() +{ + return torqueOn({}); +} + +Result AuboArm::torqueOn( + const std::function& cancellation_requested) { std::shared_ptr rpc_client; std::shared_ptr monitor; @@ -1500,22 +1548,82 @@ Result AuboArm::torqueOn() monitor = sdk_->safety_monitor; } + bool cancellation_latched = false; + bool controller_mutated = false; + const std::function cancellation_check = + [&cancellation_requested, &cancellation_latched]() { + if (!cancellation_latched) { + cancellation_latched = cancellationRequested( + cancellation_requested); + } + return cancellation_latched; + }; + const auto cancelled_before_startup = []() { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] torqueOn cancelled before controller startup"); + }; + const auto cancellation_result = [&]() -> std::optional { + if (!cancellation_check()) { + return std::nullopt; + } + if (!controller_mutated) { + return cancelled_before_startup(); + } + + try { + std::unique_lock cleanup_rpc_lock( + monitor->command_rpc_mutex); + cancelForSafetyTransition(monitor); + if (!enforceControllerTermination(rpc_client, monitor)) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn cancelled: control ownership was revoked, but controller safety termination could not be confirmed"); + } + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] torqueOn cancelled: control ownership was revoked; controller safety termination confirmed"); + } catch (const std::exception& e) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn cancelled: controller safety termination raised an exception: " + + std::string(e.what())); + } catch (...) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn cancelled: controller safety termination raised an unknown exception"); + } + }; + try { if (!monitor) { return Result::failure( ArmErrorCode::RobotNotReady, "[AuboArm] torqueOn failed: hardware safety monitor is unavailable"); } + + if (cancellation_check()) { + return cancelled_before_startup(); + } std::unique_lock command_rpc_lock( monitor->command_rpc_mutex); + if (cancellation_check()) { + return cancelled_before_startup(); + } Result interface_result; auto robot_interface = getPrimaryRobotInterface( rpc_client, "torqueOn", interface_result); if (!interface_result.ok()) { return interface_result; } + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } refreshSafetySample(rpc_client, monitor, robot_interface); + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } auto safety_snapshot = monitor->safety_state->snapshot(); const std::uint64_t entry_safety_epoch = safety_snapshot.epoch; if (monitor->emergency_stop_source.load() != 0) { @@ -1554,21 +1662,37 @@ Result AuboArm::torqueOn() std::string(safetyConditionName(condition))); } - const int restart_ret = - robot_interface->getRobotManage()->restartInterfaceBoard(); - if (restart_ret != arcs::common_interface::AUBO_OK) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] torqueOn recovery failed: restartInterfaceBoard ret=" + - std::to_string(restart_ret)); + controller_mutated = true; + if (const auto mutation_result = + aubo_internal::runTorqueOnControllerMutation( + arcs::common_interface::AUBO_OK, + [&robot_interface] { + return robot_interface->getRobotManage() + ->restartInterfaceBoard(); + }, + [](const int return_code) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn recovery failed: " + "restartInterfaceBoard ret=" + + std::to_string(return_code)); + }, + cancellation_result)) { + return *mutation_result; } const auto safety_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(10); do { + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } std::this_thread::sleep_for( std::chrono::milliseconds(100)); + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } refreshSafetySample( rpc_client, monitor, robot_interface); safety_snapshot = monitor->safety_state->snapshot(); @@ -1580,6 +1704,10 @@ Result AuboArm::torqueOn() safety_deadline); } + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } + safety_snapshot = monitor->safety_state->snapshot(); if (safety_snapshot.epoch != entry_safety_epoch) { return Result::failure( @@ -1595,6 +1723,9 @@ Result AuboArm::torqueOn() } if (recovering) { + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } const auto token = monitor->safety_state->beginRecovery( entry_safety_epoch); if (!token.has_value()) { @@ -1607,30 +1738,71 @@ Result AuboArm::torqueOn() monitor->safety_state, recovery_token); } + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } + double mass = 0.0; std::vector cog(3, 0.0); std::vector aom(3, 0.0); std::vector inertia(6, 0.0); - robot_interface->getRobotConfig()->setPayload(mass, cog, aom, inertia); + controller_mutated = true; + if (const auto mutation_result = + aubo_internal::runTorqueOnControllerMutation( + arcs::common_interface::AUBO_OK, + [&robot_interface, mass, &cog, &aom, &inertia] { + return robot_interface->getRobotConfig()->setPayload( + mass, cog, aom, inertia); + }, + [](const int return_code) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn failed: setPayload ret=" + + std::to_string(return_code)); + }, + cancellation_result)) { + return *mutation_result; + } auto current_mode = robot_interface->getRobotState()->getRobotModeType(); if (current_mode != RobotModeType::Running && current_mode != RobotModeType::Idle) { - const int poweron_ret = - robot_interface->getRobotManage()->poweron(); - if (poweron_ret != arcs::common_interface::AUBO_OK) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] torqueOn failed: poweron ret=" + - std::to_string(poweron_ret)); + if (const auto cancelled = cancellation_result()) { + return *cancelled; } - if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Idle)) { + controller_mutated = true; + if (const auto mutation_result = + aubo_internal::runTorqueOnControllerMutation( + arcs::common_interface::AUBO_OK, + [&robot_interface] { + return robot_interface->getRobotManage()->poweron(); + }, + [](const int return_code) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn failed: poweron ret=" + + std::to_string(return_code)); + }, + cancellation_result)) { + return *mutation_result; + } + const auto idle_wait = waitForRobotMode( + robot_interface, + arcs::common_interface::RobotModeType::Idle, + cancellation_check); + if (idle_wait != RobotModeWaitResult::Reached) { + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Idle"); } current_mode = RobotModeType::Idle; } + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } refreshSafetySample(rpc_client, monitor, robot_interface); auto before_brake_release = monitor->safety_state->snapshot(); @@ -1651,6 +1823,10 @@ Result AuboArm::torqueOn() } if (recovering) { + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } + controller_mutated = true; cancelForSafetyTransition(monitor); const bool cleanup_ok = current_mode == RobotModeType::Running ? enforceControllerTermination(rpc_client, monitor) @@ -1660,23 +1836,50 @@ Result AuboArm::torqueOn() ArmErrorCode::CommandFailed, "[AuboArm] torqueOn recovery failed: old controller queue could not be acknowledged and cleared before brake release"); } + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } } if (current_mode != RobotModeType::Running) { - const int startup_ret = - robot_interface->getRobotManage()->startup(); - if (startup_ret != arcs::common_interface::AUBO_OK) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] torqueOn failed: startup ret=" + - std::to_string(startup_ret)); + if (const auto cancelled = cancellation_result()) { + return *cancelled; } - if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Running)) { + controller_mutated = true; + if (const auto mutation_result = + aubo_internal::runTorqueOnControllerMutation( + arcs::common_interface::AUBO_OK, + [&robot_interface] { + return robot_interface->getRobotManage()->startup(); + }, + [](const int return_code) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn failed: startup ret=" + + std::to_string(return_code)); + }, + cancellation_result)) { + return *mutation_result; + } + const auto running_wait = waitForRobotMode( + robot_interface, + arcs::common_interface::RobotModeType::Running, + cancellation_check); + if (running_wait != RobotModeWaitResult::Reached) { + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Running"); } } + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } refreshSafetySample(rpc_client, monitor, robot_interface); + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } const auto after_startup = monitor->safety_state->snapshot(); const bool post_recovery_token_current = !recovering || (after_startup.recovery_in_progress && @@ -1692,12 +1895,18 @@ Result AuboArm::torqueOn() "[AuboArm] torqueOn rejected: safety state changed during startup; the new event remains latched"); } if (recovering) { + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } cancelForSafetyTransition(monitor); if (!enforceControllerTermination(rpc_client, monitor)) { return Result::failure( ArmErrorCode::CommandFailed, "[AuboArm] torqueOn recovery failed: controller did not reach an empty, steady state after startup"); } + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } refreshSafetySample(rpc_client, monitor, robot_interface); const bool robot_running = monitor->robot_mode.load() == @@ -1711,11 +1920,31 @@ Result AuboArm::torqueOn() "[AuboArm] torqueOn recovery failed: safety state changed during recovery"); } } + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } emergency_stopped_.store(false); servo_mode_.store(false); + if (const auto cancelled = cancellation_result()) { + return *cancelled; + } return Result::success(); } catch (const std::exception& e) { - return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOn failed: ") + e.what()); + auto primary_failure = Result::failure( + ArmErrorCode::CommandFailed, + std::string("[AuboArm] torqueOn failed: ") + e.what()); + return controller_mutated + ? aubo_internal::preservePrimaryTorqueOnFailure( + std::move(primary_failure), cancellation_result()) + : primary_failure; + } catch (...) { + auto primary_failure = Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn failed: unknown exception"); + return controller_mutated + ? aubo_internal::preservePrimaryTorqueOnFailure( + std::move(primary_failure), cancellation_result()) + : primary_failure; } } @@ -2293,9 +2522,28 @@ Result AuboArm::stopMotion_( const auto stop_request = motion_state->beginStop(forced_kind); if (!stop_request.started()) { + busy_.store(true); + constexpr auto kExistingStopTimeout = + std::chrono::seconds(6); + const auto existing_result = + motion_state->waitForStopCompletion( + stop_request, + std::chrono::duration_cast( + kExistingStopTimeout)); + busy_.store(motion_state->busy()); + if (existing_result == + aubo_internal::StopWaitStatus::Completed) { + return Result::success(); + } + if (existing_result == + aubo_internal::StopWaitStatus::Timeout) { + return Result::failure( + ArmErrorCode::Timeout, + "[AuboArm] stopMotion failed: timeout waiting for the existing stop operation to complete"); + } return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] stopMotion rejected: another stop operation is in progress"); + ArmErrorCode::CommandFailed, + "[AuboArm] stopMotion failed: the existing stop operation could not confirm controller idle"); } busy_.store(true); StopStateGuard stop_state_guard{ diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h index 4f22ddfe..8665aecc 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -36,6 +37,8 @@ public: bool supportsActionQueueMotion() const noexcept override { return true; } Result torqueOn() override; + Result torqueOn( + const std::function& cancellation_requested) override; Result torqueOff() override; Result calibrateZeroQ(const std::string& joint_name) override; Result emergencyStop() override; diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h b/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h index f3012c89..84c84367 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h @@ -5,6 +5,7 @@ #include #include #include +#include #include namespace cmvr::device::aubo_internal { @@ -54,11 +55,50 @@ enum class StopStartStatus { AlreadyStopping, }; +enum class StopWaitStatus { + Completed, + Failed, + Timeout, +}; + +class StopCompletion final { +public: + StopWaitStatus waitFor(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + if (!cv_.wait_for(lock, timeout, [this]() { return completed_; })) { + return StopWaitStatus::Timeout; + } + return succeeded_ + ? StopWaitStatus::Completed + : StopWaitStatus::Failed; + } + +private: + friend class MotionState; + + void finish(const bool succeeded) + { + { + std::lock_guard lock(mutex_); + succeeded_ = succeeded; + completed_ = true; + } + cv_.notify_all(); + } + + std::mutex mutex_; + std::condition_variable cv_; + bool completed_{false}; + bool succeeded_{false}; +}; + struct StopRequest { StopStartStatus status{StopStartStatus::AlreadyStopping}; MotionKind kind{MotionKind::None}; MotionToken active_token; bool tracked_motion{false}; + std::shared_ptr completion; bool started() const noexcept { @@ -152,10 +192,17 @@ public: { std::lock_guard lock(mutex_); if (stop_in_progress_) { - return {}; + return { + StopStartStatus::AlreadyStopping, + MotionKind::None, + {}, + false, + active_stop_completion_}; } + auto completion = std::make_shared(); stop_in_progress_ = true; + active_stop_completion_ = completion; const MotionToken active = owner_active_ ? active_token_ : MotionToken{}; @@ -186,7 +233,8 @@ public: StopStartStatus::Started, kind, active, - tracked_motion}; + tracked_motion, + completion}; } SafetyCancelResult cancelActiveForSafety() @@ -248,27 +296,51 @@ public: active_token_.generation == token.generation; } + StopWaitStatus waitForStopCompletion( + const StopRequest& request, + const std::chrono::milliseconds timeout) const + { + if (!request.completion) { + return StopWaitStatus::Failed; + } + return request.completion->waitFor(timeout); + } + bool completeStop() { - std::lock_guard lock(mutex_); - if (owner_active_) { - return false; + std::shared_ptr completion; + { + std::lock_guard lock(mutex_); + if (owner_active_) { + return false; + } + stop_in_progress_ = false; + blocked_ = false; + active_token_ = {}; + last_kind_ = MotionKind::None; + previous_kind_ = MotionKind::None; + completion = std::move(active_stop_completion_); + owner_finished_cv_.notify_all(); + } + if (completion) { + completion->finish(true); } - stop_in_progress_ = false; - blocked_ = false; - active_token_ = {}; - last_kind_ = MotionKind::None; - previous_kind_ = MotionKind::None; - owner_finished_cv_.notify_all(); return true; } void failStop() { - std::lock_guard lock(mutex_); - stop_in_progress_ = false; - blocked_ = true; - owner_finished_cv_.notify_all(); + std::shared_ptr completion; + { + std::lock_guard lock(mutex_); + stop_in_progress_ = false; + blocked_ = true; + completion = std::move(active_stop_completion_); + owner_finished_cv_.notify_all(); + } + if (completion) { + completion->finish(false); + } } bool busy() const @@ -286,6 +358,7 @@ private: MotionToken active_token_; MotionKind last_kind_{MotionKind::None}; MotionKind previous_kind_{MotionKind::None}; + std::shared_ptr active_stop_completion_; bool owner_active_{false}; bool stop_in_progress_{false}; bool blocked_{false}; diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h b/cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h new file mode 100644 index 00000000..4f2f1365 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h @@ -0,0 +1,50 @@ +#ifndef CMVR_ES_AUBO_TORQUE_ON_RESULT_H +#define CMVR_ES_AUBO_TORQUE_ON_RESULT_H + +#include +#include +#include + +#include "common/types/arm/arm_types.h" + +namespace cmvr::device::aubo_internal { + +inline Result preservePrimaryTorqueOnFailure( + Result primary_failure, + const std::optional& cancellation_outcome) +{ + if (!cancellation_outcome.has_value() || + cancellation_outcome->message.empty()) { + return primary_failure; + } + + if (!primary_failure.message.empty()) { + primary_failure.message += "; "; + } + primary_failure.message += + "cancellation handling: " + cancellation_outcome->message; + return primary_failure; +} + +template +std::optional runTorqueOnControllerMutation( + const int success_code, + Mutation&& mutation, + FailureResult&& failure_result, + CancellationOutcome&& cancellation_outcome) +{ + const int return_code = std::forward(mutation)(); + if (return_code != success_code) { + auto primary_failure = + std::forward(failure_result)(return_code); + return preservePrimaryTorqueOnFailure( + std::move(primary_failure), + std::forward(cancellation_outcome)()); + } + return std::forward(cancellation_outcome)(); +} + +} // namespace cmvr::device::aubo_internal + +#endif // CMVR_ES_AUBO_TORQUE_ON_RESULT_H diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp index fb1656e5..57bca235 100644 --- a/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp +++ b/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp @@ -1,6 +1,10 @@ #include "devices/arm/aubo_arm/aubo_motion_result.h" +#include "devices/arm/aubo_arm/aubo_torque_on_result.h" #include +#include +#include +#include namespace { @@ -19,7 +23,9 @@ int main() { using cmvr::device::aubo_internal::MotionCommandOutcome; using cmvr::device::aubo_internal::MotionWaitResult; + using cmvr::device::aubo_internal::preservePrimaryTorqueOnFailure; using cmvr::device::aubo_internal::resolveMotionCommand; + using cmvr::device::aubo_internal::runTorqueOnControllerMutation; constexpr int success_code = 0; constexpr int request_ignore_code = 13; @@ -79,5 +85,74 @@ int main() wait_cancelled) == MotionCommandOutcome::Cancelled); CHECK_TRUE(wait_calls == 1); + using cmvr::device::ArmErrorCode; + using cmvr::device::Result; + std::vector mutation_trace; + const auto failed_mutation = runTorqueOnControllerMutation( + success_code, + [&mutation_trace] { + mutation_trace.push_back("mutation"); + return 42; + }, + [&mutation_trace](const int return_code) { + mutation_trace.push_back("primary-failure"); + return Result::failure( + ArmErrorCode::CommandFailed, + "vendor failure ret=" + std::to_string(return_code)); + }, + [&mutation_trace]() -> std::optional { + mutation_trace.push_back("cancellation-cleanup"); + return Result::failure( + ArmErrorCode::CommandRejected, + "controller safety termination confirmed"); + }); + CHECK_TRUE(failed_mutation.has_value()); + CHECK_TRUE(failed_mutation->code == ArmErrorCode::CommandFailed); + CHECK_TRUE( + failed_mutation->message.find("vendor failure ret=42") != + std::string::npos); + CHECK_TRUE( + failed_mutation->message.find( + "controller safety termination confirmed") != + std::string::npos); + CHECK_TRUE(mutation_trace.size() == 3); + CHECK_TRUE(mutation_trace[0] == "mutation"); + CHECK_TRUE(mutation_trace[1] == "primary-failure"); + CHECK_TRUE(mutation_trace[2] == "cancellation-cleanup"); + + const auto cleanup_failed = preservePrimaryTorqueOnFailure( + Result::failure( + ArmErrorCode::CommandFailed, + "vendor exception"), + std::optional{Result::failure( + ArmErrorCode::CommandFailed, + "safety termination failed")}); + CHECK_TRUE(cleanup_failed.code == ArmErrorCode::CommandFailed); + CHECK_TRUE( + cleanup_failed.message.find("vendor exception") != + std::string::npos); + CHECK_TRUE( + cleanup_failed.message.find("safety termination failed") != + std::string::npos); + + int successful_cancellation_checks = 0; + const auto cancelled_after_success = runTorqueOnControllerMutation( + success_code, + [] { return 0; }, + [](const int) { + return Result::failure( + ArmErrorCode::CommandFailed, "must not be used"); + }, + [&successful_cancellation_checks]() -> std::optional { + ++successful_cancellation_checks; + return Result::failure( + ArmErrorCode::CommandRejected, + "cancelled after successful mutation"); + }); + CHECK_TRUE(cancelled_after_success.has_value()); + CHECK_TRUE( + cancelled_after_success->code == ArmErrorCode::CommandRejected); + CHECK_TRUE(successful_cancellation_checks == 1); + return 0; } diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp index bbeb9169..6f68937d 100644 --- a/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp +++ b/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp @@ -1,7 +1,9 @@ #include "devices/arm/aubo_arm/aubo_motion_state.h" +#include #include #include +#include namespace { @@ -36,8 +38,13 @@ int main() joint.token.generation); CHECK_TRUE(stop_joint.tracked_motion); CHECK_TRUE(state.cancelled(joint.token)); - CHECK_TRUE(state.beginStop().status == + const auto joined_stop_joint = state.beginStop(); + CHECK_TRUE(joined_stop_joint.status == StopStartStatus::AlreadyStopping); + CHECK_TRUE( + state.waitForStopCompletion( + joined_stop_joint, std::chrono::milliseconds(1)) == + StopWaitStatus::Timeout); CHECK_TRUE(state.begin(MotionKind::Linear).status == MotionStartStatus::Stopping); CHECK_TRUE(!state.waitForOwnerExit( @@ -48,6 +55,10 @@ int main() CHECK_TRUE(state.waitForOwnerExit( joint.token, std::chrono::milliseconds(1))); CHECK_TRUE(state.completeStop()); + CHECK_TRUE( + state.waitForStopCompletion( + joined_stop_joint, std::chrono::milliseconds(1)) == + StopWaitStatus::Completed); CHECK_TRUE(!state.busy()); const auto linear = state.begin(MotionKind::Linear); @@ -81,7 +92,14 @@ int main() const auto idle_stop = state.beginStop(); CHECK_TRUE(idle_stop.kind == MotionKind::None); CHECK_TRUE(!idle_stop.tracked_motion); + const auto joined_idle_stop = state.beginStop(); + CHECK_TRUE(joined_idle_stop.status == + StopStartStatus::AlreadyStopping); state.failStop(); + CHECK_TRUE( + state.waitForStopCompletion( + joined_idle_stop, std::chrono::milliseconds(1)) == + StopWaitStatus::Failed); const auto retry_idle_stop = state.beginStop(); CHECK_TRUE(retry_idle_stop.kind == MotionKind::None); CHECK_TRUE(!retry_idle_stop.tracked_motion); @@ -152,5 +170,44 @@ int main() CHECK_TRUE(retained_stop.tracked_motion); CHECK_TRUE(state.completeStop()); + // A waiter keeps the completion for the stop it joined even when another + // stop starts and finishes before the waiter is scheduled again. + const auto concurrent_first_stop = state.beginStop(); + CHECK_TRUE(concurrent_first_stop.started()); + const auto concurrent_first_join = state.beginStop(); + CHECK_TRUE(concurrent_first_join.status == + StopStartStatus::AlreadyStopping); + std::atomic waiter_entered{false}; + std::atomic first_wait_result{ + StopWaitStatus::Timeout}; + std::thread first_waiter([&]() { + waiter_entered.store(true, std::memory_order_release); + first_wait_result.store( + state.waitForStopCompletion( + concurrent_first_join, + std::chrono::milliseconds(500)), + std::memory_order_release); + }); + while (!waiter_entered.load(std::memory_order_acquire)) { + std::this_thread::yield(); + } + + const bool concurrent_first_completed = state.completeStop(); + const auto concurrent_second_stop = state.beginStop(); + const auto concurrent_second_join = state.beginStop(); + state.failStop(); + first_waiter.join(); + + CHECK_TRUE(concurrent_first_completed); + CHECK_TRUE(concurrent_second_stop.started()); + CHECK_TRUE(concurrent_second_join.status == + StopStartStatus::AlreadyStopping); + CHECK_TRUE(first_wait_result.load(std::memory_order_acquire) == + StopWaitStatus::Completed); + CHECK_TRUE( + state.waitForStopCompletion( + concurrent_second_join, std::chrono::milliseconds(1)) == + StopWaitStatus::Failed); + return 0; } diff --git a/cmvr-es/devices/arm/robot_arm.h b/cmvr-es/devices/arm/robot_arm.h index 4f63cf4d..fe8442e3 100644 --- a/cmvr-es/devices/arm/robot_arm.h +++ b/cmvr-es/devices/arm/robot_arm.h @@ -2,6 +2,7 @@ #define CMVR_ES_ROBOT_ARM_H #include +#include #include #include #include @@ -44,6 +45,28 @@ public: } virtual Result torqueOn() = 0; + // Long-running startup implementations may cooperatively observe loss of + // their control lease. Backends which have not adopted cancellation retain + // the legacy behavior, while still rejecting an already-cancelled request + // before entering the vendor API. + virtual Result torqueOn( + const std::function& cancellation_requested) + { + if (cancellation_requested) { + try { + if (cancellation_requested()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "torqueOn cancelled before execution"); + } + } catch (...) { + return Result::failure( + ArmErrorCode::CommandRejected, + "torqueOn cancellation check failed"); + } + } + return torqueOn(); + } virtual Result torqueOff() = 0; virtual Result calibrateZeroQ(const std::string& joint_name) = 0; diff --git a/cmvr-es/service/grpc/include/grpc_system_service.h b/cmvr-es/service/grpc/include/grpc_system_service.h index e7f714fe..6b126b5f 100644 --- a/cmvr-es/service/grpc/include/grpc_system_service.h +++ b/cmvr-es/service/grpc/include/grpc_system_service.h @@ -5,6 +5,7 @@ #ifndef GRPC_SYSTEM_SERVICE_H #define GRPC_SYSTEM_SERVICE_H +#include #include #include "cmvr/api/system_service.grpc.pb.h" @@ -19,7 +20,12 @@ namespace cmvr::service class gRPCSystemServiceImpl: public api::SystemService::Service { public: gRPCSystemServiceImpl(); + explicit gRPCSystemServiceImpl( + std::chrono::milliseconds stop_timeout); ~gRPCSystemServiceImpl() override; + // Exposed only to synchronize lifecycle concurrency tests. + static bool waitForStopDispatcherDestructionForTesting( + std::chrono::milliseconds timeout); // Called by GrpcServerTask before grpc::Server::Shutdown so accepted // ActionQueue handlers can reach a terminal result and do not hold the // synchronous server shutdown open indefinitely. @@ -32,10 +38,10 @@ namespace cmvr::service grpc::Status ExecuteActionQueue(grpc::ServerContext* context, const cmvr::api::ActionQueueCommand_Request* request, cmvr::api::ActionQueueCommand_Feedback* response) override; private: device::DeviceManager& dmgr_; - // Outlives ActionQueueExecutor and every StopAll RPC stack. A stop - // backend which ignores the shared deadline can therefore finish in - // its owned worker without accessing destroyed RPC-local state. - std::unique_ptr stop_dispatcher_; + const std::chrono::milliseconds stop_timeout_; + // Process instances share running jobs through a lifecycle registry. + // The last service owner joins every worker before replacement. + std::shared_ptr stop_dispatcher_; std::unique_ptr action_queue_; }; } diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index 89a84745..1fd6ef0e 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -173,6 +173,19 @@ grpc::Status setControlLeaseConflict( response->mutable_header(), device_id, detail); } +grpc::Status setControlCancelled( + api::CommandHeader_Feedback* response, + const std::string& device_id, + const std::string& reason) +{ + std::string message = "RobotArm control was cancelled: " + device_id; + if (!reason.empty()) { + message += ", " + reason; + } + fillFeedback(response, false, message); + return grpc::Status(grpc::StatusCode::CANCELLED, message); +} + class ScopedUnaryControlLease final { public: ScopedUnaryControlLease( @@ -383,7 +396,7 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*, } } -grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*, +grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext* context, const api::CommandHeader_Request* request, api::CommandHeader_Feedback* response) { @@ -404,7 +417,27 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*, return setControlDispatchFailure( response, device_id, control_lease, "torqueOn"); } - const auto result = arm->torqueOn(); + const auto cancellation_requested = + control_lease.cancellationRequested(context); + const auto result = arm->torqueOn(cancellation_requested); + const bool control_current = control_lease.current(); + const bool cancellation_result = + result.ok() || + result.code == device::ArmErrorCode::CommandRejected; + if (cancellation_result && + !control_lease.admissionCurrent()) { + return setStopAllRejected(response, device_id); + } + const bool rpc_cancelled = context && context->IsCancelled(); + if (cancellation_result && + (rpc_cancelled || !control_current)) { + return setControlCancelled( + response, + device_id, + rpc_cancelled + ? "the RPC was cancelled" + : "control ownership was revoked"); + } fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { logRpcSuccess("torqueOn", device_id); diff --git a/cmvr-es/service/grpc/src/grpc_system_service.cpp b/cmvr-es/service/grpc/src/grpc_system_service.cpp index 2d7fb369..d39563fa 100644 --- a/cmvr-es/service/grpc/src/grpc_system_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_system_service.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -407,6 +408,92 @@ using StopDispatcher = cmvr::service::StopOperationDispatcher; using StopHandle = StopDispatcher::Handle; using StopOutcome = StopDispatcher::OperationResult; +struct ProcessStopDispatcherRegistry final { + std::mutex mutex; + std::condition_variable available; + std::weak_ptr dispatcher; + bool destroying{false}; +}; + +ProcessStopDispatcherRegistry& processStopDispatcherRegistry() +{ + static ProcessStopDispatcherRegistry registry; + return registry; +} + +void acquireProcessStopDispatcher( + std::shared_ptr& owner) +{ + auto& registry = processStopDispatcherRegistry(); + std::unique_lock lock(registry.mutex); + registry.available.wait( + lock, [®istry] { return !registry.destroying; }); + if (auto existing = registry.dispatcher.lock()) { + owner = std::move(existing); + return; + } + + owner = std::make_shared(); + registry.dispatcher = owner; +} + +void releaseProcessStopDispatcher( + std::shared_ptr& owner) noexcept +{ + if (!owner) { + return; + } + + auto& registry = processStopDispatcherRegistry(); + std::unique_lock lock(registry.mutex); + if (owner.use_count() > 1) { + owner.reset(); + return; + } + + // Do not admit a replacement while the last dispatcher is joining a + // worker which may still be inside a device driver. + registry.destroying = true; + registry.dispatcher.reset(); + registry.available.notify_all(); + auto last_owner = std::move(owner); + lock.unlock(); + last_owner.reset(); + lock.lock(); + registry.destroying = false; + lock.unlock(); + registry.available.notify_all(); +} + +class StopDeadlineDetail final { +public: + void publishInitial(std::string detail) + { + std::lock_guard lock(mutex_); + detail_ = std::move(detail); + } + + void publishUntil( + const StopDispatcher::Deadline deadline, + std::string detail) + { + std::lock_guard lock(mutex_); + if (StopDispatcher::Clock::now() < deadline) { + detail_ = std::move(detail); + } + } + + std::string snapshot() const + { + std::lock_guard lock(mutex_); + return detail_; + } + +private: + mutable std::mutex mutex_; + std::string detail_; +}; + struct StopHandleEntry final { std::string device_id; StopHandle handle; @@ -565,8 +652,13 @@ StopOutcome stopControlWithFence( const StopDispatcher::Deadline deadline, InitialStop&& initial_stop, FinalStop&& final_stop, - const std::string& description) + const std::string& description, + const std::shared_ptr& deadline_detail) { + deadline_detail->publishUntil( + deadline, + "initial " + description + + " stop did not complete before the StopAll deadline"); const auto initial = invokeStopOperation( std::forward(initial_stop), "initial " + description + " stop"); @@ -582,18 +674,37 @@ StopOutcome stopControlWithFence( " safety barrier was unavailable after the initial stop"}; } + const std::string handler_timeout_detail = + "timed out waiting for the preempted " + description + + " control handler to exit"; + deadline_detail->publishUntil(deadline, handler_timeout_detail); const bool handler_drained = cmvr::control::ControlAuthorityManager::instance() .waitForPreemptedRelease( barrier, remainingStopBudget(deadline)); + if (handler_drained) { + deadline_detail->publishUntil( + deadline, + "final " + description + + " stop did not complete before the StopAll deadline"); + } + // This final typed stop remains mandatory even when the handler wait used + // the entire RPC budget. The dispatcher owns this worker past the RPC + // deadline, closing the race where an old handler resumes after the first + // stop while the safety barrier remains fail-closed. const auto final = invokeStopOperation( std::forward(final_stop), "final " + description + " stop"); if (!handler_drained) { - return { - false, - "timed out waiting for the preempted " + description + - " control handler to exit"}; + std::string detail = handler_timeout_detail; + if (!final.success) { + detail += "; " + + (final.detail.empty() + ? "final " + description + + " stop was not confirmed" + : final.detail); + } + return {false, std::move(detail)}; } return final; } @@ -927,13 +1038,42 @@ cmvr::api::SystemDeviceHealth toApiDeviceHealth( } // namespace gRPCSystemServiceImpl::gRPCSystemServiceImpl() - : dmgr_(DeviceManager::getInstance()), - stop_dispatcher_(std::make_unique()), - action_queue_(std::make_unique(dmgr_)) + : gRPCSystemServiceImpl(std::chrono::seconds(15)) { } -gRPCSystemServiceImpl::~gRPCSystemServiceImpl() = default; +gRPCSystemServiceImpl::gRPCSystemServiceImpl( + const std::chrono::milliseconds stop_timeout) + : dmgr_(DeviceManager::getInstance()), + stop_timeout_( + stop_timeout > std::chrono::milliseconds::zero() + ? stop_timeout + : std::chrono::seconds(15)), + action_queue_(std::make_unique(dmgr_)) +{ + // Acquire only after ActionQueue construction succeeds. This ensures an + // exception cannot release the last dispatcher outside the registry. + acquireProcessStopDispatcher(stop_dispatcher_); +} + +gRPCSystemServiceImpl::~gRPCSystemServiceImpl() +{ + // The dispatcher intentionally outlives ActionQueue, then joins any + // deadline-overrunning stop workers before the last service disappears. + action_queue_.reset(); + releaseProcessStopDispatcher(stop_dispatcher_); +} + +bool gRPCSystemServiceImpl::waitForStopDispatcherDestructionForTesting( + const std::chrono::milliseconds timeout) +{ + auto& registry = processStopDispatcherRegistry(); + std::unique_lock lock(registry.mutex); + return registry.available.wait_for( + lock, + timeout, + [®istry] { return registry.destroying; }); +} void gRPCSystemServiceImpl::prepareForShutdown() { @@ -1079,9 +1219,8 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) { (void)request; - constexpr auto stop_timeout = std::chrono::seconds(15); const auto stop_deadline = - std::chrono::steady_clock::now() + stop_timeout; + std::chrono::steady_clock::now() + stop_timeout_; std::unique_lock stop_all_lock( processStopAllMutex(), std::defer_lock); @@ -1266,41 +1405,60 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, control_stops.reserve(control_targets.size()); for (const auto& target : control_targets) { StopHandle handle; + auto deadline_detail = std::make_shared(); if (target.arm) { const auto arm = target.arm; const auto barrier = target.barrier; + deadline_detail->publishInitial( + "initial RobotArm stop did not complete before the " + "StopAll deadline"); handle = stop_dispatcher_->submit( "control:" + stopResourceKey(target.id), - [arm, barrier, stop_deadline] { + [arm, barrier, stop_deadline, deadline_detail] { return stopControlWithFence( barrier, stop_deadline, [arm] { return stopArm(arm, false); }, [arm] { return stopArm(arm, true); }, - "RobotArm"); + "RobotArm", deadline_detail); + }, + [deadline_detail] { + return deadline_detail->snapshot(); }); } else if (target.agv) { const auto agv = target.agv; const auto barrier = target.barrier; + deadline_detail->publishInitial( + "initial AGV stop did not complete before the StopAll " + "deadline"); handle = stop_dispatcher_->submit( "control:" + stopResourceKey(target.id), - [agv, barrier, stop_deadline] { + [agv, barrier, stop_deadline, deadline_detail] { return stopControlWithFence( barrier, stop_deadline, [agv] { return stopAgv(agv, false); }, [agv] { return stopAgv(agv, true); }, - "AGV"); + "AGV", deadline_detail); + }, + [deadline_detail] { + return deadline_detail->snapshot(); }); } else { const auto hand = target.hand; const auto barrier = target.barrier; + deadline_detail->publishInitial( + "initial DexHand stop did not complete before the " + "StopAll deadline"); handle = stop_dispatcher_->submit( "control:" + stopResourceKey(target.id), - [hand, barrier, stop_deadline] { + [hand, barrier, stop_deadline, deadline_detail] { return stopControlWithFence( barrier, stop_deadline, [hand] { return stopDexHand(hand, false); }, [hand] { return stopDexHand(hand, true); }, - "DexHand"); + "DexHand", deadline_detail); + }, + [deadline_detail] { + return deadline_detail->snapshot(); }); } control_stops.push_back( diff --git a/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp index f5d24016..19108a25 100644 --- a/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp @@ -1,23 +1,28 @@ #include "service/grpc/include/grpc_arm_service.h" #include +#include #include +#include #include #include #include #include #include #include +#include #include #include #include #include #include +#include #include "cmvr/config/device_manager_config/device_manager_config.pb.h" #include "manager/control_authority/include/control_authority_manager.h" #include "manager/device_manager/include/device_manager.h" +#include "service/grpc/include/grpc_error_logging_interceptor.h" #include "service/stop_all/include/stop_all_admission_gate.h" namespace cmvr::service { @@ -63,7 +68,43 @@ public: return device::ControlMode::None; } - device::Result torqueOn() override { return device::Result::success(); } + device::Result torqueOn() override { return torqueOn({}); } + device::Result torqueOn( + const std::function& cancellation_requested) override + { + std::unique_lock lock(motion_mutex_); + ++torque_on_calls_; + last_torque_on_had_cancellation_ = + static_cast(cancellation_requested); + if (!block_next_torque_on_) { + return device::Result::success(); + } + + block_next_torque_on_ = false; + blocking_torque_on_started_ = true; + torque_on_started_cv_.notify_all(); + while (!release_blocking_torque_on_) { + lock.unlock(); + const bool cancelled = + cancellation_requested && cancellation_requested(); + lock.lock(); + if (cancelled) { + last_torque_on_cancellation_requested_ = true; + if (fail_next_torque_on_cancellation_) { + fail_next_torque_on_cancellation_ = false; + return device::Result::failure( + device::ArmErrorCode::CommandFailed, + "simulated torqueOn safety termination failure"); + } + return device::Result::failure( + device::ArmErrorCode::CommandRejected, + "simulated torqueOn cancellation"); + } + torque_on_release_cv_.wait_for( + lock, std::chrono::milliseconds(5)); + } + return device::Result::success(); + } device::Result torqueOff() override { std::lock_guard lock(motion_mutex_); @@ -159,6 +200,40 @@ public: blocking_motion_name_.clear(); } + void blockNextTorqueOn() + { + std::lock_guard lock(motion_mutex_); + block_next_torque_on_ = true; + blocking_torque_on_started_ = false; + release_blocking_torque_on_ = false; + last_torque_on_cancellation_requested_ = false; + } + + void failNextTorqueOnCancellation() + { + std::lock_guard lock(motion_mutex_); + fail_next_torque_on_cancellation_ = true; + } + + bool waitForBlockingTorqueOn( + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(motion_mutex_); + return torque_on_started_cv_.wait_for( + lock, + timeout, + [this]() { return blocking_torque_on_started_; }); + } + + void releaseBlockingTorqueOn() + { + { + std::lock_guard lock(motion_mutex_); + release_blocking_torque_on_ = true; + } + torque_on_release_cv_.notify_all(); + } + bool waitForBlockingMotion( const std::string& operation, const std::chrono::milliseconds timeout) @@ -244,6 +319,24 @@ public: return torque_off_calls_; } + int torqueOnCalls() const + { + std::lock_guard lock(motion_mutex_); + return torque_on_calls_; + } + + bool lastTorqueOnHadCancellation() const + { + std::lock_guard lock(motion_mutex_); + return last_torque_on_had_cancellation_; + } + + bool lastTorqueOnCancellationRequested() const + { + std::lock_guard lock(motion_mutex_); + return last_torque_on_cancellation_requested_; + } + bool lastMotionHadCancellation() const { std::lock_guard lock(motion_mutex_); @@ -388,6 +481,8 @@ private: std::condition_variable motion_release_cv_; std::condition_variable stop_started_cv_; std::condition_variable stop_release_cv_; + std::condition_variable torque_on_started_cv_; + std::condition_variable torque_on_release_cv_; bool block_next_motion_{false}; bool blocking_motion_started_{false}; bool release_blocking_motion_{false}; @@ -396,11 +491,18 @@ private: bool release_blocking_stop_{false}; bool fail_next_stop_{false}; bool throw_next_stop_{false}; + bool block_next_torque_on_{false}; + bool blocking_torque_on_started_{false}; + bool release_blocking_torque_on_{false}; + bool fail_next_torque_on_cancellation_{false}; std::string blocking_motion_name_; int move_j_calls_{0}; int move_l_calls_{0}; int stop_motion_calls_{0}; int torque_off_calls_{0}; + int torque_on_calls_{0}; + bool last_torque_on_had_cancellation_{false}; + bool last_torque_on_cancellation_requested_{false}; bool last_motion_had_cancellation_{false}; bool last_motion_cancellation_requested_{false}; }; @@ -520,6 +622,19 @@ protected: return service_->torqueOff(&context, &request, &response); } + MoveOutcome torqueOn(const std::string& device_id) + { + api::CommandHeader_Request request; + request.set_device_id(device_id); + api::CommandHeader_Feedback response; + grpc::ServerContext context; + auto status = service_->torqueOn(&context, &request, &response); + return { + std::move(status), + response.success(), + response.error_message()}; + } + std::shared_ptr left_arm_; std::shared_ptr aubo_arm_; std::shared_ptr non_arm_; @@ -718,6 +833,166 @@ TEST_F(GrpcArmServiceTest, MoveBindsLeaseRevocationCancellation) EXPECT_FALSE(aubo_arm_->lastMotionCancellationRequested()); } +TEST_F(GrpcArmServiceTest, + StopAllCancelsInFlightTorqueOnWithoutReportingSuccess) +{ + aubo_arm_->blockNextTorqueOn(); + auto blocked_torque_on = std::async( + std::launch::async, + [this]() { return torqueOn("aubo_arm"); }); + + const bool torque_on_started = aubo_arm_->waitForBlockingTorqueOn( + std::chrono::seconds(2)); + auto& admission = globalStopAllAdmissionGate(); + StopAllAdmissionGate::StopAllTicket stop_all_ticket; + if (torque_on_started) { + stop_all_ticket = admission.beginStopAll(); + } + + const bool cancelled_promptly = + blocked_torque_on.wait_for(std::chrono::seconds(2)) == + std::future_status::ready; + if (!cancelled_promptly) { + aubo_arm_->releaseBlockingTorqueOn(); + } + const auto outcome = blocked_torque_on.get(); + + ASSERT_TRUE(torque_on_started); + ASSERT_TRUE(stop_all_ticket.valid()); + EXPECT_TRUE(cancelled_promptly); + EXPECT_EQ(outcome.status.error_code(), grpc::StatusCode::UNAVAILABLE); + EXPECT_FALSE(outcome.response_success); + EXPECT_EQ(outcome.response_error, outcome.status.error_message()); + EXPECT_NE(outcome.response_error.find("StopAll"), std::string::npos); + EXPECT_EQ(aubo_arm_->torqueOnCalls(), 1); + EXPECT_TRUE(aubo_arm_->lastTorqueOnHadCancellation()); + EXPECT_TRUE(aubo_arm_->lastTorqueOnCancellationRequested()); + + EXPECT_TRUE(admission.finishStopAll(stop_all_ticket, true)); +} + +TEST_F(GrpcArmServiceTest, + StopAllPreservesTorqueOnSafetyTerminationFailure) +{ + aubo_arm_->blockNextTorqueOn(); + aubo_arm_->failNextTorqueOnCancellation(); + auto blocked_torque_on = std::async( + std::launch::async, + [this]() { return torqueOn("aubo_arm"); }); + + const bool torque_on_started = aubo_arm_->waitForBlockingTorqueOn( + std::chrono::seconds(2)); + auto& admission = globalStopAllAdmissionGate(); + StopAllAdmissionGate::StopAllTicket stop_all_ticket; + if (torque_on_started) { + stop_all_ticket = admission.beginStopAll(); + } + + const bool completed_promptly = + blocked_torque_on.wait_for(std::chrono::seconds(2)) == + std::future_status::ready; + if (!completed_promptly) { + aubo_arm_->releaseBlockingTorqueOn(); + } + const auto outcome = blocked_torque_on.get(); + + ASSERT_TRUE(torque_on_started); + ASSERT_TRUE(stop_all_ticket.valid()); + EXPECT_TRUE(completed_promptly); + EXPECT_EQ(outcome.status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_FALSE(outcome.response_success); + EXPECT_EQ( + outcome.response_error, + "simulated torqueOn safety termination failure"); + EXPECT_EQ(outcome.status.error_message(), outcome.response_error); + EXPECT_TRUE(aubo_arm_->lastTorqueOnCancellationRequested()); + + EXPECT_FALSE(admission.finishStopAll(stop_all_ticket, false)); +} + +TEST_F(GrpcArmServiceTest, + ClientCancellationPreservesTorqueOnSafetyTerminationFailure) +{ + std::mutex records_mutex; + std::condition_variable records_changed; + std::vector records; + + grpc::ServerBuilder builder; + const std::string socket_path = + "/tmp/cmvr_grpc_arm_service_test_" + + std::to_string(static_cast(::getpid())) + ".sock"; + std::remove(socket_path.c_str()); + const std::string address = "unix:" + socket_path; + builder.AddListeningPort( + address, + grpc::InsecureServerCredentials()); + builder.RegisterService(service_.get()); + std::vector> factories; + factories.emplace_back(makeGrpcErrorLoggingInterceptorFactory( + [&records, &records_mutex, &records_changed]( + const GrpcFailureRecord& record) { + { + std::lock_guard lock(records_mutex); + records.push_back(record); + } + records_changed.notify_all(); + })); + builder.experimental().SetInterceptorCreators(std::move(factories)); + auto server = builder.BuildAndStart(); + ASSERT_NE(server, nullptr); + + const auto channel = grpc::CreateChannel( + address, + grpc::InsecureChannelCredentials()); + auto stub = api::ArmService::NewStub(channel); + grpc::ClientContext client_context; + client_context.set_deadline( + std::chrono::system_clock::now() + std::chrono::seconds(3)); + api::CommandHeader_Request request; + request.set_device_id("aubo_arm"); + api::CommandHeader_Feedback response; + grpc::Status client_status; + + aubo_arm_->blockNextTorqueOn(); + aubo_arm_->failNextTorqueOnCancellation(); + std::thread client_call([&]() { + client_status = stub->torqueOn( + &client_context, request, &response); + }); + const bool torque_on_started = aubo_arm_->waitForBlockingTorqueOn( + std::chrono::seconds(2)); + client_context.TryCancel(); + client_call.join(); + + bool failure_recorded = false; + { + std::unique_lock lock(records_mutex); + failure_recorded = records_changed.wait_for( + lock, + std::chrono::seconds(2), + [&records]() { return !records.empty(); }); + } + if (!failure_recorded) { + aubo_arm_->releaseBlockingTorqueOn(); + } + server->Shutdown(); + server->Wait(); + std::remove(socket_path.c_str()); + + ASSERT_TRUE(torque_on_started); + EXPECT_EQ(client_status.error_code(), grpc::StatusCode::CANCELLED); + ASSERT_TRUE(failure_recorded); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records.front().kind, GrpcFailureKind::GRPC_STATUS); + EXPECT_EQ(records.front().status_code, grpc::StatusCode::INTERNAL); + EXPECT_EQ(records.front().severity, GrpcFailureSeverity::ERROR); + EXPECT_EQ( + records.front().detail, + "simulated torqueOn safety termination failure"); + EXPECT_TRUE(aubo_arm_->lastTorqueOnCancellationRequested()); +} + TEST_F(GrpcArmServiceTest, StopAllGateRejectsMutatingCommandsButAllowsReadsAndStops) { diff --git a/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp index 2c43afca..ab7adbff 100644 --- a/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp @@ -3024,6 +3024,190 @@ TEST_F(GrpcSystemServiceTest, EXPECT_EQ(active_arm->motionCalls(), 1); } +TEST_F(GrpcSystemServiceTest, + StopAllSharesTimedOutArmStopAndDetailAcrossServiceInstances) +{ + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + auto trace = std::make_shared(); + auto arm = std::make_shared( + "preempted-handler-arm", trace); + manager.registerDevice(arm); + + auto& authority = control::ControlAuthorityManager::instance(); + const auto old_handler = authority.tryAcquire( + arm->id(), "blocked-control-handler", std::chrono::seconds(30)); + ASSERT_TRUE(old_handler.acquired) << old_handler.detail; + service_ = std::make_unique( + std::chrono::milliseconds(300)); + + api::StopAllCommand_Request request; + auto stop_all = std::async(std::launch::async, [this, &request] { + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll( + &context, &request, &response); + return std::make_pair(status, response); + }); + + const bool initial_stop_completed = arm->waitForStopMotionCalls( + 1, std::chrono::milliseconds(500)); + if (initial_stop_completed) { + arm->blockNextStopMotion(); + } + const auto completion = stop_all.wait_for(std::chrono::seconds(1)); + const bool final_stop_started = initial_stop_completed && + arm->waitForBlockedStopMotion(std::chrono::milliseconds(500)); + + std::optional> + result; + std::optional> + second_result; + std::unique_ptr second_service; + std::promise last_destroy_started; + auto last_destroy_started_signal = last_destroy_started.get_future(); + std::promise replacement_construct_started; + auto replacement_construct_started_signal = + replacement_construct_started.get_future(); + std::future first_service_destroy; + std::future last_service_destroy; + std::future> + replacement_service_construct; + std::optional first_destroy_before_release; + std::optional last_destroy_before_release; + std::optional last_destroy_after_release; + std::optional replacement_before_release; + std::optional replacement_after_release; + std::unique_ptr replacement_service; + bool dispatcher_destruction_observed{false}; + int stop_calls_before_second{-1}; + int stop_calls_after_second{-1}; + control::ControlAcquireResult control_while_failed_closed; + if (completion == std::future_status::ready) { + result = stop_all.get(); + stop_calls_before_second = arm->stopMotionCalls(); + second_service = std::make_unique( + std::chrono::milliseconds(300)); + api::StopAllCommand_Feedback second_response; + grpc::ServerContext second_context; + const auto second_status = second_service->StopAll( + &second_context, &request, &second_response); + second_result.emplace(second_status, std::move(second_response)); + stop_calls_after_second = arm->stopMotionCalls(); + control_while_failed_closed = authority.tryAcquire( + arm->id(), "control-after-timed-out-stop-all", + std::chrono::seconds(30)); + first_service_destroy = std::async( + std::launch::async, + [this] { service_.reset(); }); + first_destroy_before_release = + first_service_destroy.wait_for(std::chrono::seconds(1)); + if (*first_destroy_before_release == std::future_status::ready) { + first_service_destroy.get(); + last_service_destroy = std::async( + std::launch::async, + [&second_service, &last_destroy_started] { + last_destroy_started.set_value(); + second_service.reset(); + }); + last_destroy_started_signal.wait(); + dispatcher_destruction_observed = + gRPCSystemServiceImpl:: + waitForStopDispatcherDestructionForTesting( + std::chrono::seconds(1)); + if (dispatcher_destruction_observed) { + last_destroy_before_release = + last_service_destroy.wait_for( + std::chrono::milliseconds::zero()); + replacement_service_construct = std::async( + std::launch::async, + [&replacement_construct_started] { + replacement_construct_started.set_value(); + return std::make_unique( + std::chrono::milliseconds(300)); + }); + replacement_construct_started_signal.wait(); + replacement_before_release = + replacement_service_construct.wait_for( + std::chrono::milliseconds(100)); + } + } + } + + // Always release both test blocks before an assertion can abort the test; + // the dispatcher owns the final stop worker past the RPC deadline. + arm->releaseBlockedStopMotion(); + authority.release(old_handler.token); + if (first_service_destroy.valid()) { + if (first_service_destroy.wait_for(std::chrono::seconds(1)) == + std::future_status::ready) { + first_service_destroy.get(); + } + } + if (last_service_destroy.valid()) { + last_destroy_after_release = + last_service_destroy.wait_for(std::chrono::seconds(1)); + if (*last_destroy_after_release == std::future_status::ready) { + last_service_destroy.get(); + } + } + if (replacement_service_construct.valid()) { + replacement_after_release = + replacement_service_construct.wait_for(std::chrono::seconds(1)); + if (*replacement_after_release == std::future_status::ready) { + replacement_service = replacement_service_construct.get(); + } + } + replacement_service.reset(); + + EXPECT_TRUE(initial_stop_completed); + EXPECT_TRUE(final_stop_started); + ASSERT_EQ(completion, std::future_status::ready); + ASSERT_TRUE(result.has_value()); + ASSERT_TRUE(second_result.has_value()); + const auto& [status, response] = *result; + const auto& [second_status, second_response] = *second_result; + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_FALSE(response.header().success()); + EXPECT_NE( + response.header().error_message().find( + "timed out waiting for the preempted RobotArm control handler " + "to exit"), + std::string::npos); + EXPECT_EQ( + response.header().error_message().find( + "stop operation did not complete before the deadline"), + std::string::npos); + ASSERT_TRUE(second_status.ok()) << second_status.error_message(); + EXPECT_FALSE(second_response.header().success()); + EXPECT_NE( + second_response.header().error_message().find( + "timed out waiting for the preempted RobotArm control handler " + "to exit"), + std::string::npos); + EXPECT_EQ( + second_response.header().error_message().find( + "stop operation did not complete before the deadline"), + std::string::npos); + EXPECT_EQ(stop_calls_before_second, 2); + EXPECT_EQ(stop_calls_after_second, stop_calls_before_second); + EXPECT_FALSE(control_while_failed_closed.acquired); + ASSERT_TRUE(first_destroy_before_release.has_value()); + EXPECT_EQ( + *first_destroy_before_release, std::future_status::ready); + EXPECT_TRUE(dispatcher_destruction_observed); + ASSERT_TRUE(last_destroy_before_release.has_value()); + ASSERT_TRUE(last_destroy_after_release.has_value()); + ASSERT_TRUE(replacement_before_release.has_value()); + ASSERT_TRUE(replacement_after_release.has_value()); + EXPECT_EQ( + *last_destroy_before_release, std::future_status::timeout); + EXPECT_EQ(*last_destroy_after_release, std::future_status::ready); + EXPECT_EQ( + *replacement_before_release, std::future_status::timeout); + EXPECT_EQ(*replacement_after_release, std::future_status::ready); +} + TEST_F(GrpcSystemServiceTest, StopAllDoesNotStopDeviceLifecyclesAndRevokesOnlyArmLease) { diff --git a/cmvr-es/service/stop_all/include/stop_operation_dispatcher.h b/cmvr-es/service/stop_all/include/stop_operation_dispatcher.h index 1dc0b8c4..60c25804 100644 --- a/cmvr-es/service/stop_all/include/stop_operation_dispatcher.h +++ b/cmvr-es/service/stop_all/include/stop_operation_dispatcher.h @@ -36,6 +36,7 @@ public: }; using Operation = std::function; + using TimeoutDetailProvider = std::function; class Handle final { public: @@ -62,8 +63,15 @@ public: // A running job is shared by every submission for the same resource key. // Once it has completed, the next submission joins/reaps the old worker - // and starts a new job. Empty keys or operations return an invalid handle. + // and starts a new job. A timeout provider belongs to the job that is + // actually created; submissions joining a running job retain that job's + // provider. Multiple handles may call the provider concurrently, so it + // must be thread-safe. Empty keys or operations return an invalid handle. Handle submit(std::string resource_key, Operation operation); + Handle submit( + std::string resource_key, + Operation operation, + TimeoutDetailProvider timeout_detail_provider); // Exposed only to verify dispatcher lifecycle behavior in tests. std::size_t jobCountForTesting() const; diff --git a/cmvr-es/service/stop_all/src/stop_operation_dispatcher.cpp b/cmvr-es/service/stop_all/src/stop_operation_dispatcher.cpp index 351d5c9f..f53b68c2 100644 --- a/cmvr-es/service/stop_all/src/stop_operation_dispatcher.cpp +++ b/cmvr-es/service/stop_all/src/stop_operation_dispatcher.cpp @@ -12,10 +12,16 @@ namespace cmvr::service { struct StopOperationDispatcher::JobState final { + explicit JobState(TimeoutDetailProvider provider) + : timeout_detail_provider(std::move(provider)) + { + } + std::mutex mutex; std::condition_variable condition; bool completed{false}; OperationResult outcome; + const TimeoutDetailProvider timeout_detail_provider; }; struct StopOperationDispatcher::Impl final { @@ -50,9 +56,18 @@ StopOperationDispatcher::Handle::waitUntil(const Deadline deadline) const if (!state_->condition.wait_until(lock, deadline, [this] { return state_->completed; })) { - return { - false, - false, + lock.unlock(); + if (state_->timeout_detail_provider) { + try { + auto detail = state_->timeout_detail_provider(); + if (!detail.empty()) { + return {false, false, std::move(detail)}; + } + } catch (...) { + // Diagnostics must not change timeout behavior. + } + } + return {false, false, "stop operation did not complete before the deadline"}; } @@ -85,6 +100,14 @@ StopOperationDispatcher::~StopOperationDispatcher() StopOperationDispatcher::Handle StopOperationDispatcher::submit( std::string resource_key, Operation operation) +{ + return submit(std::move(resource_key), std::move(operation), {}); +} + +StopOperationDispatcher::Handle StopOperationDispatcher::submit( + std::string resource_key, + Operation operation, + TimeoutDetailProvider timeout_detail_provider) { if (resource_key.empty() || !operation) { return {}; @@ -141,7 +164,8 @@ StopOperationDispatcher::Handle StopOperationDispatcher::submit( continue; } - auto state = std::make_shared(); + auto state = std::make_shared( + std::move(timeout_detail_provider)); const auto inserted = impl_->jobs.emplace( std::piecewise_construct, std::forward_as_tuple(std::move(resource_key)), diff --git a/cmvr-es/service/stop_all/tests/stop_operation_dispatcher_test.cpp b/cmvr-es/service/stop_all/tests/stop_operation_dispatcher_test.cpp index 4f469dd7..aabb337b 100644 --- a/cmvr-es/service/stop_all/tests/stop_operation_dispatcher_test.cpp +++ b/cmvr-es/service/stop_all/tests/stop_operation_dispatcher_test.cpp @@ -78,6 +78,88 @@ TEST(StopOperationDispatcherTest, TimeoutDoesNotCancelTheJob) EXPECT_EQ(completed.detail, "stopped"); } +TEST(StopOperationDispatcherTest, + RunningJobKeepsItsOriginalTimeoutDetailProvider) +{ + StopOperationDispatcher dispatcher; + std::promise release; + auto released = release.get_future().share(); + std::atomic first_operation_calls{0}; + std::atomic duplicate_operation_calls{0}; + std::atomic first_provider_calls{0}; + std::atomic duplicate_provider_calls{0}; + + const auto first = dispatcher.submit( + "arm:one", + [&] { + ++first_operation_calls; + released.wait(); + return StopOperationDispatcher::OperationResult{true, {}}; + }, + [&] { + ++first_provider_calls; + return std::string("first job is waiting for its handler"); + }); + const auto duplicate = dispatcher.submit( + "arm:one", + [&] { + ++duplicate_operation_calls; + return StopOperationDispatcher::OperationResult{true, {}}; + }, + [&] { + ++duplicate_provider_calls; + return std::string("duplicate submission detail"); + }); + + const auto first_timeout = first.waitUntil( + StopOperationDispatcher::Clock::now() + 20ms); + const auto duplicate_timeout = duplicate.waitUntil( + StopOperationDispatcher::Clock::now() + 20ms); + release.set_value(); + const auto completed = first.waitUntil( + StopOperationDispatcher::Clock::now() + 1s); + + EXPECT_FALSE(first_timeout.completed); + EXPECT_EQ( + first_timeout.detail, "first job is waiting for its handler"); + EXPECT_FALSE(duplicate_timeout.completed); + EXPECT_EQ( + duplicate_timeout.detail, "first job is waiting for its handler"); + EXPECT_TRUE(completed.completed); + EXPECT_TRUE(completed.result); + EXPECT_EQ(first_operation_calls.load(), 1); + EXPECT_EQ(duplicate_operation_calls.load(), 0); + EXPECT_EQ(first_provider_calls.load(), 2); + EXPECT_EQ(duplicate_provider_calls.load(), 0); +} + +TEST(StopOperationDispatcherTest, + ThrowingTimeoutDetailProviderFallsBackToGenericDetail) +{ + StopOperationDispatcher dispatcher; + std::promise release; + auto released = release.get_future().share(); + const auto handle = dispatcher.submit( + "arm:one", + [released] { + released.wait(); + return StopOperationDispatcher::OperationResult{true, {}}; + }, + []() -> std::string { + throw std::runtime_error("diagnostic provider failed"); + }); + + const auto timed_out = handle.waitUntil( + StopOperationDispatcher::Clock::now() + 20ms); + release.set_value(); + + EXPECT_FALSE(timed_out.completed); + EXPECT_FALSE(timed_out.result); + EXPECT_EQ( + timed_out.detail, + "stop operation did not complete before the deadline"); +} + TEST(StopOperationDispatcherTest, RunningSubmissionsForAKeyShareOneJob) { StopOperationDispatcher dispatcher;