From 912d8689f7f7564d75a57da86093e57069f9d9ba Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Wed, 12 Aug 2026 11:15:46 +0800 Subject: [PATCH] feat(system): add serial arm and AGV action queue --- cmvr-es/common/types/arm/arm_types.h | 6 + cmvr-es/devices/agv/abstract_agv.h | 29 + .../seer_robokit/include/seer_robokit_agv.h | 15 +- .../src/seer_robokit_navigation_wait.cpp | 83 +- .../seer_robokit_control_authority_test.cpp | 138 ++ cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp | 70 +- cmvr-es/devices/arm/aubo_arm/aubo_arm.h | 1 + cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp | 63 +- cmvr-es/devices/arm/huayan_arm/huayan_arm.h | 5 +- .../huayan_arm/tests/huayan_arm_sdk_test.cpp | 42 +- cmvr-es/devices/arm/robot_arm.h | 6 + .../include/control_authority_manager.h | 16 + .../src/control_authority_manager.cpp | 126 +- .../tests/control_authority_manager_test.cpp | 133 + cmvr-es/service/CMakeLists.txt | 1 + cmvr-es/service/README.md | 51 + .../action/include/action_queue_executor.h | 65 + .../action/src/action_queue_executor.cpp | 2204 +++++++++++++++++ .../grpc/include/grpc_system_service.h | 12 +- cmvr-es/service/grpc/src/grpc_agv_service.cpp | 251 +- cmvr-es/service/grpc/src/grpc_arm_service.cpp | 13 +- .../service/grpc/src/grpc_system_service.cpp | 223 +- .../grpc/tests/grpc_agv_service_test.cpp | 234 ++ .../grpc/tests/grpc_arm_service_test.cpp | 71 + .../grpc/tests/grpc_system_service_test.cpp | 1437 +++++++++++ .../grpc_server_task/src/grpc_server_task.cpp | 5 + protos/cmvr/api/system_command.proto | 100 + protos/cmvr/api/system_service.proto | 2 + 28 files changed, 5323 insertions(+), 79 deletions(-) create mode 100644 cmvr-es/service/action/include/action_queue_executor.h create mode 100644 cmvr-es/service/action/src/action_queue_executor.cpp diff --git a/cmvr-es/common/types/arm/arm_types.h b/cmvr-es/common/types/arm/arm_types.h index 9d8c819b..60e602f3 100644 --- a/cmvr-es/common/types/arm/arm_types.h +++ b/cmvr-es/common/types/arm/arm_types.h @@ -2,6 +2,7 @@ #define CMVR_ES_ARM_TYPES_H #include +#include #include #include @@ -162,6 +163,11 @@ struct MotionOptions { double jerk{5.0}; std::vector joint_velocity_limits; bool asynchronous{false}; + // Framework-independent cancellation check used by queued synchronous + // motion. Cancellation after device acceptance retains a typed motion + // barrier; the owner must call stopMotion() to confirm physical idle. + // Drivers must not retain this callback after moveJ/moveL returns. + std::function cancellation_requested; }; struct ServoOptions { diff --git a/cmvr-es/devices/agv/abstract_agv.h b/cmvr-es/devices/agv/abstract_agv.h index 40ec6cb9..8f436a21 100644 --- a/cmvr-es/devices/agv/abstract_agv.h +++ b/cmvr-es/devices/agv/abstract_agv.h @@ -15,6 +15,12 @@ namespace cmvr::device { +enum class AgvActionKind { + NavigateToPose, + NavigateToStation, + FollowPath, +}; + /** * @brief AGV/移动底盘设备抽象基类。 * @@ -28,6 +34,16 @@ public: DeviceKind kind() const noexcept override { return DeviceKind::AGV; } + /** + * @brief Whether this backend provides terminal-state and stopped-motion + * confirmation plus bounded cancellation suitable for synchronous + * Action execution. + */ + virtual bool supportsSynchronousAction(AgvActionKind) const noexcept + { + return false; + } + /** * @brief 获取 AGV 运行状态快照。 */ @@ -167,6 +183,19 @@ public: return setVelocity(AgvVelocity{}); } + /** + * @brief 确认 AGV 已进入可安全释放控制权的停止状态。 + * + * 该接口只在导航任务已终止且底盘速度经过连续采样确认为零后返回 + * 成功；仅收到取消、停止或零速度命令的应答不构成成功。 + */ + virtual AgvResult confirmMotionStopped() + { + return AgvResult::failure( + AgvErrorCode::UnsupportedCommand, + "confirmMotionStopped not implemented"); + } + /** * @brief 查询 AGV 可用地图名称列表。 */ diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h index 5a8ec3a1..f06510f2 100644 --- a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h +++ b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h @@ -27,6 +27,17 @@ public: std::string typeName() const override { return "SeerRobokitAgv"; } + bool supportsSynchronousAction(AgvActionKind kind) const noexcept override + { + switch (kind) { + case AgvActionKind::NavigateToPose: + case AgvActionKind::NavigateToStation: + case AgvActionKind::FollowPath: + return true; + } + return false; + } + bool init() override; bool start() override; bool stop() override; @@ -57,6 +68,7 @@ public: AgvResult cancelNavigation() override; AgvResult setVelocity(const AgvVelocity& velocity) override; + AgvResult confirmMotionStopped() override; AgvResult listMaps(std::vector& maps) const override; AgvResult listStations(std::vector& stations) const override; @@ -166,7 +178,8 @@ private: AgvResult waitForCanceledTaskToStop_( const TrackedNavigationContext& context, const AgvMotionOptions& options, - const std::string& reason); + const std::string& reason, + bool require_global_stopped = false); AgvResult failAndCancelTrackedNavigation_( const TrackedNavigationContext& context, const AgvMotionOptions& options, diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp index fc9cf13f..62aaf35c 100644 --- a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp +++ b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp @@ -473,10 +473,71 @@ AgvResult SeerRobokitAgv::cancelTrackedNavigation_( return result; } +AgvResult SeerRobokitAgv::confirmMotionStopped() +{ + TrackedNavigationContext tracked_navigation; + if (currentTrackedNavigation_(tracked_navigation)) { + return waitForCanceledTaskToStop_( + tracked_navigation, + AgvMotionOptions{}, + "explicit motion-stop confirmation", + true); + } + + const auto confirmation_window = + kNavigationCancelConfirmationTimeout; + const auto deadline = + std::chrono::steady_clock::now() + confirmation_window; + int stopped_samples = 0; + std::string last_detail = "no 1101 status received"; + while (std::chrono::steady_clock::now() < deadline) { + NavigationSnapshot snapshot; + const auto snapshot_result = queryNavigationSnapshot_(snapshot); + if (!snapshot_result.ok()) { + return AgvResult::failure( + snapshot_result.code, + "SEER Robokit stopped state could not be confirmed with 1101: " + + snapshot_result.message); + } + + last_detail = snapshot.detail; + const bool no_active_navigation = + snapshot.task_status_present && + globalTaskStateIsKnownTerminal(snapshot.task_status); + if (no_active_navigation && navigationStopped(snapshot)) { + ++stopped_samples; + if (stopped_samples >= kRequiredCompletedStopSamples) { + return { + AgvErrorCode::OK, + "no active navigation and two zero-velocity samples were " + "confirmed with 1101"}; + } + } else { + stopped_samples = 0; + } + + const auto now = std::chrono::steady_clock::now(); + if (now < deadline) { + std::this_thread::sleep_for(std::min( + kNavigationCancelPollInterval, + std::chrono::duration_cast( + deadline - now))); + } + } + + return AgvResult::failure( + AgvErrorCode::Timeout, + "SEER Robokit did not confirm an inactive navigation task and two " + "zero-velocity samples within " + + std::to_string(confirmation_window.count()) + + " ms; last_status=" + last_detail); +} + AgvResult SeerRobokitAgv::waitForCanceledTaskToStop_( const TrackedNavigationContext& context, const AgvMotionOptions& options, - const std::string& reason) + const std::string& reason, + const bool require_global_stopped) { if (context.task_ids.empty()) { return AgvResult::failure( @@ -525,7 +586,8 @@ AgvResult SeerRobokitAgv::waitForCanceledTaskToStop_( const bool another_local_navigation_started = currentTrackedNavigation_(active_context) && active_context.token != context.token; - if (all_exact_tasks_terminal && another_local_navigation_started) { + if (all_exact_tasks_terminal && another_local_navigation_started && + !require_global_stopped) { // A later navigation is allowed to move after this exact task has // reached a terminal state. Its velocity must not keep the older // waiter alive or make it cancel the newer task. @@ -567,14 +629,17 @@ AgvResult SeerRobokitAgv::waitForCanceledTaskToStop_( context.target_ids.end(), snapshot.target_id) != context.target_ids.end(); - const bool global_terminal = snapshot.task_status == 0 - || (exactTaskStateIsKnownTerminal(snapshot.task_status) - && snapshot.task_type == expected_global_type - && global_target_matches); + const bool global_terminal = snapshot.task_status_present && + (snapshot.task_status == 0 + || (exactTaskStateIsKnownTerminal(snapshot.task_status) + && snapshot.task_type == expected_global_type + && global_target_matches)); + const bool task_termination_confirmed = require_global_stopped + ? (!any_exact_task_active && global_terminal) + : (all_exact_tasks_terminal + || (!any_exact_task_active && global_terminal)); - if ((all_exact_tasks_terminal - || (!any_exact_task_active && global_terminal)) - && navigationStopped(snapshot)) { + if (task_termination_confirmed && navigationStopped(snapshot)) { ++stopped_samples; if (stopped_samples >= kRequiredCompletedStopSamples) { for (const auto& task_id : context.task_ids) { diff --git a/cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp b/cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp index 905b9105..b1109f12 100644 --- a/cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp +++ b/cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp @@ -605,6 +605,144 @@ protected: std::unique_ptr agv_; }; +TEST_F(SeerRobokitControlAuthorityTest, DeclaresSynchronousActionSupport) +{ + EXPECT_TRUE(agv_->supportsSynchronousAction( + AgvActionKind::NavigateToPose)); + EXPECT_TRUE(agv_->supportsSynchronousAction( + AgvActionKind::NavigateToStation)); + EXPECT_TRUE(agv_->supportsSynchronousAction( + AgvActionKind::FollowPath)); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + ConfirmMotionStoppedWithoutTrackedTaskUsesGlobalStatusOnly) +{ + controller_.clearRecords(); + + const auto result = agv_->confirmMotionStopped(); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusAll2; + }), + 2); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }), + 0); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + ConfirmMotionStoppedForTrackedStationChecksExactTaskAndGlobalVelocity) +{ + const auto navigate_result = agv_->navigateToStation( + "station-confirm-stop", + asynchronousMotionOptions()); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":2,"target_id":"station-confirm-stop","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.clearRecords(); + + const auto result = agv_->confirmMotionStopped(); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + EXPECT_GE( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }), + 2); + EXPECT_GE( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusAll2; + }), + 2); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + ConfirmMotionStoppedTimesOutWhenTrackedTaskIsTerminalButVelocityIsNonzero) +{ + const auto navigate_result = agv_->navigateToStation( + "station-still-moving", + asynchronousMotionOptions()); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":2,"target_id":"station-still-moving","blocked":false,"vx":0.1,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.clearRecords(); + + const auto result = agv_->confirmMotionStopped(); + + EXPECT_EQ(result.code, AgvErrorCode::Timeout); + EXPECT_NE( + result.message.find("stopped velocity were not confirmed"), + std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + ConfirmMotionStoppedWaitsForGlobalTaskToBecomeTerminal) +{ + const auto navigate_result = agv_->navigateToStation( + "station-global-active", + asynchronousMotionOptions()); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":2,"target_id":"station-global-active","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.clearRecords(); + std::atomic finished{false}; + AgvResult result; + + std::thread confirmation([this, &finished, &result]() { + result = agv_->confirmMotionStopped(); + finished.store(true, std::memory_order_release); + }); + const bool zero_samples_observed = waitForCommandCount( + kRobotStatusAll2, 2, 1000); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + const bool returned_while_global_active = + finished.load(std::memory_order_acquire); + + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":2,"target_id":"station-global-active","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + confirmation.join(); + + EXPECT_TRUE(zero_samples_observed); + EXPECT_FALSE(returned_while_global_active); + EXPECT_TRUE(result.ok()) << result.message; +} + TEST_F(SeerRobokitControlAuthorityTest, EveryImplementedMutatingOperationAcquiresAuthorityFirst) { controller_.clearRecords(); diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index ed446d4f..2842c9af 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -23,6 +23,21 @@ namespace cmvr::device { namespace { +bool cancellationRequested( + const std::function& cancellation_requested) noexcept +{ + if (!cancellation_requested) { + return false; + } + try { + return cancellation_requested(); + } catch (...) { + // A broken cancellation source must never permit a queued motion to be + // submitted after its ownership can no longer be established. + return true; + } +} + class MotionOwnerGuard final { public: MotionOwnerGuard( @@ -1818,11 +1833,12 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o auto motion_control = robot_interface->getMotionControl(); motion_control->setSpeedFraction(speed_scaling_); motion_owner.requireExplicitSettlement(); - if (!validateSafetyPermit(safety_monitor, safety_permit)) { + if (cancellationRequested(options.cancellation_requested) || + !validateSafetyPermit(safety_monitor, safety_permit)) { motion_owner.settle(); return Result::failure( ArmErrorCode::CommandRejected, - "[AuboArm] moveJ cancelled by hardware safety before submission"); + "[AuboArm] moveJ cancelled before submission"); } const int ret = motion_control->moveJoint( target.position, @@ -1843,18 +1859,39 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o motion_state, safety_monitor, safety_permit, + cancellation_requested = options.cancellation_requested, token = motion.token]() { return waitArrival( robot_interface, [motion_state, safety_monitor, safety_permit, + cancellation_requested, token]() { - return motion_state->cancelled(token) || + return cancellationRequested( + cancellation_requested) || + motion_state->cancelled(token) || !validateSafetyPermit( safety_monitor, safety_permit); }); }); + if (cancellationRequested(options.cancellation_requested)) { + if (ret == arcs::common_interface::AUBO_OK) { + if (outcome == + aubo_internal::MotionCommandOutcome::CompletedAfterMotion) { + motion_owner.clearOnFinish(); + } else { + // The request was accepted but its completion is no longer + // owned by this caller. Preserve the typed motion state so + // the cancellation owner can issue stopJoint/stopLine. + motion_owner.retainKind(); + } + } + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveJ cancelled by its caller"); + } if (!validateSafetyPermit(safety_monitor, safety_permit)) { motion_owner.settle(); return Result::failure( @@ -2019,11 +2056,12 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); std::vector pose{target.x, target.y, target.z, target.rx, target.ry, target.rz}; motion_owner.requireExplicitSettlement(); - if (!validateSafetyPermit(safety_monitor, safety_permit)) { + if (cancellationRequested(options.cancellation_requested) || + !validateSafetyPermit(safety_monitor, safety_permit)) { motion_owner.settle(); return Result::failure( ArmErrorCode::CommandRejected, - "[AuboArm] moveL cancelled by hardware safety before submission"); + "[AuboArm] moveL cancelled before submission"); } const int ret = motion_control->moveLine( pose, @@ -2044,18 +2082,38 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, motion_state, safety_monitor, safety_permit, + cancellation_requested = options.cancellation_requested, token = motion.token]() { return waitArrival( robot_interface, [motion_state, safety_monitor, safety_permit, + cancellation_requested, token]() { - return motion_state->cancelled(token) || + return cancellationRequested( + cancellation_requested) || + motion_state->cancelled(token) || !validateSafetyPermit( safety_monitor, safety_permit); }); }); + if (cancellationRequested(options.cancellation_requested)) { + if (ret == arcs::common_interface::AUBO_OK) { + if (outcome == + aubo_internal::MotionCommandOutcome::CompletedAfterMotion) { + motion_owner.clearOnFinish(); + } else { + // Keep the accepted linear kind until a typed Stop confirms + // that the controller is idle. + motion_owner.retainKind(); + } + } + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveL cancelled by its caller"); + } if (!validateSafetyPermit(safety_monitor, safety_permit)) { motion_owner.settle(); return Result::failure( diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h index e708c748..4f22ddfe 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h @@ -33,6 +33,7 @@ public: RobotMode getRobotMode() const override; SafetyMode getSafetyMode() const override; ControlMode getControlMode() const override; + bool supportsActionQueueMotion() const noexcept override { return true; } Result torqueOn() override; Result torqueOff() override; diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp index dbc2093a..90c0e627 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp @@ -34,6 +34,21 @@ constexpr auto kControllerStopTimeout = std::chrono::milliseconds(3000); constexpr auto kOwnerExitTimeout = std::chrono::milliseconds(3000); constexpr auto kCompletionCorrelationGrace = std::chrono::milliseconds(250); +bool cancellationRequested( + const std::function& cancellation_requested) noexcept +{ + if (!cancellation_requested) { + return false; + } + try { + return cancellation_requested(); + } catch (...) { + // Cancellation sources are part of the motion-admission safety gate. + // Treat an exception as cancellation instead of admitting new motion. + return true; + } +} + double radToDeg(const double value) { return value * 180.0 / kPi; @@ -662,11 +677,12 @@ Result HuayanRobot::moveJ( if (targetReached_(&target.position, nullptr) && controllerIdleStable_(runtime, std::chrono::milliseconds(300))) { - if (!runtime->safety.validate(permit)) { + if (cancellationRequested(options.cancellation_requested) || + !runtime->safety.validate(permit)) { runtime->motion.finish(start.token, MotionFinishMode::Clear); return Result::failure( ArmErrorCode::CommandRejected, - "[HuayanRobot] moveJ cancelled by a safety transition"); + "[HuayanRobot] moveJ cancelled by its caller or a safety transition"); } runtime->motion.finish(start.token, MotionFinishMode::Clear); return Result::success(); @@ -687,18 +703,20 @@ Result HuayanRobot::moveJ( const double blend = metersToMm(options.blend_radius); const auto command_id = nextCommandId_(); - if (!runtime->safety.validate(permit)) { + if (cancellationRequested(options.cancellation_requested) || + !runtime->safety.validate(permit)) { runtime->motion.finish(start.token, MotionFinishMode::Clear); return Result::failure( ArmErrorCode::CommandRejected, - "[HuayanRobot] moveJ cancelled before submission by a safety transition"); + "[HuayanRobot] moveJ cancelled before submission"); } int ret = 0; { std::lock_guard submission_lock( runtime->submission_mutex); std::lock_guard sdk_lock(sdk_mutex_); - if (runtime->motion.cancelled(start.token) || + if (cancellationRequested(options.cancellation_requested) || + runtime->motion.cancelled(start.token) || !runtime->safety.validate(permit)) { runtime->motion.finish(start.token, MotionFinishMode::Clear); return Result::failure( @@ -719,7 +737,8 @@ Result HuayanRobot::moveJ( admission_lock.unlock(); return waitMotionDone_( "moveJ", runtime, start.token, permit, command_id, - &target.position, nullptr, 60000); + &target.position, nullptr, 60000, + options.cancellation_requested); } Result HuayanRobot::speedJ( @@ -837,11 +856,12 @@ Result HuayanRobot::moveL( if (targetReached_(nullptr, &target) && controllerIdleStable_(runtime, std::chrono::milliseconds(300))) { - if (!runtime->safety.validate(permit)) { + if (cancellationRequested(options.cancellation_requested) || + !runtime->safety.validate(permit)) { runtime->motion.finish(start.token, MotionFinishMode::Clear); return Result::failure( ArmErrorCode::CommandRejected, - "[HuayanRobot] moveL cancelled by a safety transition"); + "[HuayanRobot] moveL cancelled by its caller or a safety transition"); } runtime->motion.finish(start.token, MotionFinishMode::Clear); return Result::success(); @@ -868,18 +888,20 @@ Result HuayanRobot::moveL( const double blend = metersToMm(options.blend_radius); const auto command_id = nextCommandId_(); - if (!runtime->safety.validate(permit)) { + if (cancellationRequested(options.cancellation_requested) || + !runtime->safety.validate(permit)) { runtime->motion.finish(start.token, MotionFinishMode::Clear); return Result::failure( ArmErrorCode::CommandRejected, - "[HuayanRobot] moveL cancelled before submission by a safety transition"); + "[HuayanRobot] moveL cancelled before submission"); } int ret = 0; { std::lock_guard submission_lock( runtime->submission_mutex); std::lock_guard sdk_lock(sdk_mutex_); - if (runtime->motion.cancelled(start.token) || + if (cancellationRequested(options.cancellation_requested) || + runtime->motion.cancelled(start.token) || !runtime->safety.validate(permit)) { runtime->motion.finish(start.token, MotionFinishMode::Clear); return Result::failure( @@ -900,7 +922,8 @@ Result HuayanRobot::moveL( admission_lock.unlock(); return waitMotionDone_( "moveL", runtime, start.token, permit, command_id, - nullptr, &target, 60000); + nullptr, &target, 60000, + options.cancellation_requested); } Result HuayanRobot::speedL( @@ -2279,7 +2302,8 @@ Result HuayanRobot::waitMotionDone_( const std::string& command_id, const std::vector* joint_target, const CartesianPose* tcp_target, - const int timeout_ms) const + const int timeout_ms, + const std::function& cancellation_requested) const { const auto started_at = std::chrono::steady_clock::now(); bool saw_motion = false; @@ -2287,13 +2311,20 @@ Result HuayanRobot::waitMotionDone_( int stable_completion_samples = 0; while (runtime->monitor_running.load()) { - if (runtime->motion.cancelled(motion_token) || + const bool caller_cancelled = + cancellationRequested(cancellation_requested); + if (caller_cancelled || + runtime->motion.cancelled(motion_token) || !runtime->safety.validate(safety_permit)) { - runtime->motion.finish(motion_token, MotionFinishMode::Clear); + runtime->motion.finish( + motion_token, + caller_cancelled + ? MotionFinishMode::Retain + : MotionFinishMode::Clear); return Result::failure( ArmErrorCode::CommandRejected, "[HuayanRobot] " + context + - " cancelled by Stop or a safety transition"); + " cancelled by its caller, Stop, or a safety transition"); } bool done = false; diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h index 010e0339..9e5a684f 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +42,7 @@ public: RobotMode getRobotMode() const override; SafetyMode getSafetyMode() const override; ControlMode getControlMode() const override; + bool supportsActionQueueMotion() const noexcept override { return true; } Result torqueOn() override; Result torqueOff() override; @@ -149,7 +151,8 @@ private: const std::string& command_id, const std::vector* joint_target, const CartesianPose* tcp_target, - int timeout_ms) const; + int timeout_ms, + const std::function& cancellation_requested = {}) const; bool targetReached_( const std::vector* joint_target, const CartesianPose* tcp_target) const; diff --git a/cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp b/cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp index b2569995..00cb8e27 100644 --- a/cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp +++ b/cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp @@ -592,11 +592,51 @@ int main() MotionOptions options; options.velocity = 0.4; options.acceleration = 0.8; + CHECK_TRUE(arm.supportsActionQueueMotion()); + + JointPositionCommand joint_a{{0.10, -0.05, 0.08, 0.0, 0.02, -0.03}}; + MotionOptions cancelled_options = options; + cancelled_options.cancellation_requested = []() { return true; }; + CHECK_TRUE(!arm.moveJ(joint_a, cancelled_options).ok()); + CartesianPose cancelled_pose; + cancelled_pose.x = 0.20; + cancelled_pose.z = 0.30; + CHECK_TRUE(!arm.moveL(cancelled_pose, cancelled_options).ok()); + { + std::lock_guard lock(g_sdk.mutex); + CHECK_TRUE(g_sdk.move_j_calls == 0); + CHECK_TRUE(g_sdk.move_l_calls == 0); + } + + // Once a command has been accepted, caller cancellation returns promptly + // but retains the typed motion barrier until Stop confirms controller idle. + std::atomic cancel_during_wait{false}; + MotionOptions cancellable_options = options; + cancellable_options.cancellation_requested = [&cancel_during_wait]() { + return cancel_during_wait.load(); + }; + JointPositionCommand cancelled_in_wait{ + {0.05, -0.02, 0.04, 0.01, 0.0, -0.01}}; + holdNextMotion(); + auto cancelled_motion = std::async(std::launch::async, [&]() { + return arm.moveJ(cancelled_in_wait, cancellable_options); + }); + CHECK_TRUE(waitUntil([&]() { + std::lock_guard lock(g_sdk.mutex); + return g_sdk.move_j_calls > 0; + })); + cancel_during_wait.store(true); + CHECK_TRUE(cancelled_motion.wait_for(1s) == std::future_status::ready); + if (cancelled_motion.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(!cancelled_motion.get().ok()); + } + CHECK_TRUE(arm.busy()); + CHECK_TRUE(arm.stopMotion().ok()); + CHECK_TRUE(!arm.busy()); // The first IsMotionDone read intentionally reports the preceding idle // state. Completion must be correlated with the command/target. Once the // target is reached, an identical command is an idempotent no-op. - JointPositionCommand joint_a{{0.10, -0.05, 0.08, 0.0, 0.02, -0.03}}; CHECK_TRUE(arm.moveJ(joint_a, options).ok()); int move_j_after_first = 0; { diff --git a/cmvr-es/devices/arm/robot_arm.h b/cmvr-es/devices/arm/robot_arm.h index 071f3ce9..4f63cf4d 100644 --- a/cmvr-es/devices/arm/robot_arm.h +++ b/cmvr-es/devices/arm/robot_arm.h @@ -28,6 +28,12 @@ public: virtual SafetyMode getSafetyMode() const = 0; virtual ControlMode getControlMode() const = 0; + // Queued actions require synchronous completion, cooperative cancellation + // at the final device-submission boundary, and a bounded typed Stop which + // returns success only after controller idle is confirmed. Backends must + // opt in only after all of these semantics have been validated. + virtual bool supportsActionQueueMotion() const noexcept { return false; } + // ArmTeleop requires an explicitly reviewed group-servo implementation. // Existing and vendor arms remain unavailable until their implementations // override this capability after timing and partial-write validation. 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 b3547c2f..bda89ad5 100644 --- a/cmvr-es/manager/control_authority/include/control_authority_manager.h +++ b/cmvr-es/manager/control_authority/include/control_authority_manager.h @@ -49,6 +49,20 @@ public: const std::string& resource_id, const std::string& owner_id, Duration ttl); + + // Converts the expected normal lease into a safety barrier only while it + // is still the current lease. A stale token never preempts a successor or + // joins an existing safety barrier. + ControlAcquireResult preemptAcquireIfCurrent( + const ControlLeaseToken& expected_token, + const std::string& owner_id, + Duration ttl); + + // Permanently blocks the resource only if the expected normal lease is + // still current. Quarantine does not allocate and can only be removed by + // an explicit revoke/clear. + bool quarantineIfCurrent( + const ControlLeaseToken& expected_token) noexcept; bool renew(const ControlLeaseToken& token, Duration ttl); bool validate(const ControlLeaseToken& token); void release(const ControlLeaseToken& token) noexcept; @@ -69,9 +83,11 @@ private: std::uint64_t generation{0}; std::chrono::steady_clock::time_point deadline; bool preemptible{true}; + bool quarantined{false}; std::unordered_map safety_holders; }; + static void quarantine_(Entry& entry) noexcept; bool expired_(const Entry& entry) const noexcept; std::mutex mutex_; 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 53facdd8..00c89730 100644 --- a/cmvr-es/manager/control_authority/src/control_authority_manager.cpp +++ b/cmvr-es/manager/control_authority/src/control_authority_manager.cpp @@ -1,5 +1,6 @@ #include "manager/control_authority/include/control_authority_manager.h" +#include #include namespace cmvr::control { @@ -44,6 +45,7 @@ ControlAcquireResult ControlAuthorityManager::tryAcquire( token.generation, std::chrono::steady_clock::now() + ttl, true, + false, {}}); return {true, std::move(token), {}}; } @@ -71,22 +73,112 @@ ControlAcquireResult ControlAuthorityManager::preemptAcquire( 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{ + const bool has_existing = existing != entries_.end(); + const bool replacing_normal = + has_existing && existing->second.preemptible; + try { + ControlLeaseToken token; + token.resource_id = resource_id; + token.owner_id = owner_id; + token.generation = ++next_generation_; + Entry replacement{ owner_id, token.generation, std::chrono::steady_clock::time_point::max(), false, - {{token.generation, owner_id}}}); - return {true, std::move(token), {}}; + false, + {{token.generation, owner_id}}}; + + if (has_existing) { + static_assert( + std::is_nothrow_move_assignable_v, + "safety barrier replacement must not throw"); + existing->second = std::move(replacement); + } else { + entries_.emplace(resource_id, std::move(replacement)); + } + return {true, std::move(token), {}}; + } catch (...) { + if (replacing_normal && existing->second.preemptible) { + quarantine_(existing->second); + } + throw; + } +} + +ControlAcquireResult ControlAuthorityManager::preemptAcquireIfCurrent( + const ControlLeaseToken& expected_token, + const std::string& owner_id, + const Duration ttl) +{ + if (!expected_token.valid() || owner_id.empty() || + ttl <= Duration::zero()) { + return {false, {}, "invalid conditional control barrier request"}; + } + + std::lock_guard lock(mutex_); + const auto existing = entries_.find(expected_token.resource_id); + if (existing == entries_.end() || + expired_(existing->second) || + !existing->second.preemptible || + existing->second.owner_id != expected_token.owner_id || + existing->second.generation != expected_token.generation) { + return { + false, + {}, + "expected control lease is no longer current"}; + } + + try { + ControlLeaseToken token; + token.resource_id = expected_token.resource_id; + token.owner_id = owner_id; + token.generation = ++next_generation_; + Entry replacement{ + owner_id, + token.generation, + std::chrono::steady_clock::time_point::max(), + false, + false, + {{token.generation, owner_id}}}; + + static_assert( + std::is_nothrow_move_assignable_v, + "safety barrier replacement must not throw"); + existing->second = std::move(replacement); + return {true, std::move(token), {}}; + } catch (...) { + if (existing->second.preemptible) { + quarantine_(existing->second); + } + throw; + } +} + +bool ControlAuthorityManager::quarantineIfCurrent( + const ControlLeaseToken& expected_token) noexcept +{ + if (!expected_token.valid()) { + return false; + } + try { + std::lock_guard lock(mutex_); + const auto existing = + entries_.find(expected_token.resource_id); + if (existing == entries_.end() || + expired_(existing->second) || + !existing->second.preemptible || + existing->second.owner_id != expected_token.owner_id || + existing->second.generation != expected_token.generation) { + return false; + } + quarantine_(existing->second); + return true; + } catch (...) { + return false; + } } bool ControlAuthorityManager::renew( @@ -164,7 +256,8 @@ void ControlAuthorityManager::release( return; } found->second.safety_holders.erase(holder); - if (found->second.safety_holders.empty()) { + if (found->second.safety_holders.empty() && + !found->second.quarantined) { entries_.erase(found); } } else if (found->second.owner_id == token.owner_id && @@ -215,7 +308,16 @@ void ControlAuthorityManager::clear() noexcept bool ControlAuthorityManager::expired_( const Entry& entry) const noexcept { - return std::chrono::steady_clock::now() >= entry.deadline; + return !entry.quarantined && + std::chrono::steady_clock::now() >= entry.deadline; +} + +void ControlAuthorityManager::quarantine_(Entry& entry) noexcept +{ + entry.deadline = + std::chrono::steady_clock::time_point::max(); + entry.preemptible = false; + entry.quarantined = true; } } // namespace cmvr::control diff --git a/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp b/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp index 855d4853..c95cef21 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 @@ -93,6 +93,139 @@ TEST_F(ControlAuthorityManagerTest, EXPECT_FALSE(manager.isLeased("right_arm")); } +TEST_F(ControlAuthorityManagerTest, + ConditionalSafetyBarrierPreemptsMatchingCurrentLease) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 100ms); + ASSERT_TRUE(control.acquired); + + const auto barrier = manager.preemptAcquireIfCurrent( + control.token, "timed-out-action", 100ms); + ASSERT_TRUE(barrier.acquired) << barrier.detail; + EXPECT_FALSE(manager.validate(control.token)); + EXPECT_TRUE(manager.validate(barrier.token)); + manager.release(control.token); + EXPECT_TRUE(manager.validate(barrier.token)); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "new-move", 100ms) + .acquired); + + manager.release(barrier.token); + EXPECT_FALSE(manager.isLeased("right_arm")); +} + +TEST_F(ControlAuthorityManagerTest, + ConditionalSafetyBarrierDoesNotPreemptSuccessorForStaleToken) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto old = + manager.tryAcquire("right_arm", "move-session", 100ms); + ASSERT_TRUE(old.acquired); + const auto direct_stop = manager.preemptAcquire( + "right_arm", "direct-stop", 100ms); + ASSERT_TRUE(direct_stop.acquired) << direct_stop.detail; + manager.release(direct_stop.token); + const auto successor = + manager.tryAcquire("right_arm", "move-session", 100ms); + ASSERT_TRUE(successor.acquired); + ASSERT_NE(old.token.generation, successor.token.generation); + + const auto barrier = manager.preemptAcquireIfCurrent( + old.token, "delayed-stop", 100ms); + EXPECT_FALSE(barrier.acquired); + EXPECT_FALSE(barrier.token.valid()); + EXPECT_TRUE(manager.validate(successor.token)); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "competing-move", 100ms) + .acquired); + + manager.release(successor.token); + EXPECT_FALSE(manager.isLeased("right_arm")); +} + +TEST_F(ControlAuthorityManagerTest, + ConditionalSafetyBarrierDoesNotJoinExistingSafetyBarrier) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 100ms); + ASSERT_TRUE(control.acquired); + const auto existing_barrier = manager.preemptAcquire( + "right_arm", "direct-stop", 100ms); + ASSERT_TRUE(existing_barrier.acquired) << existing_barrier.detail; + + const auto delayed_barrier = manager.preemptAcquireIfCurrent( + control.token, "delayed-action-stop", 100ms); + EXPECT_FALSE(delayed_barrier.acquired); + EXPECT_FALSE(delayed_barrier.token.valid()); + EXPECT_TRUE(manager.validate(existing_barrier.token)); + + manager.release(existing_barrier.token); + EXPECT_FALSE(manager.isLeased("right_arm")); + EXPECT_TRUE( + manager.tryAcquire("right_arm", "new-move", 100ms) + .acquired); +} + +TEST_F(ControlAuthorityManagerTest, + QuarantineSurvivesNormalAndTemporarySafetyTokenRelease) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 20ms); + ASSERT_TRUE(control.acquired); + + ASSERT_TRUE(manager.quarantineIfCurrent(control.token)); + EXPECT_FALSE(manager.validate(control.token)); + EXPECT_TRUE(manager.isLeased("right_arm")); + + manager.release(control.token); + std::this_thread::sleep_for(30ms); + EXPECT_TRUE(manager.isLeased("right_arm")); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "new-move", 100ms) + .acquired); + + const auto temporary_stop = manager.preemptAcquire( + "right_arm", "temporary-stop", 100ms); + ASSERT_TRUE(temporary_stop.acquired) << temporary_stop.detail; + EXPECT_TRUE(manager.validate(temporary_stop.token)); + manager.release(temporary_stop.token); + + EXPECT_TRUE(manager.isLeased("right_arm")); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "new-move", 100ms) + .acquired); + + manager.revoke("right_arm"); + EXPECT_FALSE(manager.isLeased("right_arm")); +} + +TEST_F(ControlAuthorityManagerTest, + QuarantineWithStaleTokenDoesNotAffectSuccessor) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto old = + manager.tryAcquire("right_arm", "move-session", 100ms); + ASSERT_TRUE(old.acquired); + manager.release(old.token); + const auto successor = + manager.tryAcquire("right_arm", "move-session", 100ms); + ASSERT_TRUE(successor.acquired); + ASSERT_NE(old.token.generation, successor.token.generation); + + EXPECT_FALSE(manager.quarantineIfCurrent(old.token)); + EXPECT_TRUE(manager.validate(successor.token)); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "competing-move", 100ms) + .acquired); + + manager.release(successor.token); + EXPECT_FALSE(manager.isLeased("right_arm")); +} + TEST_F(ControlAuthorityManagerTest, ExpiryAndRenewUseMonotonicLocalTime) { auto& manager = ControlAuthorityManager::instance(); diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index f4a39cf3..920a0433 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -1,5 +1,6 @@ add_library(service + action/src/action_queue_executor.cpp grpc/src/grpc_camera_service.cpp grpc/src/grpc_system_service.cpp grpc/src/grpc_speaker_service.cpp diff --git a/cmvr-es/service/README.md b/cmvr-es/service/README.md index 2369d49e..9a78d3e6 100644 --- a/cmvr-es/service/README.md +++ b/cmvr-es/service/README.md @@ -8,6 +8,7 @@ | 目录 | 职责 | | --- | --- | +| `action/` | SystemService ActionQueue 的校验、幂等账本和边缘端 FIFO 执行器 | | `grpc/` | 入站设备控制、状态查询和兼容流式接口 | | `quic_edge/` | 边缘端主动连接平台的 QUIC client、控制状态机和媒体 packetizer | | `quic_edge/tests/` | 已登记到 CTest 的 QUIC 协议测试 | @@ -39,6 +40,56 @@ grpcurl -plaintext \ cmvr.api.SystemService/GetDeviceList ``` +## SystemService ActionQueue + +`SystemService/ExecuteActionQueue` 接收一个完整的有限动作序列，在边缘端排队并 +逐步串行执行，所有步骤结束后返回最终结果。平台只需要提交一次请求，因此连续机械臂 +动作不会再受到每个单独 gRPC 往返和 Wi-Fi 抖动的影响。 + +当前 v1 仅允许以下 `ActionStep.command`： + +- 机械臂同步 `MoveJ`、`MoveL`； +- AGV 同步 `navigateToPose`、`navigateToStation`、`followPath`； +- 边缘端本地 `delay`。 + +机械臂和 AGV 请求复用各自已有的类型化 Request，目标设备仍由每一步的 +`header.device_id` 指定。所有运动步骤必须设置 `asynchronous=false`；`MoveL` v1 仅接受 +Base frame；AGV 后端还必须明确支持同步导航终态确认。`speedJ`、`speedL`、`servoJ`、 +AGV `translate`、速度控制、查询和流式 RPC 都不属于 ActionQueue v1。 + +ActionQueue 遵循以下执行语义： + +- 平台先调用 `GetSystemInfo` 读取 `action_service_instance_id`，并在每次提交和重试中填入 + `expected_service_instance_id`。ActionQueue 账本随服务实例重建；若断线期间边缘服务重启， + 旧实例 ID 会被拒绝，平台必须先对账，不能用新 ID 自动重放不确定的动作； +- `action_id` 是必填的全局唯一幂等键；同一服务实例内，相同内容的已受理请求不会重复下发 + 设备命令，相同 ID 但内容不同的请求必须拒绝。服务端缓存最近 4096 个完整结果，更早的 + 已执行 ID 由精确 retired-ID 账本 fail-closed 拒绝、不会重跑；单实例最多记录 + 262144 个已受理 ID，达到容量后仅拒绝新 ID，已有 ID 仍可查询； +- 入队前校验全部步骤、设备、参数和同步能力，校验失败时不会执行任何步骤； +- v1 每个请求最多 256 步、序列化大小最多 512 KiB、排队或执行中的 Action 最多 64 个、 + 同时提交或等待结果的 RPC 最多 256 个；Action 与单步超时上限均为 24 小时， + `total_timeout_ms=0` 使用 30 分钟默认值，AGV 路径最多 4096 段； +- `total_timeout_ms` 包含排队与执行时间，单步 `timeout_ms=0` 时继承 Action 剩余时间 + 或服务端默认值；所有超时值均由服务端施加上限； +- 任一步失败、取消或超时后立即停止序列，不再执行后续步骤；`completed_steps` 表示此前 + 成功完成的步骤数，`failed_step_index` 仅在存在对应失败步骤时出现； +- Action 一旦受理，不因平台连接中断而自动取消；断线只结束该 RPC waiter，边缘动作继续。 + 平台可用相同 `action_id` 重试并取得仍在缓存中的同一次执行结果； +- `StopAll`、机械臂 `stopMotion`、AGV `cancelNavigation` 和软件急停不进入 FIFO，必须 + 作为高优先级安全/抢占路径执行。它们仍不具备功能安全等级。 +- 机械臂步骤超时会立即走 typed `stopMotion` 并等待停车确认；若无法确认停车，设备控制权 + 保持隔离，不会继续后续步骤或接受新的普通控制命令；需先按设备安全流程确认状态，再 + 重启边缘服务恢复控制。 +- AGV 的取消 ACK、零速度 ACK 均不等于停稳；ActionQueue 和安全停止 RPC 只有在导航任务 + 终态且底盘连续零速度采样确认后才释放控制权，否则同样保留隔离。 + +已知的执行完成、业务失败、取消、超时和预校验拒绝由 `ActionResultCode` 与 +`CommandHeader.Feedback` 表达。`ActionDeduplicationStatus` 结构化区分新受理、合并等待、 +缓存结果、已淘汰结果、账本耗尽、ID 冲突和服务实例不匹配；平台不得通过解析错误字符串 +判断动作是否执行过。 +`ACTION_RESULT_CODE_UNSPECIFIED` 不得作为服务端最终结果。 + ## MotorService `MotorService` 将 gRPC 电机命令适配到已经由 `DeviceManager` 创建的 diff --git a/cmvr-es/service/action/include/action_queue_executor.h b/cmvr-es/service/action/include/action_queue_executor.h new file mode 100644 index 00000000..3dbcac59 --- /dev/null +++ b/cmvr-es/service/action/include/action_queue_executor.h @@ -0,0 +1,65 @@ +#ifndef CMVR_ES_ACTION_QUEUE_EXECUTOR_H +#define CMVR_ES_ACTION_QUEUE_EXECUTOR_H + +#include +#include +#include +#include +#include + +#include "cmvr/api/system_command.pb.h" + +namespace cmvr::device { +class DeviceManager; +} + +namespace cmvr::service { + +// Owns the process-local FIFO used by SystemService ActionQueue requests. +// The executor intentionally has no grpc::ServerContext dependency: once a +// request is accepted, loss of the platform connection must not cancel device +// motion on the edge. +class ActionQueueExecutor final { +public: + static constexpr std::size_t kDefaultMaxAcceptedActionIds = + 256U * 1024U; + + enum class WaitResult { + Terminal, + CanceledBeforeAdmission, + CanceledAfterAdmission, + }; + + explicit ActionQueueExecutor( + device::DeviceManager& device_manager, + std::size_t max_accepted_action_ids = + kDefaultMaxAcceptedActionIds); + ~ActionQueueExecutor(); + + ActionQueueExecutor(const ActionQueueExecutor&) = delete; + ActionQueueExecutor& operator=(const ActionQueueExecutor&) = delete; + + // Validates, idempotently enqueues, and waits for the terminal result. + // Protocol and execution outcomes are represented in Feedback. + WaitResult submitAndWait( + const api::ActionQueueCommand_Request& request, + api::ActionQueueCommand_Feedback& feedback, + const std::function& waiter_canceled = {}); + + // StopAll uses this fail-closed transition. It rejects future submissions, + // cancels pending actions, and requests a typed stop for the active action. + // Returns true when every active Action device reported a confirmed stop. + // False means at least one resource remains fail-closed quarantined. + bool cancelAllAndDisable(); + + bool waitForIdle(std::chrono::milliseconds timeout); + const std::string& instanceId() const noexcept; + +private: + struct Impl; + std::unique_ptr impl_; +}; + +} // namespace cmvr::service + +#endif // CMVR_ES_ACTION_QUEUE_EXECUTOR_H diff --git a/cmvr-es/service/action/src/action_queue_executor.cpp b/cmvr-es/service/action/src/action_queue_executor.cpp new file mode 100644 index 00000000..e8641dd4 --- /dev/null +++ b/cmvr-es/service/action/src/action_queue_executor.cpp @@ -0,0 +1,2204 @@ +#include "service/action/include/action_queue_executor.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include "common/base/logging/logger.h" +#include "devices/agv/abstract_agv.h" +#include "devices/arm/robot_arm.h" +#include "manager/control_authority/include/control_authority_manager.h" +#include "manager/device_manager/include/device_manager.h" + +namespace cmvr::service { +namespace { + +using Clock = std::chrono::steady_clock; +using Milliseconds = std::chrono::milliseconds; + +constexpr std::size_t kMaxSteps = 256; +constexpr std::size_t kMaxQueuedActions = 64; +constexpr std::size_t kMaxTerminalResults = 4096; +constexpr std::size_t kMaxRequestBytes = 512U * 1024U; +constexpr std::size_t kMaxIdentifierBytes = 128; +constexpr std::size_t kMaxPathSegments = 4096; +constexpr std::size_t kMaxConcurrentSubmitters = 256; +constexpr Milliseconds kDefaultTotalTimeout = std::chrono::minutes(30); +constexpr Milliseconds kMaximumTimeout = std::chrono::hours(24); +constexpr Milliseconds kLeaseRetryPeriod{10}; +constexpr Milliseconds kWaiterCancellationPollPeriod{20}; +constexpr Milliseconds kArmWatchdogPollPeriod{5}; +constexpr Milliseconds kArmIdleConfirmationPollPeriod{10}; +constexpr auto kConcurrentArmStopConfirmationTimeout = + std::chrono::seconds(7); +constexpr auto kControlLeaseTtl = std::chrono::hours(25); + +enum class PreparedStepKind { + ArmMoveJ, + ArmMoveL, + AgvNavigateToPose, + AgvNavigateToStation, + AgvFollowPath, + Delay, +}; + +struct PreparedStep { + PreparedStepKind kind{PreparedStepKind::Delay}; + std::string device_id; + std::shared_ptr arm; + std::shared_ptr agv; +}; + +struct PreparedAction { + std::vector steps; + std::vector resource_ids; +}; + +struct ValidationResult { + bool valid{false}; + std::string error; + int failed_step_index{-1}; + PreparedAction prepared; +}; + +struct RequestFingerprint { + std::array words{}; + std::uint64_t serialized_size{0}; + + bool operator==(const RequestFingerprint& other) const noexcept + { + return serialized_size == other.serialized_size && + words == other.words; + } + + bool operator!=(const RequestFingerprint& other) const noexcept + { + return !(*this == other); + } +}; + +std::uint64_t stableHash( + const std::string& value, + const std::uint64_t seed) noexcept +{ + std::uint64_t hash = 1469598103934665603ULL ^ seed; + for (const unsigned char byte : value) { + hash ^= static_cast(byte); + hash *= 1099511628211ULL; + } + hash ^= hash >> 33U; + hash *= 0xff51afd7ed558ccdULL; + hash ^= hash >> 33U; + hash *= 0xc4ceb9fe1a85ec53ULL; + hash ^= hash >> 33U; + return hash; +} + +std::string generateServiceInstanceId() +{ + std::array bytes{}; + std::size_t offset = 0; + while (offset < bytes.size()) { + const auto received = ::getrandom( + bytes.data() + offset, + bytes.size() - offset, + 0); + if (received < 0) { + if (errno == EINTR) { + continue; + } + throw std::system_error( + errno, + std::generic_category(), + "could not generate the ActionQueue service instance id"); + } + if (received == 0) { + throw std::runtime_error( + "could not generate the ActionQueue service instance id"); + } + offset += static_cast(received); + } + + // RFC 4122 variant and version bits make the epoch recognizable as a + // random UUID without reducing its collision resistance materially. + bytes[6] = static_cast((bytes[6] & 0x0fU) | 0x40U); + bytes[8] = static_cast((bytes[8] & 0x3fU) | 0x80U); + std::ostringstream stream; + stream << std::hex << std::setfill('0'); + for (std::size_t index = 0; index < bytes.size(); ++index) { + if (index == 4U || index == 6U || index == 8U || index == 10U) { + stream << '-'; + } + stream << std::setw(2) << static_cast(bytes[index]); + } + return stream.str(); +} + +std::size_t validatedAcceptedActionLimit(const std::size_t limit) +{ + if (limit == 0U) { + throw std::invalid_argument( + "ActionQueue accepted-action limit must be positive"); + } + return limit; +} + +void fillTerminalFeedback( + api::ActionQueueCommand_Feedback& feedback, + const std::string& action_id, + const api::ActionResultCode result, + const std::uint32_t completed_steps, + const std::string& message, + const int failed_step_index = -1) +{ + feedback.Clear(); + feedback.set_action_id(action_id); + feedback.set_result(result); + feedback.set_completed_steps(completed_steps); + if (failed_step_index >= 0) { + feedback.set_failed_step_index( + static_cast(failed_step_index)); + } + auto* header = feedback.mutable_header(); + header->set_success(result == api::ACTION_RESULT_CODE_COMPLETED); + header->set_error_message(message); + *header->mutable_timestamp() = + google::protobuf::util::TimeUtil::GetCurrentTime(); +} + +bool finite(const double value) noexcept +{ + return std::isfinite(value); +} + +bool finiteNonNegative(const double value) noexcept +{ + return finite(value) && value >= 0.0; +} + +bool validArmOptions( + const api::MotionOptions& options, + std::string& error) +{ + if (options.asynchronous()) { + error = "ActionQueue requires synchronous RobotArm motion"; + return false; + } + if (!finiteNonNegative(options.velocity()) || + !finiteNonNegative(options.acceleration()) || + !finiteNonNegative(options.blend_radius()) || + !finiteNonNegative(options.jerk())) { + error = "RobotArm motion options must be finite and non-negative"; + return false; + } + for (const double limit : options.joint_velocity_limits()) { + if (!finiteNonNegative(limit)) { + error = "RobotArm joint velocity limits must be finite and non-negative"; + return false; + } + } + return true; +} + +bool validAgvOptions( + const msgs::AgvMotionOptions& options, + std::string& error) +{ + if (options.asynchronous()) { + error = "ActionQueue requires synchronous AGV navigation"; + return false; + } + if (!finiteNonNegative(options.max_speed()) || + !finiteNonNegative(options.max_angular_speed()) || + !finiteNonNegative(options.max_acceleration()) || + !finiteNonNegative(options.max_angular_acceleration()) || + !finiteNonNegative(options.reach_distance()) || + !finiteNonNegative(options.reach_angle()) || + !finiteNonNegative(options.speed_ratio())) { + error = "AGV motion options must be finite and non-negative"; + return false; + } + if (options.speed_ratio() > 1.0) { + error = "AGV speed_ratio must be in [0, 1]"; + return false; + } + if (options.wait_timeout_ms() < 0 || + options.poll_interval_ms() < 0) { + error = "AGV timeout and poll interval must be non-negative"; + return false; + } + if (options.poll_interval_ms() > 5000) { + error = "AGV poll_interval_ms must not exceed 5000"; + return false; + } + if (options.wait_timeout_ms() > 0 && + options.poll_interval_ms() > options.wait_timeout_ms()) { + error = "AGV poll_interval_ms must not exceed wait_timeout_ms"; + return false; + } + return true; +} + +device::FrameType toFrameType(const api::ArmFrameType frame) +{ + switch (frame) { + case api::ARM_FRAME_BASE: + return device::FrameType::Base; + case api::ARM_FRAME_TOOL: + return device::FrameType::Tool; + case api::ARM_FRAME_WORLD: + return device::FrameType::World; + case api::ARM_FRAME_USER: + return device::FrameType::User; + } + return device::FrameType::Base; +} + +device::MotionOptions toArmMotionOptions( + const api::MotionOptions& source) +{ + device::MotionOptions destination; + destination.velocity = source.velocity(); + destination.acceleration = source.acceleration(); + destination.blend_radius = source.blend_radius(); + destination.jerk = source.jerk() > 0.0 ? source.jerk() : 5.0; + destination.joint_velocity_limits.assign( + source.joint_velocity_limits().begin(), + source.joint_velocity_limits().end()); + destination.asynchronous = source.asynchronous(); + return destination; +} + +device::AgvAdapterParams toAgvAdapterParams( + const msgs::AgvAdapterParams& source) +{ + device::AgvAdapterParams destination; + for (const auto& item : source.values()) { + destination.values.emplace(item.first, item.second); + } + return destination; +} + +device::AgvMotionOptions toAgvMotionOptions( + const msgs::AgvMotionOptions& source) +{ + device::AgvMotionOptions destination; + destination.max_speed = source.max_speed(); + destination.max_angular_speed = source.max_angular_speed(); + destination.max_acceleration = source.max_acceleration(); + destination.max_angular_acceleration = + source.max_angular_acceleration(); + destination.reach_distance = source.reach_distance(); + destination.reach_angle = source.reach_angle(); + destination.speed_ratio = + source.speed_ratio() > 0.0 ? source.speed_ratio() : 1.0; + destination.asynchronous = source.asynchronous(); + destination.wait_timeout_ms = source.wait_timeout_ms(); + destination.poll_interval_ms = source.poll_interval_ms(); + return destination; +} + +std::string stepPrefix( + const int index, + const api::ActionStep& step) +{ + std::string prefix = "step " + std::to_string(index); + if (!step.step_id().empty()) { + prefix += " (" + step.step_id() + ")"; + } + return prefix + ": "; +} + +std::string canonicalRequest( + const api::ActionQueueCommand_Request& request) +{ + api::ActionQueueCommand_Request normalized(request); + normalized.DiscardUnknownFields(); + for (auto& step : *normalized.mutable_steps()) { + switch (step.command_case()) { + case api::ActionStep::kArmMoveJ: + step.mutable_arm_move_j() + ->mutable_header()->clear_timestamp(); + break; + case api::ActionStep::kArmMoveL: + step.mutable_arm_move_l() + ->mutable_header()->clear_timestamp(); + break; + case api::ActionStep::kAgvNavigateToPose: + step.mutable_agv_navigate_to_pose() + ->mutable_header()->clear_timestamp(); + break; + case api::ActionStep::kAgvNavigateToStation: + step.mutable_agv_navigate_to_station() + ->mutable_header()->clear_timestamp(); + break; + case api::ActionStep::kAgvFollowPath: + step.mutable_agv_follow_path() + ->mutable_header()->clear_timestamp(); + break; + case api::ActionStep::kDelay: + case api::ActionStep::COMMAND_NOT_SET: + break; + } + } + + std::string serialized; + google::protobuf::io::StringOutputStream stream(&serialized); + google::protobuf::io::CodedOutputStream coded_stream(&stream); + coded_stream.SetSerializationDeterministic(true); + if (!normalized.SerializeToCodedStream(&coded_stream)) { + return {}; + } + coded_stream.Trim(); + return serialized; +} + +std::optional fingerprintRequest( + const api::ActionQueueCommand_Request& request) +{ + const std::string serialized = canonicalRequest(request); + if (serialized.empty()) { + return std::nullopt; + } + RequestFingerprint fingerprint; + fingerprint.serialized_size = serialized.size(); + constexpr std::array seeds{ + 0xa4093822299f31d0ULL, + 0x082efa98ec4e6c89ULL, + 0x452821e638d01377ULL, + 0xbe5466cf34e90c6cULL}; + for (std::size_t index = 0; index < seeds.size(); ++index) { + fingerprint.words[index] = stableHash(serialized, seeds[index]); + } + return fingerprint; +} + +std::string validateRequestEnvelope( + const api::ActionQueueCommand_Request& request) +{ + if (request.action_id().empty()) { + return "action_id is required"; + } + if (request.action_id().size() > kMaxIdentifierBytes) { + return "action_id is too long"; + } + if (request.expected_service_instance_id().empty()) { + return "expected_service_instance_id is required; obtain it from GetSystemInfo"; + } + if (request.expected_service_instance_id().size() > + kMaxIdentifierBytes) { + return "expected_service_instance_id is too long"; + } + if (request.steps().empty()) { + return "ActionQueue requires at least one step"; + } + if (static_cast(request.steps_size()) > kMaxSteps) { + return "ActionQueue exceeds the maximum step count"; + } + if (request.ByteSizeLong() > kMaxRequestBytes) { + return "ActionQueue request exceeds the 512 KiB application limit"; + } + if (request.total_timeout_ms() > + static_cast(kMaximumTimeout.count())) { + return "ActionQueue total timeout exceeds the server limit"; + } + return {}; +} + +ValidationResult validateRequest( + device::DeviceManager& device_manager, + const api::ActionQueueCommand_Request& request) +{ + ValidationResult result; + result.error = validateRequestEnvelope(request); + if (!result.error.empty()) { + return result; + } + + std::unordered_set step_ids; + std::set resources; + result.prepared.steps.reserve( + static_cast(request.steps_size())); + + for (int index = 0; index < request.steps_size(); ++index) { + result.failed_step_index = index; + const auto& step = request.steps(index); + const std::string prefix = stepPrefix(index, step); + if (step.step_id().empty()) { + result.error = prefix + "step_id is required"; + return result; + } + if (step.step_id().size() > kMaxIdentifierBytes) { + result.error = prefix + "step_id is too long"; + return result; + } + if (!step_ids.emplace(step.step_id()).second) { + result.error = prefix + "step_id must be unique"; + return result; + } + if (step.timeout_ms() > + static_cast(kMaximumTimeout.count())) { + result.error = prefix + "timeout exceeds the server limit"; + return result; + } + + PreparedStep prepared; + std::string options_error; + switch (step.command_case()) { + case api::ActionStep::kArmMoveJ: { + prepared.kind = PreparedStepKind::ArmMoveJ; + const auto& command = step.arm_move_j(); + prepared.device_id = command.header().device_id(); + if (prepared.device_id.empty()) { + result.error = prefix + "RobotArm device_id is required"; + return result; + } + prepared.arm = device_manager.getDevice( + prepared.device_id); + if (!prepared.arm) { + result.error = prefix + "RobotArm device not found: " + + prepared.device_id; + return result; + } + if (!prepared.arm->supportsActionQueueMotion()) { + result.error = prefix + + "RobotArm backend does not support safe ActionQueue motion: " + + prepared.device_id; + return result; + } + if (!validArmOptions(command.options(), options_error)) { + result.error = prefix + options_error; + return result; + } + const auto dof = prepared.arm->getDof(); + if (dof == 0U || command.target().position_size() != + static_cast(dof)) { + result.error = prefix + + "MoveJ target size does not match RobotArm DOF"; + return result; + } + if (!command.options().joint_velocity_limits().empty() && + command.options().joint_velocity_limits_size() != + static_cast(dof)) { + result.error = prefix + + "MoveJ joint velocity limit size does not match RobotArm DOF"; + return result; + } + for (const double position : command.target().position()) { + if (!finite(position)) { + result.error = prefix + + "MoveJ target must contain finite values"; + return result; + } + } + resources.emplace(prepared.device_id); + break; + } + case api::ActionStep::kArmMoveL: { + prepared.kind = PreparedStepKind::ArmMoveL; + const auto& command = step.arm_move_l(); + prepared.device_id = command.header().device_id(); + if (prepared.device_id.empty()) { + result.error = prefix + "RobotArm device_id is required"; + return result; + } + prepared.arm = device_manager.getDevice( + prepared.device_id); + if (!prepared.arm) { + result.error = prefix + "RobotArm device not found: " + + prepared.device_id; + return result; + } + if (!prepared.arm->supportsActionQueueMotion()) { + result.error = prefix + + "RobotArm backend does not support safe ActionQueue motion: " + + prepared.device_id; + return result; + } + if (!validArmOptions(command.options(), options_error)) { + result.error = prefix + options_error; + return result; + } + if (!api::ArmFrameType_IsValid(command.frame())) { + result.error = prefix + "MoveL frame is invalid"; + return result; + } + if (command.frame() != api::ARM_FRAME_BASE) { + result.error = prefix + + "ActionQueue MoveL currently supports the Base frame only"; + return result; + } + const auto dof = prepared.arm->getDof(); + if (!command.options().joint_velocity_limits().empty() && + command.options().joint_velocity_limits_size() != + static_cast(dof)) { + result.error = prefix + + "MoveL joint velocity limit size does not match RobotArm DOF"; + return result; + } + const auto& target = command.target(); + if (!finite(target.x()) || !finite(target.y()) || + !finite(target.z()) || !finite(target.rx()) || + !finite(target.ry()) || !finite(target.rz())) { + result.error = prefix + + "MoveL target must contain finite values"; + return result; + } + resources.emplace(prepared.device_id); + break; + } + case api::ActionStep::kAgvNavigateToPose: { + prepared.kind = PreparedStepKind::AgvNavigateToPose; + const auto& command = step.agv_navigate_to_pose(); + prepared.device_id = command.header().device_id(); + if (prepared.device_id.empty()) { + result.error = prefix + "AGV device_id is required"; + return result; + } + prepared.agv = device_manager.getDevice( + prepared.device_id); + if (!prepared.agv) { + result.error = prefix + "AGV device not found: " + + prepared.device_id; + return result; + } + if (!prepared.agv->supportsSynchronousAction( + device::AgvActionKind::NavigateToPose)) { + result.error = prefix + + "AGV backend cannot confirm synchronous pose navigation: " + + prepared.device_id; + return result; + } + if (!validAgvOptions(command.options(), options_error)) { + result.error = prefix + options_error; + return result; + } + if (!finite(command.pose().x()) || + !finite(command.pose().y()) || + !finite(command.pose().theta())) { + result.error = prefix + + "AGV pose must contain finite values"; + return result; + } + resources.emplace(prepared.device_id); + break; + } + case api::ActionStep::kAgvNavigateToStation: { + prepared.kind = PreparedStepKind::AgvNavigateToStation; + const auto& command = step.agv_navigate_to_station(); + prepared.device_id = command.header().device_id(); + if (prepared.device_id.empty()) { + result.error = prefix + "AGV device_id is required"; + return result; + } + if (command.station_id().empty()) { + result.error = prefix + "AGV station_id is required"; + return result; + } + prepared.agv = device_manager.getDevice( + prepared.device_id); + if (!prepared.agv) { + result.error = prefix + "AGV device not found: " + + prepared.device_id; + return result; + } + if (!prepared.agv->supportsSynchronousAction( + device::AgvActionKind::NavigateToStation)) { + result.error = prefix + + "AGV backend cannot confirm synchronous station navigation: " + + prepared.device_id; + return result; + } + if (!validAgvOptions(command.options(), options_error)) { + result.error = prefix + options_error; + return result; + } + resources.emplace(prepared.device_id); + break; + } + case api::ActionStep::kAgvFollowPath: { + prepared.kind = PreparedStepKind::AgvFollowPath; + const auto& command = step.agv_follow_path(); + prepared.device_id = command.header().device_id(); + if (prepared.device_id.empty()) { + result.error = prefix + "AGV device_id is required"; + return result; + } + if (command.path().empty()) { + result.error = prefix + "AGV path must not be empty"; + return result; + } + if (static_cast(command.path_size()) > + kMaxPathSegments) { + result.error = prefix + "AGV path is too large"; + return result; + } + for (const auto& segment : command.path()) { + if (segment.source_station().empty() || + segment.target_station().empty()) { + result.error = prefix + + "AGV path station ids must not be empty"; + return result; + } + } + prepared.agv = device_manager.getDevice( + prepared.device_id); + if (!prepared.agv) { + result.error = prefix + "AGV device not found: " + + prepared.device_id; + return result; + } + if (!prepared.agv->supportsSynchronousAction( + device::AgvActionKind::FollowPath)) { + result.error = prefix + + "AGV backend cannot confirm synchronous path navigation: " + + prepared.device_id; + return result; + } + if (!validAgvOptions(command.options(), options_error)) { + result.error = prefix + options_error; + return result; + } + resources.emplace(prepared.device_id); + break; + } + case api::ActionStep::kDelay: + prepared.kind = PreparedStepKind::Delay; + if (step.delay().duration_ms() > + static_cast(kMaximumTimeout.count())) { + result.error = prefix + + "delay exceeds the server limit"; + return result; + } + break; + case api::ActionStep::COMMAND_NOT_SET: + result.error = prefix + "command is not set"; + return result; + } + result.prepared.steps.push_back(std::move(prepared)); + } + + result.prepared.resource_ids.assign( + resources.begin(), resources.end()); + result.failed_step_index = -1; + result.valid = true; + return result; +} + +device::AgvActionKind toAgvActionKind( + const PreparedStepKind kind) +{ + switch (kind) { + case PreparedStepKind::AgvNavigateToPose: + return device::AgvActionKind::NavigateToPose; + case PreparedStepKind::AgvNavigateToStation: + return device::AgvActionKind::NavigateToStation; + case PreparedStepKind::AgvFollowPath: + return device::AgvActionKind::FollowPath; + default: + return device::AgvActionKind::NavigateToPose; + } +} + +} // namespace + +struct ActionQueueExecutor::Impl { + struct Record { + api::ActionQueueCommand_Request request; + RequestFingerprint fingerprint; + PreparedAction prepared; + Clock::time_point deadline; + std::atomic cancel_requested{false}; + std::atomic timed_out{false}; + // -1 means that no device step is currently inside a backend call. + // StopAll reads this without taking Record::mutex so it can preempt + // the physically active device before stopping the remaining action + // resources. + std::atomic active_step_index{-1}; + // Exact normal leases acquired for this execution. Typed-stop paths + // may convert only these generations into safety barriers, so a stale + // Action can never preempt a later command on the same device. + std::unordered_map + control_tokens; + // Fallback for an invariant or manager failure which prevents an exact + // lease from being converted into a permanent quarantine. + std::atomic retain_control_leases{false}; + std::mutex mutex; + std::condition_variable condition; + bool done{false}; + std::string stop_error; + api::ActionQueueCommand_Feedback feedback; + }; + + struct TerminalResult { + RequestFingerprint fingerprint; + api::ActionQueueCommand_Feedback feedback; + }; + + struct LeaseSet { + explicit LeaseSet(std::shared_ptr owner_record) + : record(std::move(owner_record)) + { + } + + ~LeaseSet() + { + if (record && record->retain_control_leases.load( + std::memory_order_acquire)) { + return; + } + auto& manager = + control::ControlAuthorityManager::instance(); + for (const auto& token : tokens) { + manager.release(token); + } + } + + const control::ControlLeaseToken* find( + const std::string& resource_id) const + { + const auto found = std::find_if( + tokens.begin(), tokens.end(), + [&resource_id](const auto& token) { + return token.resource_id == resource_id; + }); + return found == tokens.end() ? nullptr : &*found; + } + + std::vector tokens; + std::shared_ptr record; + }; + + struct SubmitterGuard { + explicit SubmitterGuard(std::atomic& value) + : count(value) + { + } + + ~SubmitterGuard() + { + count.fetch_sub(1U, std::memory_order_acq_rel); + } + + std::atomic& count; + }; + + struct DeduplicationFeedbackGuard { + ~DeduplicationFeedbackGuard() + { + feedback.set_deduplication_status(status); + } + + api::ActionQueueCommand_Feedback& feedback; + api::ActionDeduplicationStatus& status; + }; + + explicit Impl( + device::DeviceManager& manager, + const std::size_t accepted_action_limit) + : device_manager(manager), + instance_id(generateServiceInstanceId()), + max_accepted_action_ids( + validatedAcceptedActionLimit(accepted_action_limit)), + worker([this]() { workerLoop(); }) + { + } + + ~Impl() + { + shutdown(); + } + + void shutdown() + { + std::shared_ptr active_record; + { + std::lock_guard lock(mutex); + if (joined) { + return; + } + accepting = false; + stopping = true; + for (const auto& record : queue) { + record->cancel_requested.store( + true, std::memory_order_release); + record->condition.notify_all(); + } + active_record = active; + if (active_record) { + active_record->cancel_requested.store( + true, std::memory_order_release); + active_record->condition.notify_all(); + } + } + (void)requestTypedStop(active_record); + queue_condition.notify_all(); + if (worker.joinable()) { + worker.join(); + } + joined = true; + } + + bool cancelAllAndDisable() + { + std::shared_ptr active_record; + { + std::lock_guard lock(mutex); + accepting = false; + for (const auto& record : queue) { + record->cancel_requested.store( + true, std::memory_order_release); + record->condition.notify_all(); + } + active_record = active; + if (active_record) { + active_record->cancel_requested.store( + true, std::memory_order_release); + active_record->condition.notify_all(); + } + } + const bool stopped = requestTypedStop(active_record); + queue_condition.notify_all(); + return stopped; + } + + static bool waitForArmIdle( + const std::shared_ptr& arm, + const Clock::duration timeout) + { + const auto deadline = Clock::now() + timeout; + while (Clock::now() < deadline) { + try { + if (!arm->busy()) { + return true; + } + } catch (...) { + return false; + } + std::this_thread::sleep_for( + kArmIdleConfirmationPollPeriod); + } + try { + return !arm->busy(); + } catch (...) { + return false; + } + } + + bool requestTypedStop(const std::shared_ptr& record) + { + if (!record) { + return true; + } + bool all_stopped = true; + std::unordered_set stopped_arms; + std::unordered_set stopped_agvs; + std::vector stop_order; + stop_order.reserve(record->prepared.steps.size()); + const int active_step_index = + record->active_step_index.load(std::memory_order_acquire); + if (active_step_index >= 0 && + static_cast(active_step_index) < + record->prepared.steps.size()) { + stop_order.push_back( + static_cast(active_step_index)); + } + for (std::size_t index = 0; + index < record->prepared.steps.size(); ++index) { + if (active_step_index >= 0 && + index == static_cast(active_step_index)) { + continue; + } + stop_order.push_back(index); + } + for (const std::size_t index : stop_order) { + const auto& step = record->prepared.steps[index]; + const bool is_arm = step.arm && + stopped_arms.emplace(step.device_id).second; + const bool is_agv = step.agv && + stopped_agvs.emplace(step.device_id).second; + if (!is_arm && !is_agv) { + continue; + } + auto& authority = + control::ControlAuthorityManager::instance(); + const std::string owner = + "grpc-system:action-cancel:" + + record->request.action_id() + ":" + + std::to_string( + sequence.fetch_add( + 1U, std::memory_order_relaxed) + 1U); + const auto ttl = std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>( + std::chrono::minutes(1)); + std::optional expected_token; + { + std::lock_guard lock(record->mutex); + const auto found = + record->control_tokens.find(step.device_id); + if (found != record->control_tokens.end()) { + expected_token = found->second; + } + } + if (!expected_token) { + // Cancellation may race an Action which is still waiting to + // acquire its device set. No Action-owned motion has been + // submitted in that state, so there is nothing to stop. + if (active_step_index >= 0) { + all_stopped = false; + record->retain_control_leases.store( + true, std::memory_order_release); + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] active device has no exact " + "control token; retaining Action leases, id=" + << step.device_id; + } + continue; + } + + control::ControlAcquireResult barrier; + try { + barrier = authority.preemptAcquireIfCurrent( + *expected_token, owner, ttl); + } catch (const std::exception& error) { + all_stopped = false; + (void)authority.quarantineIfCurrent(*expected_token); + record->retain_control_leases.store( + true, std::memory_order_release); + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] could not establish typed stop " + "barrier; control remains quarantined, id=" + << step.device_id << ", error=" << error.what(); + continue; + } catch (...) { + all_stopped = false; + (void)authority.quarantineIfCurrent(*expected_token); + record->retain_control_leases.store( + true, std::memory_order_release); + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] could not establish typed stop " + "barrier; control remains quarantined, id=" + << step.device_id; + continue; + } + if (!barrier.acquired) { + if (authority.quarantineIfCurrent(*expected_token)) { + all_stopped = false; + record->retain_control_leases.store( + true, std::memory_order_release); + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] exact typed stop barrier was " + "not established while the Action lease remained " + "current; control remains quarantined, id=" + << step.device_id + << ", detail=" << barrier.detail; + } + continue; + } + + bool stop_confirmed = false; + try { + if (is_arm) { + const auto result = step.arm->stopMotion(); + stop_confirmed = result.ok(); + if (!stop_confirmed) { + CMVR_LOG(WARNING) + << "[ActionQueueExecutor] typed RobotArm stop failed, id=" + << step.device_id << ", error=" << result.message; + stop_confirmed = waitForArmIdle( + step.arm, + kConcurrentArmStopConfirmationTimeout); + } + } else { + const auto cancel = step.agv->cancelNavigation(); + if (!cancel.ok()) { + CMVR_LOG(WARNING) + << "[ActionQueueExecutor] typed AGV cancel failed, id=" + << step.device_id << ", error=" << cancel.message; + } + const auto velocity_stop = + step.agv->stopVelocityControl(); + if (!velocity_stop.ok() && + velocity_stop.code != + device::AgvErrorCode::UnsupportedCommand) { + CMVR_LOG(WARNING) + << "[ActionQueueExecutor] typed AGV velocity stop failed, id=" + << step.device_id + << ", error=" << velocity_stop.message; + } + const auto stopped = + step.agv->confirmMotionStopped(); + stop_confirmed = stopped.ok(); + if (!stop_confirmed) { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] AGV stopped state was " + "not confirmed, id=" + << step.device_id + << ", error=" << stopped.message; + } + } + } catch (const std::exception& error) { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] typed stop threw, id=" + << step.device_id << ", error=" << error.what(); + } catch (...) { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] typed stop threw, id=" + << step.device_id; + } + if (stop_confirmed) { + authority.release(barrier.token); + } else { + all_stopped = false; + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] typed stop was not confirmed; " + "control remains quarantined, id=" + << step.device_id; + // Retain this safety holder fail-closed. A normal lease must + // not be admitted while the physical outcome is unknown. + } + } + return all_stopped; + } + + static bool waiterCanceled( + const std::function& waiter_canceled) noexcept + { + if (!waiter_canceled) { + return false; + } + try { + return waiter_canceled(); + } catch (...) { + // A broken waiter must not cancel an admitted edge action. End only + // this caller's wait and leave the worker-owned Record untouched. + return true; + } + } + + ActionQueueExecutor::WaitResult submitAndWait( + const api::ActionQueueCommand_Request& request, + api::ActionQueueCommand_Feedback& feedback, + const std::function& waiter_canceled) + { + api::ActionDeduplicationStatus deduplication_status = + api::ACTION_DEDUPLICATION_STATUS_UNSPECIFIED; + DeduplicationFeedbackGuard deduplication_feedback{ + feedback, deduplication_status}; + const auto previous_submitters = concurrent_submitters.fetch_add( + 1U, std::memory_order_acq_rel); + if (previous_submitters >= kMaxConcurrentSubmitters) { + concurrent_submitters.fetch_sub(1U, std::memory_order_acq_rel); + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "ActionQueue has too many concurrent submitters"); + return ActionQueueExecutor::WaitResult::Terminal; + } + SubmitterGuard submitter_guard(concurrent_submitters); + + const std::string envelope_error = + validateRequestEnvelope(request); + if (!envelope_error.empty()) { + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + envelope_error); + return ActionQueueExecutor::WaitResult::Terminal; + } + if (request.expected_service_instance_id() != instance_id) { + deduplication_status = + api::ACTION_DEDUPLICATION_STATUS_SERVICE_INSTANCE_MISMATCH; + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "expected_service_instance_id does not match the active ActionQueue service instance; reconcile the prior action before submitting a new id"); + return ActionQueueExecutor::WaitResult::Terminal; + } + const auto fingerprint = fingerprintRequest(request); + if (!fingerprint) { + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "ActionQueue request could not be serialized"); + return ActionQueueExecutor::WaitResult::Terminal; + } + std::shared_ptr record; + const auto lookup_existing_locked = [&]() { + const auto found = records.find(request.action_id()); + if (found != records.end()) { + if (found->second->fingerprint != *fingerprint) { + deduplication_status = + api::ACTION_DEDUPLICATION_STATUS_ACTION_ID_CONFLICT; + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "action_id is already associated with a different request"); + return true; + } + record = found->second; + { + std::lock_guard record_lock(record->mutex); + deduplication_status = record->done + ? api::ACTION_DEDUPLICATION_STATUS_CACHED_RESULT + : api::ACTION_DEDUPLICATION_STATUS_JOINED_IN_FLIGHT; + } + return false; + } + + const auto terminal = terminal_results.find(request.action_id()); + if (terminal != terminal_results.end()) { + if (terminal->second.fingerprint != *fingerprint) { + deduplication_status = + api::ACTION_DEDUPLICATION_STATUS_ACTION_ID_CONFLICT; + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "action_id is already associated with a different request"); + } else { + feedback = terminal->second.feedback; + deduplication_status = + api::ACTION_DEDUPLICATION_STATUS_CACHED_RESULT; + } + return true; + } + + if (retired_action_ids.find(request.action_id()) != + retired_action_ids.end()) { + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "action_id was already completed but its result is no longer cached; it will not be re-executed"); + deduplication_status = + api::ACTION_DEDUPLICATION_STATUS_RESULT_EVICTED; + return true; + } + return false; + }; + + bool handled = false; + const bool initially_canceled = waiterCanceled(waiter_canceled); + { + std::lock_guard lock(mutex); + handled = lookup_existing_locked(); + } + if (handled) { + return ActionQueueExecutor::WaitResult::Terminal; + } + if (record) { + return waitForRecord(record, feedback, waiter_canceled); + } + if (initially_canceled) { + return ActionQueueExecutor::WaitResult::CanceledBeforeAdmission; + } + + const auto validation = validateRequest(device_manager, request); + if (!validation.valid) { + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + validation.error, + validation.failed_step_index); + return ActionQueueExecutor::WaitResult::Terminal; + } + const auto total_timeout = request.total_timeout_ms() == 0U + ? kDefaultTotalTimeout + : Milliseconds(request.total_timeout_ms()); + auto candidate = std::make_shared(); + candidate->request = request; + candidate->fingerprint = *fingerprint; + candidate->prepared = validation.prepared; + candidate->deadline = Clock::now() + total_timeout; + + const bool canceled_before_admission = + waiterCanceled(waiter_canceled); + bool canceled_without_record = false; + { + std::lock_guard lock(mutex); + handled = lookup_existing_locked(); + if (!handled && !record && canceled_before_admission) { + canceled_without_record = true; + } else if (!handled && !record && (!accepting || stopping)) { + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "ActionQueue is disabled by a system stop"); + handled = true; + } else if (!handled && !record && + queue.size() + (active ? 1U : 0U) >= + kMaxQueuedActions) { + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "ActionQueue is full"); + handled = true; + } else if (!handled && !record && + records.size() + terminal_results.size() + + retired_action_ids.size() >= + max_accepted_action_ids) { + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "ActionQueue idempotency ledger capacity is exhausted; restart with a new service instance only after reconciling prior actions"); + deduplication_status = + api::ACTION_DEDUPLICATION_STATUS_LEDGER_EXHAUSTED; + handled = true; + } else if (!handled && !record) { + record = std::move(candidate); + const auto inserted = + records.emplace(request.action_id(), record); + if (!inserted.second) { + throw std::logic_error( + "ActionQueue admission record already exists"); + } + try { + queue.push_back(record); + } catch (...) { + // No other thread can observe the record while the queue + // mutex is held. Roll it back so a failed deque allocation + // cannot leave an ID which waits forever without work. + records.erase(inserted.first); + record.reset(); + throw; + } + deduplication_status = + api::ACTION_DEDUPLICATION_STATUS_ACCEPTED_NEW; + queue_condition.notify_one(); + } + } + if (handled) { + return ActionQueueExecutor::WaitResult::Terminal; + } + if (record) { + return waitForRecord(record, feedback, waiter_canceled); + } + if (canceled_without_record) { + return ActionQueueExecutor::WaitResult::CanceledBeforeAdmission; + } + return waitForRecord(record, feedback, waiter_canceled); + } + + static ActionQueueExecutor::WaitResult waitForRecord( + const std::shared_ptr& record, + api::ActionQueueCommand_Feedback& feedback, + const std::function& waiter_canceled) + { + std::unique_lock lock(record->mutex); + if (!waiter_canceled) { + record->condition.wait(lock, [&record]() { + return record->done; + }); + feedback = record->feedback; + return ActionQueueExecutor::WaitResult::Terminal; + } + + for (;;) { + if (record->done) { + feedback = record->feedback; + return ActionQueueExecutor::WaitResult::Terminal; + } + lock.unlock(); + if (waiterCanceled(waiter_canceled)) { + return ActionQueueExecutor::WaitResult::CanceledAfterAdmission; + } + lock.lock(); + record->condition.wait_for( + lock, + kWaiterCancellationPollPeriod, + [&record]() { return record->done; }); + } + } + + bool waitForIdle(const Milliseconds timeout) + { + std::unique_lock lock(mutex); + return idle_condition.wait_for(lock, timeout, [this]() { + return queue.empty() && !active; + }); + } + + void workerLoop() + { + for (;;) { + std::shared_ptr record; + { + std::unique_lock lock(mutex); + queue_condition.wait(lock, [this]() { + return stopping || !queue.empty(); + }); + if (stopping && queue.empty()) { + return; + } + record = queue.front(); + queue.pop_front(); + active = record; + } + + try { + execute(record); + } catch (const std::exception& error) { + complete( + record, api::ACTION_RESULT_CODE_FAILED, 0, + std::string("ActionQueue worker failed: ") + + error.what()); + } catch (...) { + complete( + record, api::ACTION_RESULT_CODE_FAILED, 0, + "ActionQueue worker failed with an unknown exception"); + } + { + std::lock_guard lock(mutex); + if (active == record) { + active.reset(); + } + try { + TerminalResult terminal; + terminal.fingerprint = record->fingerprint; + { + std::lock_guard record_lock(record->mutex); + terminal.feedback = record->feedback; + } + const std::string action_id = + record->request.action_id(); + const auto live = records.find(action_id); + terminal_result_order.push_back(action_id); + try { + const auto inserted = terminal_results.emplace( + action_id, std::move(terminal)); + if (!inserted.second) { + throw std::logic_error( + "ActionQueue terminal result already exists"); + } + } catch (...) { + // Keep the full live record as the source of truth if + // the compact cache cannot be committed atomically. + terminal_result_order.pop_back(); + throw; + } + if (live != records.end()) { + records.erase(live); + } + while (terminal_result_order.size() > + kMaxTerminalResults) { + const std::string retired = + terminal_result_order.front(); + // Insert into the exact ledger before dropping the + // cached result. Allocation failure must retain the + // old result rather than create a replay window. + retired_action_ids.emplace(retired); + terminal_results.erase(retired); + terminal_result_order.pop_front(); + } + } catch (const std::exception& error) { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] could not compact terminal " + "idempotency state; retaining existing state: " + << error.what(); + } catch (...) { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] could not compact terminal " + "idempotency state; retaining existing state"; + } + idle_condition.notify_all(); + } + } + } + + void complete( + const std::shared_ptr& record, + const api::ActionResultCode result, + const std::uint32_t completed_steps, + const std::string& message, + const int failed_step_index = -1) + { + { + std::lock_guard lock(record->mutex); + fillTerminalFeedback( + record->feedback, + record->request.action_id(), + result, + completed_steps, + message, + failed_step_index); + record->done = true; + } + record->condition.notify_all(); + } + + bool acquireLeases( + const std::shared_ptr& record, + LeaseSet& leases, + std::string& error) + { + auto& authority = + control::ControlAuthorityManager::instance(); + const std::string owner = + "grpc-system:action:" + record->request.action_id() + ":" + + std::to_string( + sequence.fetch_add(1U, std::memory_order_relaxed) + 1U); + const auto ttl = std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>(kControlLeaseTtl); + + while (Clock::now() < record->deadline) { + if (record->cancel_requested.load(std::memory_order_acquire)) { + error = "ActionQueue was canceled before device reservation"; + return false; + } + bool all_acquired = true; + std::string conflict; + for (const auto& resource_id : record->prepared.resource_ids) { + auto acquired = authority.tryAcquire( + resource_id, owner, ttl); + if (!acquired.acquired) { + all_acquired = false; + conflict = std::move(acquired.detail); + break; + } + leases.tokens.push_back(std::move(acquired.token)); + } + if (all_acquired) { + std::lock_guard record_lock(record->mutex); + record->control_tokens.clear(); + for (const auto& token : leases.tokens) { + record->control_tokens.emplace( + token.resource_id, token); + } + return true; + } + for (const auto& token : leases.tokens) { + authority.release(token); + } + leases.tokens.clear(); + error = conflict.empty() + ? "device control is unavailable" + : std::move(conflict); + + std::unique_lock record_lock(record->mutex); + const auto wake_at = std::min( + record->deadline, + Clock::now() + kLeaseRetryPeriod); + record->condition.wait_until( + record_lock, wake_at, [&record]() { + return record->cancel_requested.load( + std::memory_order_acquire); + }); + } + record->timed_out.store(true, std::memory_order_release); + error = "ActionQueue timed out while waiting for device control"; + return false; + } + + bool validateLeases(const LeaseSet& leases) const + { + auto& authority = + control::ControlAuthorityManager::instance(); + return std::all_of( + leases.tokens.begin(), leases.tokens.end(), + [&authority](const auto& token) { + return authority.validate(token); + }); + } + + bool validateDevicesIdle( + const std::shared_ptr& record, + std::string& error) const + { + std::unordered_set inspected_arms; + std::unordered_set inspected_agvs; + for (const auto& step : record->prepared.steps) { + if (step.arm && + inspected_arms.emplace(step.device_id).second && + step.arm->busy()) { + error = "RobotArm already has active motion before ActionQueue execution: " + + step.device_id; + return false; + } + if (!step.agv || + !inspected_agvs.emplace(step.device_id).second) { + continue; + } + const auto runtime = step.agv->runtimeState(); + const auto navigation = step.agv->navigationStatus(); + if (runtime.emergency_stopped || runtime.fault) { + error = "AGV is faulted or emergency-stopped before ActionQueue execution: " + + step.device_id; + return false; + } + if (runtime.moving || + navigation.state == device::AgvTaskState::Waiting || + navigation.state == device::AgvTaskState::Running || + navigation.state == device::AgvTaskState::Paused) { + error = "AGV already has active motion before ActionQueue execution: " + + step.device_id; + return false; + } + } + return true; + } + + Clock::time_point stepDeadline( + const std::shared_ptr& record, + const api::ActionStep& step) const + { + if (step.timeout_ms() == 0U) { + return record->deadline; + } + return std::min( + record->deadline, + Clock::now() + Milliseconds(step.timeout_ms())); + } + + void armTimeoutWatchdog( + const std::shared_ptr& record, + const PreparedStep& step, + const Clock::time_point deadline, + const std::shared_ptr>& disarmed, + const std::shared_ptr>& stop_started) + { + while (!disarmed->load(std::memory_order_acquire)) { + const auto now = Clock::now(); + if (now >= deadline) { + record->timed_out.store(true, std::memory_order_release); + requestTimedOutArmStop(record, step, stop_started); + record->condition.notify_all(); + return; + } + std::this_thread::sleep_for(std::min( + kArmWatchdogPollPeriod, + std::chrono::duration_cast(deadline - now))); + } + } + + void requestTimedOutArmStop( + const std::shared_ptr& record, + const PreparedStep& step, + const std::shared_ptr>& stop_started) + { + bool expected = false; + if (!stop_started->compare_exchange_strong( + expected, true, + std::memory_order_acq_rel, + std::memory_order_acquire)) { + return; + } + auto& authority = + control::ControlAuthorityManager::instance(); + const std::string owner = + "grpc-system:action-timeout:" + + record->request.action_id(); + const auto ttl = std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>( + std::chrono::minutes(1)); + std::optional expected_token; + { + std::lock_guard lock(record->mutex); + const auto found = record->control_tokens.find(step.device_id); + if (found != record->control_tokens.end()) { + expected_token = found->second; + } + } + if (!step.arm || !expected_token) { + record->retain_control_leases.store( + true, std::memory_order_release); + const std::string detail = + "could not establish timed-out RobotArm stop barrier: " + "the active Action lease is unavailable"; + { + std::lock_guard lock(record->mutex); + record->stop_error = detail; + } + CMVR_LOG(ERROR) << "[ActionQueueExecutor] " << detail + << ", id=" << step.device_id; + return; + } + + control::ControlAcquireResult barrier; + try { + barrier = authority.preemptAcquireIfCurrent( + *expected_token, owner, ttl); + } catch (...) { + (void)authority.quarantineIfCurrent(*expected_token); + record->retain_control_leases.store( + true, std::memory_order_release); + throw; + } + if (!barrier.acquired) { + // A direct Stop or another Action stop may already have converted + // our lease. Do not join that barrier and, critically, do not + // preempt a successor which acquired control after it completed. + if (authority.quarantineIfCurrent(*expected_token)) { + record->retain_control_leases.store( + true, std::memory_order_release); + const std::string detail = + "could not establish timed-out RobotArm stop barrier " + "while the Action lease remained current: " + + barrier.detail; + { + std::lock_guard lock(record->mutex); + record->stop_error = detail; + } + CMVR_LOG(ERROR) << "[ActionQueueExecutor] " << detail + << ", id=" << step.device_id; + } + return; + } + bool stop_confirmed = false; + std::string stop_error; + try { + const auto stop = step.arm->stopMotion(); + stop_confirmed = stop.ok(); + if (!stop_confirmed) { + stop_error = stop.message.empty() + ? "RobotArm stop did not confirm idle" + : stop.message; + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] timed-out RobotArm stop failed, id=" + << step.device_id << ", error=" << stop.message; + } + } catch (const std::exception& error) { + stop_error = error.what(); + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] timed-out RobotArm stop threw, id=" + << step.device_id << ", error=" << error.what(); + } catch (...) { + stop_error = "RobotArm stop threw an unknown exception"; + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] timed-out RobotArm stop threw, id=" + << step.device_id; + } + if (!stop_confirmed) { + // A user Stop/StopAll may already own the driver termination state. + // In that case this second stop call is expected to be rejected. + // Keep our safety holder while waiting for the first owner to + // publish the driver's confirmed-idle state; only quarantine if + // that bounded confirmation also fails. + stop_confirmed = waitForArmIdle( + step.arm, + kConcurrentArmStopConfirmationTimeout); + } + if (stop_confirmed) { + authority.release(barrier.token); + } else { + // Deliberately retain the safety barrier when idle was not + // confirmed. Releasing it would allow a new command to overlap an + // unknown physical outcome. Recovery requires an explicit device + // safety procedure or process restart. + std::lock_guard lock(record->mutex); + record->stop_error = + "RobotArm stop was not confirmed; control remains quarantined: " + + stop_error; + } + } + + enum class StepOutcome { + Completed, + Failed, + Canceled, + TimedOut, + }; + + struct StepResult { + StepOutcome outcome{StepOutcome::Failed}; + std::string message; + }; + + StepResult executeArmStep( + const std::shared_ptr& record, + const PreparedStep& prepared, + const api::ActionStep& source, + const control::ControlLeaseToken& token, + const Clock::time_point deadline) + { + auto& authority = + control::ControlAuthorityManager::instance(); + auto cancellation_requested = + [record, token, deadline, &authority]() { + return record->cancel_requested.load( + std::memory_order_acquire) || + Clock::now() >= deadline || + !authority.validate(token); + }; + + auto disarmed = std::make_shared>(false); + auto timeout_stop_started = + std::make_shared>(false); + std::thread watchdog( + [this, record, prepared, deadline, disarmed, + timeout_stop_started]() { + try { + armTimeoutWatchdog( + record, prepared, deadline, disarmed, + timeout_stop_started); + } catch (const std::exception& error) { + try { + std::lock_guard lock(record->mutex); + record->stop_error = + "RobotArm timeout watchdog failed: " + + std::string(error.what()); + } catch (...) { + } + try { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] RobotArm timeout " + "watchdog failed, id=" + << prepared.device_id + << ", error=" << error.what(); + } catch (...) { + } + } catch (...) { + try { + std::lock_guard lock(record->mutex); + record->stop_error = + "RobotArm timeout watchdog failed with an unknown exception"; + } catch (...) { + } + try { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] RobotArm timeout " + "watchdog failed, id=" + << prepared.device_id; + } catch (...) { + } + } + }); + + device::Result motion_result; + try { + if (prepared.kind == PreparedStepKind::ArmMoveJ) { + const auto& command = source.arm_move_j(); + device::JointPositionCommand target; + target.position.assign( + command.target().position().begin(), + command.target().position().end()); + auto options = toArmMotionOptions(command.options()); + options.cancellation_requested = cancellation_requested; + motion_result = prepared.arm->moveJ(target, options); + } else { + const auto& command = source.arm_move_l(); + const device::CartesianPose target{ + command.target().x(), command.target().y(), + command.target().z(), command.target().rx(), + command.target().ry(), command.target().rz()}; + auto options = toArmMotionOptions(command.options()); + options.cancellation_requested = cancellation_requested; + motion_result = prepared.arm->moveL( + target, options, toFrameType(command.frame())); + } + } catch (const std::exception& error) { + motion_result = device::Result::failure( + device::ArmErrorCode::CommandFailed, error.what()); + } catch (...) { + motion_result = device::Result::failure( + device::ArmErrorCode::CommandFailed, + "RobotArm command threw an unknown exception"); + } + + const bool expired = + record->timed_out.load(std::memory_order_acquire) || + Clock::now() >= deadline; + if (expired) { + // The device callback can observe the deadline and return before + // the watchdog gets scheduled. Preserve the invariant that every + // timed-out synchronous arm command goes through a typed Stop. + record->timed_out.store(true, std::memory_order_release); + requestTimedOutArmStop( + record, prepared, timeout_stop_started); + } + disarmed->store(true, std::memory_order_release); + if (watchdog.joinable()) { + watchdog.join(); + } + if (expired || + record->timed_out.load(std::memory_order_acquire)) { + std::string message = "RobotArm ActionQueue step timed out"; + { + std::lock_guard lock(record->mutex); + if (!record->stop_error.empty()) { + message += "; " + record->stop_error; + } + } + return {StepOutcome::TimedOut, + std::move(message)}; + } + if (record->cancel_requested.load(std::memory_order_acquire) || + !authority.validate(token)) { + std::string message = + "RobotArm ActionQueue step was canceled or preempted"; + if (!requestTypedStop(record)) { + message += + "; one or more Action devices did not confirm a stopped " + "state and remain quarantined"; + } + return {StepOutcome::Canceled, std::move(message)}; + } + if (!motion_result.ok()) { + std::string message = motion_result.message; + if (!requestTypedStop(record)) { + message += + "; one or more Action devices did not confirm a stopped " + "state and remain quarantined"; + } + return {StepOutcome::Failed, std::move(message)}; + } + return {StepOutcome::Completed, {}}; + } + + StepResult executeAgvStep( + const std::shared_ptr& record, + const PreparedStep& prepared, + const api::ActionStep& source, + const control::ControlLeaseToken& token, + const Clock::time_point deadline) + { + auto& authority = + control::ControlAuthorityManager::instance(); + auto cancellation_requested = + [record, token, deadline, &authority]() { + return record->cancel_requested.load( + std::memory_order_acquire) || + Clock::now() >= deadline || + !authority.validate(token); + }; + + device::AgvResult navigation_result; + try { + if (prepared.kind == PreparedStepKind::AgvNavigateToPose) { + const auto& command = source.agv_navigate_to_pose(); + auto options = toAgvMotionOptions(command.options()); + applyAgvDeadline(options, deadline); + options.cancellation_requested = cancellation_requested; + navigation_result = prepared.agv->navigateToPose( + {command.pose().x(), command.pose().y(), + command.pose().theta()}, + options, + toAgvAdapterParams(command.adapter_params())); + } else if (prepared.kind == + PreparedStepKind::AgvNavigateToStation) { + const auto& command = source.agv_navigate_to_station(); + auto options = toAgvMotionOptions(command.options()); + applyAgvDeadline(options, deadline); + options.cancellation_requested = cancellation_requested; + navigation_result = prepared.agv->navigateToStation( + command.station_id(), + options, + toAgvAdapterParams(command.adapter_params())); + } else { + const auto& command = source.agv_follow_path(); + std::vector path; + path.reserve(static_cast(command.path_size())); + for (const auto& segment : command.path()) { + path.push_back({ + segment.source_station(), + segment.target_station()}); + } + auto options = toAgvMotionOptions(command.options()); + applyAgvDeadline(options, deadline); + options.cancellation_requested = cancellation_requested; + navigation_result = prepared.agv->followPath(path, options); + } + } catch (const std::exception& error) { + navigation_result = device::AgvResult::failure( + device::AgvErrorCode::CommandFailed, error.what()); + } catch (...) { + navigation_result = device::AgvResult::failure( + device::AgvErrorCode::CommandFailed, + "AGV command threw an unknown exception"); + } + + const auto stopOrQuarantine = + [this, &record](std::string message) { + if (!requestTypedStop(record)) { + message += + "; one or more Action devices did not confirm a " + "stopped state and remain quarantined"; + } + return message; + }; + + if (Clock::now() >= deadline || + navigation_result.code == device::AgvErrorCode::Timeout) { + record->timed_out.store(true, std::memory_order_release); + return {StepOutcome::TimedOut, + stopOrQuarantine( + navigation_result.message.empty() + ? "AGV ActionQueue step timed out" + : navigation_result.message)}; + } + if (record->cancel_requested.load(std::memory_order_acquire) || + !authority.validate(token) || + navigation_result.code == device::AgvErrorCode::TaskCanceled) { + return {StepOutcome::Canceled, + stopOrQuarantine( + navigation_result.message.empty() + ? "AGV ActionQueue step was canceled or preempted" + : navigation_result.message)}; + } + if (!navigation_result.ok()) { + return {StepOutcome::Failed, + stopOrQuarantine(navigation_result.message)}; + } + return {StepOutcome::Completed, {}}; + } + + static void applyAgvDeadline( + device::AgvMotionOptions& options, + const Clock::time_point deadline) + { + const auto remaining = std::max( + 1, + std::chrono::duration_cast( + deadline - Clock::now()).count()); + const int bounded = static_cast(std::min( + remaining, + std::numeric_limits::max())); + if (options.wait_timeout_ms <= 0 || + options.wait_timeout_ms > bounded) { + options.wait_timeout_ms = bounded; + } + if (options.poll_interval_ms > options.wait_timeout_ms) { + options.poll_interval_ms = options.wait_timeout_ms; + } + } + + StepResult executeDelay( + const std::shared_ptr& record, + const api::ActionStep& step, + const Clock::time_point deadline) + { + const auto delay_deadline = std::min( + deadline, + Clock::now() + Milliseconds(step.delay().duration_ms())); + std::unique_lock lock(record->mutex); + const bool canceled = record->condition.wait_until( + lock, delay_deadline, [&record]() { + return record->cancel_requested.load( + std::memory_order_acquire); + }); + if (canceled) { + return {StepOutcome::Canceled, + "ActionQueue delay was canceled"}; + } + if (Clock::now() >= deadline) { + record->timed_out.store(true, std::memory_order_release); + return {StepOutcome::TimedOut, + "ActionQueue delay timed out"}; + } + return {StepOutcome::Completed, {}}; + } + + void execute(const std::shared_ptr& record) + { + if (record->cancel_requested.load(std::memory_order_acquire)) { + complete(record, api::ACTION_RESULT_CODE_CANCELED, 0, + "ActionQueue was canceled before execution"); + return; + } + if (Clock::now() >= record->deadline) { + complete(record, api::ACTION_RESULT_CODE_TIMED_OUT, 0, + "ActionQueue expired while waiting in the queue"); + return; + } + + LeaseSet leases(record); + std::string lease_error; + if (!acquireLeases(record, leases, lease_error)) { + if (record->timed_out.load(std::memory_order_acquire)) { + complete(record, api::ACTION_RESULT_CODE_TIMED_OUT, 0, + lease_error); + } else { + complete(record, api::ACTION_RESULT_CODE_CANCELED, 0, + lease_error); + } + return; + } + if (!validateLeases(leases)) { + complete(record, api::ACTION_RESULT_CODE_CANCELED, 0, + "ActionQueue device control was preempted before execution"); + return; + } + + std::string dynamic_error; + if (!validateDevicesIdle(record, dynamic_error)) { + complete(record, api::ACTION_RESULT_CODE_REJECTED, 0, + dynamic_error); + return; + } + + std::uint32_t completed_steps = 0; + for (int index = 0; index < record->request.steps_size(); ++index) { + if (record->cancel_requested.load(std::memory_order_acquire) || + !validateLeases(leases)) { + complete( + record, api::ACTION_RESULT_CODE_CANCELED, + completed_steps, + "ActionQueue was canceled or device control was preempted"); + return; + } + if (Clock::now() >= record->deadline) { + complete( + record, api::ACTION_RESULT_CODE_TIMED_OUT, + completed_steps, + "ActionQueue total timeout elapsed"); + return; + } + + const auto& source = record->request.steps(index); + const auto& prepared = record->prepared.steps[ + static_cast(index)]; + const auto deadline = stepDeadline(record, source); + StepResult step_result; + if (prepared.kind == PreparedStepKind::Delay) { + record->active_step_index.store( + -1, std::memory_order_release); + step_result = executeDelay(record, source, deadline); + } else { + const auto* token = leases.find(prepared.device_id); + if (!token) { + complete( + record, api::ACTION_RESULT_CODE_FAILED, + completed_steps, + "ActionQueue internal device reservation is missing", + index); + return; + } + if (prepared.arm) { + record->active_step_index.store( + index, std::memory_order_release); + step_result = executeArmStep( + record, prepared, source, *token, deadline); + } else { + if (!prepared.agv->supportsSynchronousAction( + toAgvActionKind(prepared.kind))) { + complete( + record, api::ACTION_RESULT_CODE_REJECTED, + completed_steps, + "AGV ActionQueue capability changed before execution", + index); + return; + } + record->active_step_index.store( + index, std::memory_order_release); + step_result = executeAgvStep( + record, prepared, source, *token, deadline); + } + record->active_step_index.store( + -1, std::memory_order_release); + } + + switch (step_result.outcome) { + case StepOutcome::Completed: + ++completed_steps; + break; + case StepOutcome::Failed: + complete( + record, api::ACTION_RESULT_CODE_FAILED, + completed_steps, step_result.message, index); + return; + case StepOutcome::Canceled: + complete( + record, api::ACTION_RESULT_CODE_CANCELED, + completed_steps, step_result.message, index); + return; + case StepOutcome::TimedOut: + complete( + record, api::ACTION_RESULT_CODE_TIMED_OUT, + completed_steps, step_result.message, index); + return; + } + } + + complete(record, api::ACTION_RESULT_CODE_COMPLETED, + completed_steps, {}); + CMVR_LOG(DEBUG) + << "[ActionQueueExecutor] action completed, id=" + << record->request.action_id() + << ", steps=" << completed_steps; + } + + device::DeviceManager& device_manager; + const std::string instance_id; + const std::size_t max_accepted_action_ids; + std::mutex mutex; + std::condition_variable queue_condition; + std::condition_variable idle_condition; + std::deque> queue; + std::unordered_map> records; + std::unordered_map terminal_results; + std::deque terminal_result_order; + std::unordered_set retired_action_ids; + std::shared_ptr active; + bool accepting{true}; + bool stopping{false}; + bool joined{false}; + std::atomic sequence{0}; + std::atomic concurrent_submitters{0}; + std::thread worker; +}; + +ActionQueueExecutor::ActionQueueExecutor( + device::DeviceManager& device_manager, + const std::size_t max_accepted_action_ids) + : impl_(std::make_unique( + device_manager, max_accepted_action_ids)) +{ +} + +ActionQueueExecutor::~ActionQueueExecutor() = default; + +ActionQueueExecutor::WaitResult ActionQueueExecutor::submitAndWait( + const api::ActionQueueCommand_Request& request, + api::ActionQueueCommand_Feedback& feedback, + const std::function& waiter_canceled) +{ + const auto result = + impl_->submitAndWait(request, feedback, waiter_canceled); + feedback.set_service_instance_id(impl_->instance_id); + return result; +} + +bool ActionQueueExecutor::cancelAllAndDisable() +{ + return impl_->cancelAllAndDisable(); +} + +bool ActionQueueExecutor::waitForIdle( + const std::chrono::milliseconds timeout) +{ + return impl_->waitForIdle(timeout); +} + +const std::string& ActionQueueExecutor::instanceId() const noexcept +{ + return impl_->instance_id; +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/include/grpc_system_service.h b/cmvr-es/service/grpc/include/grpc_system_service.h index 978ca2f4..f579aba6 100644 --- a/cmvr-es/service/grpc/include/grpc_system_service.h +++ b/cmvr-es/service/grpc/include/grpc_system_service.h @@ -5,23 +5,33 @@ #ifndef GRPC_SYSTEM_SERVICE_H #define GRPC_SYSTEM_SERVICE_H +#include + #include "cmvr/api/system_service.grpc.pb.h" #include "common/base/grpc_utils.h" #include "manager/device_manager/include/device_manager.h" namespace cmvr::service { + class ActionQueueExecutor; + class gRPCSystemServiceImpl: public api::SystemService::Service { public: gRPCSystemServiceImpl(); - ~gRPCSystemServiceImpl() override = default; + ~gRPCSystemServiceImpl() override; + // 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. + void prepareForShutdown(); grpc::Status GetSystemInfo(grpc::ServerContext* context, const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response) override; grpc::Status GetSystemStatus(grpc::ServerContext* context, const api::GetSystemStatusCommand_Request* request, api::GetSystemStatusCommand_Feedback* response) override; grpc::Status GetDeviceList(grpc::ServerContext* context, const api::GetDeviceListCommand_Request* request, api::GetDeviceListCommand_Feedback* response) override; grpc::Status UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) override; grpc::Status StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) override; + grpc::Status ExecuteActionQueue(grpc::ServerContext* context, const cmvr::api::ActionQueueCommand_Request* request, cmvr::api::ActionQueueCommand_Feedback* response) override; private: device::DeviceManager& dmgr_; + std::unique_ptr action_queue_; }; } diff --git a/cmvr-es/service/grpc/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/src/grpc_agv_service.cpp index 4615e83c..b905593f 100644 --- a/cmvr-es/service/grpc/src/grpc_agv_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_agv_service.cpp @@ -1,12 +1,18 @@ #include "service/grpc/include/grpc_agv_service.h" +#include +#include #include #include #include +#include #include #include +#include "common/base/logging/logger.h" +#include "manager/control_authority/include/control_authority_manager.h" + using google::protobuf::util::TimeUtil; namespace cmvr::service { @@ -75,6 +81,114 @@ grpc::Status setDeviceNotFound(api::CommandHeader_Feedback* response, const std: return grpc::Status(grpc::StatusCode::NOT_FOUND, message); } +grpc::Status setControlLeaseConflict( + api::CommandHeader_Feedback* response, + const std::string& device_id, + const std::string& detail) +{ + const std::string message = + "AGV control is leased by another active control operation: " + + device_id; + if (!detail.empty()) { + CMVR_LOG(WARNING) << "[gRPCAgvServiceImpl] control lease conflict, id=" + << device_id << ", detail=" << detail; + } + fillFeedback(response, false, message); + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, message); +} + +template +grpc::Status setControlLeaseConflict( + Response* response, + const std::string& device_id, + const std::string& detail) +{ + return setControlLeaseConflict( + response->mutable_header(), device_id, detail); +} + +class ScopedUnaryAgvControlLease final { +public: + ScopedUnaryAgvControlLease( + const std::string& device_id, + const char* operation, + const bool preemptive = false) + : manager_(control::ControlAuthorityManager::instance()) + { + static std::atomic sequence{0}; + const std::string owner = + std::string(preemptive + ? "grpc-agv-safety:" + : "grpc-agv-unary:") + + operation + ":" + + std::to_string( + sequence.fetch_add( + 1U, std::memory_order_relaxed) + + 1U); + 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); + } + + ~ScopedUnaryAgvControlLease() + { + if (release_on_destroy_) { + manager_.release(token_); + } + } + + bool acquired() const noexcept { return acquired_; } + const std::string& detail() const noexcept { return detail_; } + + // Unknown physical outcomes stay fail-closed until an explicit device + // safety procedure or process restart clears the retained holder. + void quarantine() noexcept { release_on_destroy_ = false; } + +private: + control::ControlAuthorityManager& manager_; + control::ControlLeaseToken token_; + std::string detail_; + bool acquired_{false}; + bool release_on_destroy_{true}; +}; + +template +grpc::Status executeConfirmedAgvStop( + Response* response, + const std::shared_ptr& agv, + ScopedUnaryAgvControlLease& control_barrier, + const char* operation_name, + Operation&& operation) +{ + try { + const auto command_result = operation(); + const auto stopped = agv->confirmMotionStopped(); + if (!stopped.ok()) { + control_barrier.quarantine(); + std::string message = std::string(operation_name) + + " did not reach a confirmed stopped state: " + + stopped.message; + if (!command_result.ok()) { + message += "; command_result=" + command_result.message; + } + return setResponseResult( + response, + device::AgvResult::failure(stopped.code, message)); + } + return setResponseResult(response, command_result); + } catch (...) { + control_barrier.quarantine(); + throw; + } +} + template grpc::Status setNavigationRequestCanceled(Response* response) { @@ -409,11 +523,20 @@ grpc::Status gRPCAgvServiceImpl::emergencyStop(grpc::ServerContext*, api::CommandHeader_Feedback* response) { try { - auto agv = dmgr_.getDevice(request->device_id()); + const std::string device_id = request->device_id(); + auto agv = dmgr_.getDevice(device_id); if (!agv) { - return setDeviceNotFound(response, request->device_id()); + return setDeviceNotFound(response, device_id); } - return setResponseResult(response, agv->emergencyStop()); + ScopedUnaryAgvControlLease control_barrier( + device_id, "emergencyStop", true); + if (!control_barrier.acquired()) { + return setControlLeaseConflict( + response, device_id, control_barrier.detail()); + } + return executeConfirmedAgvStop( + response, agv, control_barrier, "emergencyStop", + [&agv]() { return agv->emergencyStop(); }); } catch (const std::exception& e) { fillFeedback(response, false, e.what()); return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); @@ -425,9 +548,16 @@ grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext*, api::CommandHeader_Feedback* response) { try { - auto agv = dmgr_.getDevice(request->device_id()); + const std::string device_id = request->device_id(); + auto agv = dmgr_.getDevice(device_id); if (!agv) { - return setDeviceNotFound(response, request->device_id()); + return setDeviceNotFound(response, device_id); + } + ScopedUnaryAgvControlLease control_lease( + device_id, "clearFault"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } return setResponseResult(response, agv->clearFault()); } catch (const std::exception& e) { @@ -449,6 +579,12 @@ grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext* context, if (!agv) { return setDeviceNotFound(response, device_id); } + ScopedUnaryAgvControlLease control_lease( + device_id, "navigateToPose"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); + } return setResponseResult(response, agv->navigateToPose( toPose2d(request->pose()), toMotionOptions(request->options(), context), @@ -472,6 +608,12 @@ grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext* context, if (!agv) { return setDeviceNotFound(response, device_id); } + ScopedUnaryAgvControlLease control_lease( + device_id, "navigateToStation"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); + } return setResponseResult(response, agv->navigateToStation( request->station_id(), toMotionOptions(request->options(), context), @@ -495,6 +637,12 @@ grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext* context, if (!agv) { return setDeviceNotFound(response, device_id); } + ScopedUnaryAgvControlLease control_lease( + device_id, "followPath"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); + } std::vector path; path.reserve(static_cast(request->path_size())); for (const auto& segment : request->path()) { @@ -527,6 +675,12 @@ grpc::Status gRPCAgvServiceImpl::translate( if (!agv) { return setDeviceNotFound(response, device_id); } + ScopedUnaryAgvControlLease control_lease( + device_id, "translate"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); + } return setResponseResult( response, agv->translate(toTranslation(request->translation()))); @@ -545,9 +699,16 @@ grpc::Status gRPCAgvServiceImpl::pauseNavigation(grpc::ServerContext*, api::CommandHeader_Feedback* response) { try { - auto agv = dmgr_.getDevice(request->device_id()); + const std::string device_id = request->device_id(); + auto agv = dmgr_.getDevice(device_id); if (!agv) { - return setDeviceNotFound(response, request->device_id()); + return setDeviceNotFound(response, device_id); + } + ScopedUnaryAgvControlLease control_lease( + device_id, "pauseNavigation"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } return setResponseResult(response, agv->pauseNavigation()); } catch (const std::exception& e) { @@ -561,9 +722,16 @@ grpc::Status gRPCAgvServiceImpl::resumeNavigation(grpc::ServerContext*, api::CommandHeader_Feedback* response) { try { - auto agv = dmgr_.getDevice(request->device_id()); + const std::string device_id = request->device_id(); + auto agv = dmgr_.getDevice(device_id); if (!agv) { - return setDeviceNotFound(response, request->device_id()); + return setDeviceNotFound(response, device_id); + } + ScopedUnaryAgvControlLease control_lease( + device_id, "resumeNavigation"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } return setResponseResult(response, agv->resumeNavigation()); } catch (const std::exception& e) { @@ -577,11 +745,20 @@ grpc::Status gRPCAgvServiceImpl::cancelNavigation(grpc::ServerContext*, api::CommandHeader_Feedback* response) { try { - auto agv = dmgr_.getDevice(request->device_id()); + const std::string device_id = request->device_id(); + auto agv = dmgr_.getDevice(device_id); if (!agv) { - return setDeviceNotFound(response, request->device_id()); + return setDeviceNotFound(response, device_id); } - return setResponseResult(response, agv->cancelNavigation()); + ScopedUnaryAgvControlLease control_barrier( + device_id, "cancelNavigation", true); + if (!control_barrier.acquired()) { + return setControlLeaseConflict( + response, device_id, control_barrier.detail()); + } + return executeConfirmedAgvStop( + response, agv, control_barrier, "cancelNavigation", + [&agv]() { return agv->cancelNavigation(); }); } catch (const std::exception& e) { fillFeedback(response, false, e.what()); return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); @@ -598,6 +775,12 @@ grpc::Status gRPCAgvServiceImpl::setVelocity(grpc::ServerContext*, if (!agv) { return setDeviceNotFound(response, device_id); } + ScopedUnaryAgvControlLease control_lease( + device_id, "setVelocity"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); + } return setResponseResult(response, agv->setVelocity(toVelocity(request->velocity()))); } catch (const std::exception& e) { fillFeedback(response->mutable_header(), false, e.what()); @@ -610,11 +793,20 @@ grpc::Status gRPCAgvServiceImpl::stopVelocityControl(grpc::ServerContext*, api::CommandHeader_Feedback* response) { try { - auto agv = dmgr_.getDevice(request->device_id()); + const std::string device_id = request->device_id(); + auto agv = dmgr_.getDevice(device_id); if (!agv) { - return setDeviceNotFound(response, request->device_id()); + return setDeviceNotFound(response, device_id); } - return setResponseResult(response, agv->stopVelocityControl()); + ScopedUnaryAgvControlLease control_barrier( + device_id, "stopVelocityControl", true); + if (!control_barrier.acquired()) { + return setControlLeaseConflict( + response, device_id, control_barrier.detail()); + } + return executeConfirmedAgvStop( + response, agv, control_barrier, "stopVelocityControl", + [&agv]() { return agv->stopVelocityControl(); }); } catch (const std::exception& e) { fillFeedback(response, false, e.what()); return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); @@ -679,6 +871,12 @@ grpc::Status gRPCAgvServiceImpl::switchMap(grpc::ServerContext*, if (!agv) { return setDeviceNotFound(response, device_id); } + ScopedUnaryAgvControlLease control_lease( + device_id, "switchMap"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); + } return setResponseResult(response, agv->switchMap(request->map_name())); } catch (const std::exception& e) { fillFeedback(response->mutable_header(), false, e.what()); @@ -696,6 +894,12 @@ grpc::Status gRPCAgvServiceImpl::uploadMap(grpc::ServerContext*, if (!agv) { return setDeviceNotFound(response, device_id); } + ScopedUnaryAgvControlLease control_lease( + device_id, "uploadMap"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); + } return setResponseResult(response, agv->uploadMap(request->map_name(), request->content())); } catch (const std::exception& e) { fillFeedback(response->mutable_header(), false, e.what()); @@ -735,6 +939,12 @@ grpc::Status gRPCAgvServiceImpl::startMapping(grpc::ServerContext*, if (!agv) { return setDeviceNotFound(response, device_id); } + ScopedUnaryAgvControlLease control_lease( + device_id, "startMapping"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); + } device::AgvMappingOptions options; options.dimension = toMapDimension(request->dimension()); options.map_name = request->map_name(); @@ -825,9 +1035,16 @@ grpc::Status gRPCAgvServiceImpl::stopMapping(grpc::ServerContext*, api::CommandHeader_Feedback* response) { try { - auto agv = dmgr_.getDevice(request->device_id()); + const std::string device_id = request->device_id(); + auto agv = dmgr_.getDevice(device_id); if (!agv) { - return setDeviceNotFound(response, request->device_id()); + return setDeviceNotFound(response, device_id); + } + ScopedUnaryAgvControlLease control_lease( + device_id, "stopMapping"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } return setResponseResult(response, agv->stopMapping()); } catch (const std::exception& e) { diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index a8fb5622..ed635ce3 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -175,21 +175,26 @@ public: acquired_ = acquired.acquired; token_ = std::move(acquired.token); detail_ = std::move(acquired.detail); + release_on_destroy_ = !preemptive; } ~ScopedUnaryControlLease() { - manager_.release(token_); + if (release_on_destroy_) { + manager_.release(token_); + } } bool acquired() const noexcept { return acquired_; } const std::string& detail() const noexcept { return detail_; } + void confirmSafeToRelease() noexcept { release_on_destroy_ = true; } private: control::ControlAuthorityManager& manager_; control::ControlLeaseToken token_; std::string detail_; bool acquired_{false}; + bool release_on_destroy_{true}; }; } // namespace @@ -216,6 +221,9 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*, response, device_id, control_barrier.detail()); } const auto result = arm->torqueOff(); + if (result.ok()) { + control_barrier.confirmSafeToRelease(); + } fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { logRpcSuccess("torqueOff", device_id); @@ -424,6 +432,9 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*, response, device_id, control_barrier.detail()); } const auto result = arm->stopMotion(); + if (result.ok()) { + control_barrier.confirmSafeToRelease(); + } fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { logRpcSuccess("stopMotion", 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 87c592cc..f67a72c9 100644 --- a/cmvr-es/service/grpc/src/grpc_system_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_system_service.cpp @@ -12,7 +12,10 @@ #include #include "common/base/logging/logger.h" +#include "devices/agv/abstract_agv.h" +#include "devices/arm/robot_arm.h" #include "manager/control_authority/include/control_authority_manager.h" +#include "service/action/include/action_queue_executor.h" using namespace cmvr::device; using namespace cmvr::service; @@ -25,8 +28,10 @@ public: { auto& authority = cmvr::control::ControlAuthorityManager::instance(); - for (const auto& token : tokens_) { - authority.release(token); + for (const auto& barrier : barriers_) { + if (barrier.release_on_destroy) { + authority.release(barrier.token); + } } } @@ -49,18 +54,44 @@ public: detail = result.detail; return false; } - try { - tokens_.push_back(result.token); - } catch (...) { - cmvr::control::ControlAuthorityManager::instance().release( - result.token); - throw; - } + // StopAll barriers default to fail-closed. If retaining the token in + // this local vector throws, deliberately leave the manager-side safety + // holder installed: releasing it would reopen control after StopAll + // already preempted an in-flight command. + barriers_.push_back({result.token, false}); return true; } + void quarantine(const std::string& device_id) + { + for (auto& barrier : barriers_) { + if (barrier.token.resource_id == device_id) { + barrier.release_on_destroy = false; + } + } + } + + void quarantineAll() + { + for (auto& barrier : barriers_) { + barrier.release_on_destroy = false; + } + } + + void confirmSafeToReleaseAll() + { + for (auto& barrier : barriers_) { + barrier.release_on_destroy = true; + } + } + private: - std::vector tokens_; + struct Barrier { + cmvr::control::ControlLeaseToken token; + bool release_on_destroy{false}; + }; + + std::vector barriers_; }; std::uint64_t unixTimeMs() noexcept @@ -154,7 +185,18 @@ cmvr::api::SystemDeviceHealth toApiDeviceHealth( } // namespace -gRPCSystemServiceImpl::gRPCSystemServiceImpl(): dmgr_(DeviceManager::getInstance()) {} +gRPCSystemServiceImpl::gRPCSystemServiceImpl() + : dmgr_(DeviceManager::getInstance()), + action_queue_(std::make_unique(dmgr_)) +{ +} + +gRPCSystemServiceImpl::~gRPCSystemServiceImpl() = default; + +void gRPCSystemServiceImpl::prepareForShutdown() +{ + (void)action_queue_->cancelAllAndDisable(); +} grpc::Status gRPCSystemServiceImpl::GetSystemInfo(grpc::ServerContext* context, const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response) @@ -162,6 +204,8 @@ grpc::Status gRPCSystemServiceImpl::GetSystemInfo(grpc::ServerContext* context, try { response->set_version(dmgr_.version()); response->set_system_name(dmgr_.name()); + response->set_action_service_instance_id( + action_queue_->instanceId()); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (GetSystemInfo): success, name=" @@ -287,27 +331,132 @@ grpc::Status gRPCSystemServiceImpl::UpdateParams(grpc::ServerContext* context, c grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) { + (void)context; + (void)request; try { const auto snapshot = dmgr_.snapshot(); ScopedControlBarrierSet control_barriers; for (const auto& device : snapshot.devices) { - if (device.kind == cmvr::device::DeviceKind::Arm) { + if (device.kind == cmvr::device::DeviceKind::Arm || + device.kind == cmvr::device::DeviceKind::AGV) { std::string detail; if (!control_barriers.acquire(device.id, detail)) { CMVR_LOG(WARNING) << "[gRPCSystemServiceImpl] (StopAll): failed to " - "acquire arm safety barrier, id=" + "acquire device safety barrier, id=" << device.id << ", detail=" << detail; throw std::runtime_error( - "StopAll could not acquire the RobotArm safety " + "StopAll could not acquire the device safety " "barrier: " + device.id); } } } + const bool action_stop_confirmed = + action_queue_->cancelAllAndDisable(); + if (!action_stop_confirmed) { + control_barriers.quarantineAll(); + } + std::vector unconfirmed_devices; + for (const auto& device : snapshot.devices) { + if (device.kind != cmvr::device::DeviceKind::Arm) { + continue; + } + auto arm = dmgr_.getDevice(device.id); + if (!arm) { + continue; + } + try { + const auto stopped = arm->stopMotion(); + if (!stopped.ok()) { + control_barriers.quarantine(device.id); + unconfirmed_devices.push_back( + device.id + ": " + stopped.message); + } + } catch (const std::exception& error) { + control_barriers.quarantine(device.id); + unconfirmed_devices.push_back( + device.id + ": stop threw: " + error.what()); + } catch (...) { + control_barriers.quarantine(device.id); + unconfirmed_devices.push_back( + device.id + ": stop threw an unknown exception"); + } + } + + // AbstractAGV::stop() is a lifecycle hook and some backends do not + // map it to a motion stop. Use the typed non-E-stop controls here; + // StopAll must not be silently upgraded to emergencyStop semantics. + for (const auto& device : snapshot.devices) { + if (device.kind != cmvr::device::DeviceKind::AGV) { + continue; + } + auto agv = dmgr_.getDevice( + device.id); + if (!agv) { + continue; + } + try { + const auto cancel_result = agv->cancelNavigation(); + if (!cancel_result.ok() && + cancel_result.code != + cmvr::device::AgvErrorCode::UnsupportedCommand) { + CMVR_LOG(WARNING) + << "[gRPCSystemServiceImpl] (StopAll): AGV navigation " + "cancel failed, id=" + << device.id << ", detail=" << cancel_result.message; + } + const auto velocity_stop = agv->stopVelocityControl(); + if (!velocity_stop.ok() && + velocity_stop.code != + cmvr::device::AgvErrorCode::UnsupportedCommand) { + CMVR_LOG(WARNING) + << "[gRPCSystemServiceImpl] (StopAll): AGV velocity " + "stop failed, id=" + << device.id << ", detail=" + << velocity_stop.message; + } + const auto stopped = agv->confirmMotionStopped(); + if (!stopped.ok()) { + control_barriers.quarantine(device.id); + unconfirmed_devices.push_back( + device.id + ": " + stopped.message); + } + } catch (const std::exception& error) { + control_barriers.quarantine(device.id); + unconfirmed_devices.push_back( + device.id + ": stop confirmation threw: " + + error.what()); + } catch (...) { + control_barriers.quarantine(device.id); + unconfirmed_devices.push_back( + device.id + + ": stop confirmation threw an unknown exception"); + } + } + if (!action_queue_->waitForIdle(std::chrono::seconds(15))) { + control_barriers.quarantineAll(); + throw std::runtime_error( + "StopAll timed out waiting for ActionQueue to become idle"); + } + if (!action_stop_confirmed) { + throw std::runtime_error( + "StopAll could not confirm that every active ActionQueue " + "device stopped; affected control resources remain " + "quarantined"); + } + if (!unconfirmed_devices.empty()) { + throw std::runtime_error( + "StopAll could not confirm that every device stopped; " + "affected control resources remain quarantined: " + + unconfirmed_devices.front()); + } + // Do not close device transports while the Action worker may still be + // unwinding a synchronous driver call. dmgr_.stop(); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (StopAll): success"; + control_barriers.confirmSafeToReleaseAll(); return grpc::Status::OK; } catch (std::exception& e) { @@ -317,3 +466,49 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, return grpc::Status::OK; } } + +grpc::Status gRPCSystemServiceImpl::ExecuteActionQueue( + grpc::ServerContext* context, + const cmvr::api::ActionQueueCommand_Request* request, + cmvr::api::ActionQueueCommand_Feedback* response) +{ + if (!request || !response) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "ActionQueue request and response are required"); + } + try { + // The callback is consumed only on this synchronous handler stack. It + // is never retained by the worker-owned action Record, so returning the + // RPC cannot leave a dangling ServerContext reference. + const auto wait_result = action_queue_->submitAndWait( + *request, + *response, + [context]() { + return context && context->IsCancelled(); + }); + if (wait_result == + ActionQueueExecutor::WaitResult::CanceledBeforeAdmission) { + return grpc::Status( + grpc::StatusCode::CANCELLED, + "ActionQueue RPC was canceled before admission"); + } + if (wait_result == + ActionQueueExecutor::WaitResult::CanceledAfterAdmission) { + return grpc::Status( + grpc::StatusCode::CANCELLED, + "ActionQueue RPC waiter was canceled after admission; the edge action continues and its result can be retrieved with the same action_id"); + } + return grpc::Status::OK; + } catch (const std::exception& error) { + response->Clear(); + response->set_action_id(request->action_id()); + response->set_service_instance_id(action_queue_->instanceId()); + response->set_result(api::ACTION_RESULT_CODE_FAILED); + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message(error.what()); + setCurrentTimestamp( + response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; + } +} diff --git a/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp index ff144764..e6c60806 100644 --- a/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp @@ -1,5 +1,6 @@ #include "service/grpc/include/grpc_agv_service.h" +#include #include #include #include @@ -8,6 +9,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 { @@ -27,6 +29,14 @@ public: std::string typeName() const override { return "FakeAgv"; } + device::AgvResult emergencyStop() override + { + ++emergency_stop_calls_; + emergency_stop_barrier_observed_ = normalLeaseIsBlocked_( + "test-probe:emergencyStop"); + return device::AgvResult::success(); + } + device::AgvResult navigateToPose( const math::Pose2d& pose, const device::AgvMotionOptions& options, @@ -79,6 +89,41 @@ public: kNativeErrorMessage); } + device::AgvResult cancelNavigation() override + { + ++cancel_navigation_calls_; + cancel_navigation_barrier_observed_ = normalLeaseIsBlocked_( + "test-probe:cancelNavigation"); + return device::AgvResult::success(); + } + + device::AgvResult stopVelocityControl() override + { + ++stop_velocity_calls_; + stop_velocity_barrier_observed_ = normalLeaseIsBlocked_( + "test-probe:stopVelocityControl"); + return device::AgvResult::success(); + } + + device::AgvResult confirmMotionStopped() override + { + ++confirm_stopped_calls_; + confirm_stopped_barrier_observed_ = normalLeaseIsBlocked_( + "test-probe:confirmMotionStopped"); + return confirm_stopped_result_; + } + + bool normalLeaseIsBlocked_(const std::string& owner) + { + auto& authority = control::ControlAuthorityManager::instance(); + const auto probe = authority.tryAcquire( + id_, owner, std::chrono::hours(1)); + if (probe.acquired) { + authority.release(probe.token); + } + return !probe.acquired; + } + math::Pose2d pose_; device::AgvMotionOptions pose_options_; device::AgvResult pose_result_{device::AgvResult::success()}; @@ -93,6 +138,16 @@ public: bool station_cancellation_requested_during_call_{false}; bool path_cancellation_bound_{false}; bool path_cancellation_requested_during_call_{false}; + int emergency_stop_calls_{0}; + int cancel_navigation_calls_{0}; + int stop_velocity_calls_{0}; + int confirm_stopped_calls_{0}; + bool emergency_stop_barrier_observed_{false}; + bool cancel_navigation_barrier_observed_{false}; + bool stop_velocity_barrier_observed_{false}; + bool confirm_stopped_barrier_observed_{false}; + device::AgvResult confirm_stopped_result_{ + device::AgvResult::success()}; }; class LegacyFollowPathAgv final : public device::AbstractAGV { @@ -113,6 +168,7 @@ class GrpcAgvServiceTest : public ::testing::Test { protected: void SetUp() override { + control::ControlAuthorityManager::instance().clear(); config::DeviceManagerConfig config; auto& manager = device::DeviceManager::getInstance(config); agv_ = std::make_shared(); @@ -125,6 +181,7 @@ protected: service_.reset(); agv_.reset(); device::DeviceManager::destroyInstance(); + control::ControlAuthorityManager::instance().clear(); } std::shared_ptr agv_; @@ -293,6 +350,171 @@ TEST_F(GrpcAgvServiceTest, ExplicitAsynchronousNavigationIsForwarded) EXPECT_FALSE(agv_->station_cancellation_requested_during_call_); } +TEST_F(GrpcAgvServiceTest, ActionLeaseBlocksOrdinaryMutatingRpcs) +{ + auto& authority = control::ControlAuthorityManager::instance(); + const auto action_lease = authority.tryAcquire( + "test-agv", + "action-sequence:test-action", + std::chrono::hours(1)); + ASSERT_TRUE(action_lease.acquired) << action_lease.detail; + + api::AgvNavigateToPoseCommand_Request navigation_request; + navigation_request.mutable_header()->set_device_id("test-agv"); + api::AgvNavigateToPoseCommand_Feedback navigation_response; + grpc::ServerContext navigation_context; + const auto navigation_status = service_->navigateToPose( + &navigation_context, + &navigation_request, + &navigation_response); + EXPECT_EQ( + navigation_status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_FALSE(navigation_response.header().success()); + + api::CommandHeader_Request clear_fault_request; + clear_fault_request.set_device_id("test-agv"); + api::CommandHeader_Feedback clear_fault_response; + grpc::ServerContext clear_fault_context; + const auto clear_fault_status = service_->clearFault( + &clear_fault_context, + &clear_fault_request, + &clear_fault_response); + EXPECT_EQ( + clear_fault_status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_FALSE(clear_fault_response.success()); + + api::AgvMapCommand_Request switch_map_request; + switch_map_request.mutable_header()->set_device_id("test-agv"); + switch_map_request.set_map_name("map-1"); + api::AgvMapCommand_Feedback switch_map_response; + grpc::ServerContext switch_map_context; + const auto switch_map_status = service_->switchMap( + &switch_map_context, + &switch_map_request, + &switch_map_response); + EXPECT_EQ( + switch_map_status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_FALSE(switch_map_response.header().success()); + + authority.release(action_lease.token); +} + +TEST_F(GrpcAgvServiceTest, QueriesBypassAndSafetyStopsPreemptActionLease) +{ + auto& authority = control::ControlAuthorityManager::instance(); + const auto action_lease = authority.tryAcquire( + "test-agv", + "action-sequence:test-action", + std::chrono::hours(1)); + ASSERT_TRUE(action_lease.acquired) << action_lease.detail; + + api::AgvRuntimeStateCommand_Request state_request; + state_request.mutable_header()->set_device_id("test-agv"); + api::AgvRuntimeStateCommand_Feedback state_response; + grpc::ServerContext state_context; + const auto state_status = service_->getRuntimeState( + &state_context, + &state_request, + &state_response); + EXPECT_TRUE(state_status.ok()) << state_status.error_message(); + EXPECT_TRUE(state_response.header().success()); + EXPECT_TRUE(authority.validate(action_lease.token)); + + api::CommandHeader_Request stop_request; + stop_request.set_device_id("test-agv"); + + api::CommandHeader_Feedback emergency_response; + grpc::ServerContext emergency_context; + const auto emergency_status = service_->emergencyStop( + &emergency_context, + &stop_request, + &emergency_response); + EXPECT_TRUE(emergency_status.ok()) << emergency_status.error_message(); + EXPECT_FALSE(authority.validate(action_lease.token)); + EXPECT_TRUE(agv_->emergency_stop_barrier_observed_); + + const auto lease_after_emergency = authority.tryAcquire( + "test-agv", + "action-sequence:after-emergency", + std::chrono::hours(1)); + ASSERT_TRUE(lease_after_emergency.acquired) + << lease_after_emergency.detail; + + api::CommandHeader_Feedback cancel_response; + grpc::ServerContext cancel_context; + const auto cancel_status = service_->cancelNavigation( + &cancel_context, + &stop_request, + &cancel_response); + EXPECT_TRUE(cancel_status.ok()) << cancel_status.error_message(); + EXPECT_FALSE(authority.validate(lease_after_emergency.token)); + EXPECT_TRUE(agv_->cancel_navigation_barrier_observed_); + + const auto lease_after_cancel = authority.tryAcquire( + "test-agv", + "action-sequence:after-cancel", + std::chrono::hours(1)); + ASSERT_TRUE(lease_after_cancel.acquired) + << lease_after_cancel.detail; + + api::CommandHeader_Feedback velocity_response; + grpc::ServerContext velocity_context; + const auto velocity_status = service_->stopVelocityControl( + &velocity_context, + &stop_request, + &velocity_response); + EXPECT_TRUE(velocity_status.ok()) << velocity_status.error_message(); + EXPECT_FALSE(authority.validate(lease_after_cancel.token)); + EXPECT_TRUE(agv_->stop_velocity_barrier_observed_); + + EXPECT_EQ(agv_->emergency_stop_calls_, 1); + EXPECT_EQ(agv_->cancel_navigation_calls_, 1); + EXPECT_EQ(agv_->stop_velocity_calls_, 1); + EXPECT_EQ(agv_->confirm_stopped_calls_, 3); + EXPECT_TRUE(agv_->confirm_stopped_barrier_observed_); + + const auto lease_after_stops = authority.tryAcquire( + "test-agv", + "action-sequence:after-stops", + std::chrono::hours(1)); + ASSERT_TRUE(lease_after_stops.acquired) + << lease_after_stops.detail; + authority.release(lease_after_stops.token); +} + +TEST_F(GrpcAgvServiceTest, + SafetyStopQuarantinesControlWhenStoppedStateIsUnconfirmed) +{ + agv_->confirm_stopped_result_ = device::AgvResult::failure( + device::AgvErrorCode::Timeout, + "two zero-velocity samples were not observed"); + + api::CommandHeader_Request request; + request.set_device_id("test-agv"); + api::CommandHeader_Feedback response; + grpc::ServerContext context; + const auto status = service_->cancelNavigation( + &context, &request, &response); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::DEADLINE_EXCEEDED); + EXPECT_FALSE(response.success()); + EXPECT_NE( + response.error_message().find("confirmed stopped state"), + std::string::npos); + EXPECT_TRUE(agv_->cancel_navigation_barrier_observed_); + EXPECT_TRUE(agv_->confirm_stopped_barrier_observed_); + + const auto lease = + control::ControlAuthorityManager::instance().tryAcquire( + "test-agv", + "normal-control-after-unconfirmed-stop", + std::chrono::hours(1)); + EXPECT_FALSE(lease.acquired); +} + TEST_F(GrpcAgvServiceTest, NavigateToStationForwardsPgvAdapterParams) { api::AgvNavigateToStationCommand_Request request; @@ -384,5 +606,17 @@ TEST(AbstractAgvCompatibilityTest, FollowPathOptionsDelegateToLegacyOverride) EXPECT_EQ(legacy.path_[0].target_station, "station-2"); } +TEST(AbstractAgvCompatibilityTest, SynchronousActionSupportDefaultsToFalse) +{ + LegacyFollowPathAgv legacy; + + EXPECT_FALSE(legacy.supportsSynchronousAction( + device::AgvActionKind::NavigateToPose)); + EXPECT_FALSE(legacy.supportsSynchronousAction( + device::AgvActionKind::NavigateToStation)); + EXPECT_FALSE(legacy.supportsSynchronousAction( + device::AgvActionKind::FollowPath)); +} + } // namespace } // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp index f5b2c46f..e44671d3 100644 --- a/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -135,6 +136,16 @@ public: lock, [this]() { return release_blocking_stop_; }); } + if (throw_next_stop_) { + throw_next_stop_ = false; + throw std::runtime_error("simulated stopMotion exception"); + } + if (fail_next_stop_) { + fail_next_stop_ = false; + return device::Result::failure( + device::ArmErrorCode::CommandFailed, + "simulated stopMotion failure"); + } return device::Result::success(); } @@ -178,6 +189,18 @@ public: release_blocking_stop_ = false; } + void failNextStopMotion() + { + std::lock_guard lock(motion_mutex_); + fail_next_stop_ = true; + } + + void throwNextStopMotion() + { + std::lock_guard lock(motion_mutex_); + throw_next_stop_ = true; + } + bool waitForBlockingStop(const std::chrono::milliseconds timeout) { std::unique_lock lock(motion_mutex_); @@ -350,6 +373,8 @@ private: bool block_next_stop_{false}; bool blocking_stop_started_{false}; bool release_blocking_stop_{false}; + bool fail_next_stop_{false}; + bool throw_next_stop_{false}; std::string blocking_motion_name_; int move_j_calls_{0}; int move_l_calls_{0}; @@ -688,5 +713,51 @@ TEST_F(GrpcArmServiceTest, EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1); } +TEST_F(GrpcArmServiceTest, StopMotionFailureRetainsSafetyBarrier) +{ + auto& authority = control::ControlAuthorityManager::instance(); + const auto action_lease = authority.tryAcquire( + "aubo_arm", "action-queue:test", std::chrono::hours(1)); + ASSERT_TRUE(action_lease.acquired) << action_lease.detail; + aubo_arm_->failNextStopMotion(); + + api::CommandHeader_Feedback stop_response; + const auto stop_status = stopMotion("aubo_arm", stop_response); + const auto rejected_move = moveJ("aubo_arm"); + + EXPECT_EQ(stop_status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_FALSE(stop_response.success()); + EXPECT_FALSE(authority.validate(action_lease.token)); + EXPECT_TRUE(authority.isLeased("aubo_arm")); + EXPECT_EQ( + rejected_move.status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_EQ(aubo_arm_->moveJCalls(), 0); + EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1); +} + +TEST_F(GrpcArmServiceTest, StopMotionExceptionRetainsSafetyBarrier) +{ + auto& authority = control::ControlAuthorityManager::instance(); + const auto action_lease = authority.tryAcquire( + "aubo_arm", "action-queue:test", std::chrono::hours(1)); + ASSERT_TRUE(action_lease.acquired) << action_lease.detail; + aubo_arm_->throwNextStopMotion(); + + api::CommandHeader_Feedback stop_response; + const auto stop_status = stopMotion("aubo_arm", stop_response); + const auto rejected_move = moveL("aubo_arm"); + + EXPECT_EQ(stop_status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_FALSE(stop_response.success()); + EXPECT_FALSE(authority.validate(action_lease.token)); + EXPECT_TRUE(authority.isLeased("aubo_arm")); + EXPECT_EQ( + rejected_move.status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_EQ(aubo_arm_->moveLCalls(), 0); + 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 22eac5e4..70e991a6 100644 --- a/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp @@ -1,13 +1,17 @@ #include "service/grpc/include/grpc_system_service.h" +#include #include +#include #include #include #include #include #include #include +#include #include +#include #include #include @@ -15,8 +19,11 @@ #include #include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "devices/agv/abstract_agv.h" +#include "devices/arm/robot_arm.h" #include "manager/control_authority/include/control_authority_manager.h" #include "manager/device_manager/include/device_manager.h" +#include "service/action/include/action_queue_executor.h" namespace cmvr::service { namespace { @@ -94,6 +101,613 @@ private: bool release_stop_{false}; }; +class ActionTrace final { +public: + using TimePoint = std::chrono::steady_clock::time_point; + + struct Entry { + std::string name; + TimePoint time; + }; + + void add(std::string name) + { + std::lock_guard lock(mutex_); + entries_.push_back({std::move(name), std::chrono::steady_clock::now()}); + } + + std::vector names() const + { + std::lock_guard lock(mutex_); + std::vector result; + result.reserve(entries_.size()); + for (const auto& entry : entries_) { + result.push_back(entry.name); + } + return result; + } + + std::optional firstTime(const std::string& name) const + { + std::lock_guard lock(mutex_); + const auto found = std::find_if( + entries_.begin(), entries_.end(), + [&name](const Entry& entry) { return entry.name == name; }); + return found == entries_.end() + ? std::nullopt + : std::optional(found->time); + } + +private: + mutable std::mutex mutex_; + std::vector entries_; +}; + +class ActionTestArm final : public device::RobotArm { +public: + ActionTestArm(std::string id, std::shared_ptr trace) + : trace_(std::move(trace)) + { + id_ = std::move(id); + } + + std::string typeName() const override { return "ActionTestArm"; } + bool supportsActionQueueMotion() const noexcept override { return true; } + device::RobotModel getRobotModel() const override + { + device::RobotModel model; + model.name = "ActionTestArm"; + model.dof = kDof; + model.joint_names.assign(kDof, "joint"); + return model; + } + std::size_t getDof() const override { return kDof; } + device::ArmState getRobotState() const override { return {}; } + device::JointGroupState getJointState() const override { return {}; } + device::CartesianPose getTcpPose( + device::FrameType = device::FrameType::Base) const override + { + return {}; + } + device::RobotMode getRobotMode() const override + { + return device::RobotMode::Idle; + } + device::SafetyMode getSafetyMode() const override + { + return device::SafetyMode::Normal; + } + device::ControlMode getControlMode() const override + { + return device::ControlMode::Position; + } + + device::Result torqueOn() override { return device::Result::success(); } + device::Result torqueOff() override { return device::Result::success(); } + device::Result calibrateZeroQ(const std::string&) override + { + return device::Result::success(); + } + device::Result emergencyStop() override + { + return device::Result::success(); + } + device::Result protectiveStop() override + { + return device::Result::success(); + } + device::Result setSpeedScaling(double) override + { + return device::Result::success(); + } + double getSpeedScaling() const override { return 1.0; } + bool isProtectiveStopped() const override { return false; } + bool isEmergencyStopped() const override { return false; } + bool isFault() const override { return false; } + + device::Result moveJ(const device::JointPositionCommand&, + const device::MotionOptions& options) override + { + return performMotion("arm:J", options); + } + device::Result speedJ(const device::JointVelocityCommand&, + double, + double) override + { + return device::Result::success(); + } + device::Result stopJ(double) override { return stopMotion(); } + device::Result moveL( + const device::CartesianPose& target, + const device::MotionOptions& options, + device::FrameType = device::FrameType::Base) override + { + return performMotion( + "arm:L:" + std::to_string(static_cast(target.x)), + options); + } + device::Result speedL( + const device::CartesianVelocity&, + double, + double, + device::FrameType = device::FrameType::Base) override + { + return device::Result::success(); + } + device::Result stopL(std::optional = std::nullopt) override + { + return stopMotion(); + } + device::Result stopMotion() override + { + { + std::lock_guard lock(mutex_); + ++stop_motion_calls_; + stop_requested_ = true; + } + trace_->add("arm:stop:" + id_); + motion_condition_.notify_all(); + return device::Result::success(); + } + + device::Result startServoMode(const device::ServoOptions&) override + { + return device::Result::success(); + } + device::Result servoJ(const device::JointPositionCommand&) override + { + return device::Result::success(); + } + device::Result servoL( + const device::CartesianPose&, + device::FrameType = device::FrameType::Base) override + { + return device::Result::success(); + } + device::Result servoSpeedJ( + const device::JointVelocityCommand&) override + { + return device::Result::success(); + } + device::Result servoSpeedL( + const device::CartesianVelocity&, + device::FrameType = device::FrameType::Base) override + { + return device::Result::success(); + } + device::Result stopServoMode() override + { + return device::Result::success(); + } + + device::Result connect(const std::string&, int) override + { + return device::Result::success(); + } + device::Result disconnect() override { return device::Result::success(); } + bool isConnected() const override { return true; } + device::Result powerOn() override { return device::Result::success(); } + device::Result powerOff() override { return device::Result::success(); } + device::Result brakeRelease() override { return device::Result::success(); } + device::Result shutdown() override { return device::Result::success(); } + device::Result clearFault() override { return device::Result::success(); } + device::Result unlockProtectiveStop() override + { + return device::Result::success(); + } + device::Result loadProgram(const std::string&) override + { + return device::Result::success(); + } + device::Result playProgram() override { return device::Result::success(); } + device::Result pauseProgram() override { return device::Result::success(); } + device::Result stopProgram() override { return device::Result::success(); } + std::vector ik(const std::string&, + const std::string&, + const device::CartesianPose&) override + { + return {}; + } + std::shared_ptr kinematicsSolver() const override + { + return nullptr; + } + device::CartesianPose fk(const std::string&, + const std::string&) override + { + return {}; + } + device::CartesianPose fk(bool = true) override { return {}; } + device::CartesianVelocity getSpeedLCommandTwistBase() const override + { + return {}; + } + bool busy() const override + { + std::lock_guard lock(mutex_); + return active_motions_ != 0; + } + + void blockNextMotion() + { + std::lock_guard lock(mutex_); + block_next_motion_ = true; + release_blocked_motion_ = false; + stop_requested_ = false; + } + + void blockCanceledMotionReturn() + { + std::lock_guard lock(mutex_); + block_canceled_motion_return_ = true; + canceled_motion_return_blocked_ = false; + release_canceled_motion_return_ = false; + } + + bool waitForCanceledMotionReturn( + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return canceled_motion_return_condition_.wait_for( + lock, + timeout, + [this]() { return canceled_motion_return_blocked_; }); + } + + void releaseCanceledMotionReturn() + { + { + std::lock_guard lock(mutex_); + release_canceled_motion_return_ = true; + } + motion_condition_.notify_all(); + } + + void releaseBlockedMotion() + { + { + std::lock_guard lock(mutex_); + release_blocked_motion_ = true; + } + motion_condition_.notify_all(); + } + + void failOnMotionCall(const int call_index) + { + std::lock_guard lock(mutex_); + fail_on_motion_call_ = call_index; + } + + bool waitForMotionCalls( + const int expected, + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return motion_started_condition_.wait_for( + lock, timeout, + [this, expected]() { return motion_calls_ >= expected; }); + } + + int motionCalls() const + { + std::lock_guard lock(mutex_); + return motion_calls_; + } + + int stopMotionCalls() const + { + std::lock_guard lock(mutex_); + return stop_motion_calls_; + } + + int maxActiveMotions() const + { + std::lock_guard lock(mutex_); + return max_active_motions_; + } + +private: + device::Result performMotion( + const std::string& event, + const device::MotionOptions& options) + { + bool block = false; + bool fail = false; + { + std::lock_guard lock(mutex_); + ++motion_calls_; + ++active_motions_; + max_active_motions_ = std::max( + max_active_motions_, active_motions_); + block = block_next_motion_; + block_next_motion_ = false; + fail = motion_calls_ == fail_on_motion_call_; + } + trace_->add(event); + motion_started_condition_.notify_all(); + + bool canceled = false; + bool safety_timeout = false; + if (block) { + const auto safety_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + std::unique_lock lock(mutex_); + while (!release_blocked_motion_ && !stop_requested_ && + !(options.cancellation_requested && + options.cancellation_requested())) { + if (std::chrono::steady_clock::now() >= safety_deadline) { + safety_timeout = true; + break; + } + motion_condition_.wait_for(lock, std::chrono::milliseconds(1)); + } + canceled = stop_requested_ || + (options.cancellation_requested && + options.cancellation_requested()); + } + + { + std::unique_lock lock(mutex_); + --active_motions_; + if (canceled && block_canceled_motion_return_) { + block_canceled_motion_return_ = false; + canceled_motion_return_blocked_ = true; + canceled_motion_return_condition_.notify_all(); + motion_condition_.wait( + lock, + [this]() { return release_canceled_motion_return_; }); + } + } + if (safety_timeout) { + return device::Result::failure( + device::ArmErrorCode::Timeout, + "test arm safety wait expired"); + } + if (canceled) { + return device::Result::failure( + device::ArmErrorCode::CommandRejected, + "test arm motion canceled"); + } + if (fail) { + return device::Result::failure( + device::ArmErrorCode::CommandFailed, + "injected RobotArm failure"); + } + return device::Result::success(); + } + + static constexpr std::size_t kDof = 6U; + std::shared_ptr trace_; + mutable std::mutex mutex_; + std::condition_variable motion_condition_; + std::condition_variable motion_started_condition_; + std::condition_variable canceled_motion_return_condition_; + int motion_calls_{0}; + int stop_motion_calls_{0}; + int active_motions_{0}; + int max_active_motions_{0}; + int fail_on_motion_call_{-1}; + bool block_next_motion_{false}; + bool release_blocked_motion_{false}; + bool stop_requested_{false}; + bool block_canceled_motion_return_{false}; + bool canceled_motion_return_blocked_{false}; + bool release_canceled_motion_return_{false}; +}; + +class ActionTestAgv final : public device::AbstractAGV { +public: + ActionTestAgv(std::string id, std::shared_ptr trace) + : trace_(std::move(trace)) + { + id_ = std::move(id); + } + + std::string typeName() const override { return "ActionTestAgv"; } + bool supportsSynchronousAction(device::AgvActionKind) const noexcept override + { + return supports_synchronous_.load(std::memory_order_acquire); + } + device::AgvRuntimeState runtimeState() const override { return {}; } + device::AgvNavigationStatus navigationStatus() const override { return {}; } + device::AgvResult navigateToPose( + const math::Pose2d& pose, + const device::AgvMotionOptions&, + const device::AgvAdapterParams&) override + { + { + std::lock_guard lock(parameters_mutex_); + last_pose_ = pose; + } + return record("agv:pose"); + } + device::AgvResult navigateToStation( + const std::string& station_id, + const device::AgvMotionOptions&, + const device::AgvAdapterParams&) override + { + return record("agv:station:" + station_id); + } + device::AgvResult followPath( + const std::vector& path, + const device::AgvMotionOptions&) override + { + { + std::lock_guard lock(parameters_mutex_); + last_path_ = path; + } + return record("agv:path"); + } + device::AgvResult cancelNavigation() override + { + cancel_calls_.fetch_add(1, std::memory_order_relaxed); + trace_->add("agv:cancel"); + return device::AgvResult::success(); + } + device::AgvResult confirmMotionStopped() override + { + return stopped_confirmed_.load(std::memory_order_acquire) + ? device::AgvResult::success() + : device::AgvResult::failure( + device::AgvErrorCode::Timeout, + "test AGV stopped state is unconfirmed"); + } + void setSupportsSynchronous(const bool value) + { + supports_synchronous_.store(value, std::memory_order_release); + } + + void setNavigationFailure(const bool value) + { + fail_navigation_.store(value, std::memory_order_release); + } + + void setStoppedConfirmed(const bool value) + { + stopped_confirmed_.store(value, std::memory_order_release); + } + + int navigationCalls() const + { + return navigation_calls_.load(std::memory_order_relaxed); + } + + math::Pose2d lastPose() const + { + std::lock_guard lock(parameters_mutex_); + return last_pose_; + } + + std::vector lastPath() const + { + std::lock_guard lock(parameters_mutex_); + return last_path_; + } + +private: + device::AgvResult record(std::string event) + { + navigation_calls_.fetch_add(1, std::memory_order_relaxed); + trace_->add(std::move(event)); + if (fail_navigation_.load(std::memory_order_acquire)) { + return device::AgvResult::failure( + device::AgvErrorCode::TaskFailed, + "injected AGV navigation failure"); + } + return device::AgvResult::success(); + } + + std::shared_ptr trace_; + mutable std::mutex parameters_mutex_; + math::Pose2d last_pose_{}; + std::vector last_path_; + std::atomic supports_synchronous_{true}; + std::atomic fail_navigation_{false}; + std::atomic stopped_confirmed_{true}; + std::atomic navigation_calls_{0}; + std::atomic cancel_calls_{0}; +}; + +api::ActionStep* addMoveLStep( + api::ActionQueueCommand_Request& request, + const std::string& step_id, + const std::string& device_id, + const double marker, + const std::uint32_t timeout_ms = 0U, + const bool asynchronous = false) +{ + auto* step = request.add_steps(); + step->set_step_id(step_id); + step->set_timeout_ms(timeout_ms); + auto* command = step->mutable_arm_move_l(); + command->mutable_header()->set_device_id(device_id); + command->mutable_target()->set_x(marker); + command->set_frame(api::ARM_FRAME_BASE); + command->mutable_options()->set_asynchronous(asynchronous); + return step; +} + +api::ActionStep* addMoveJStep( + api::ActionQueueCommand_Request& request, + const std::string& step_id, + const std::string& device_id, + const bool asynchronous = false) +{ + auto* step = request.add_steps(); + step->set_step_id(step_id); + auto* command = step->mutable_arm_move_j(); + command->mutable_header()->set_device_id(device_id); + for (std::size_t index = 0; index < 6U; ++index) { + command->mutable_target()->add_position( + static_cast(index) * 0.1); + } + command->mutable_options()->set_asynchronous(asynchronous); + return step; +} + +api::ActionStep* addAgvStationStep( + api::ActionQueueCommand_Request& request, + const std::string& step_id, + const std::string& device_id, + const std::string& station_id, + const bool asynchronous = false) +{ + auto* step = request.add_steps(); + step->set_step_id(step_id); + auto* command = step->mutable_agv_navigate_to_station(); + command->mutable_header()->set_device_id(device_id); + command->set_station_id(station_id); + command->mutable_options()->set_asynchronous(asynchronous); + return step; +} + +api::ActionStep* addAgvPoseStep( + api::ActionQueueCommand_Request& request, + const std::string& step_id, + const std::string& device_id, + const double x, + const double y, + const double theta) +{ + auto* step = request.add_steps(); + step->set_step_id(step_id); + auto* command = step->mutable_agv_navigate_to_pose(); + command->mutable_header()->set_device_id(device_id); + command->mutable_pose()->set_x(x); + command->mutable_pose()->set_y(y); + command->mutable_pose()->set_theta(theta); + return step; +} + +api::ActionStep* addAgvPathStep( + api::ActionQueueCommand_Request& request, + const std::string& step_id, + const std::string& device_id) +{ + auto* step = request.add_steps(); + step->set_step_id(step_id); + auto* command = step->mutable_agv_follow_path(); + command->mutable_header()->set_device_id(device_id); + auto* first = command->add_path(); + first->set_source_station("start"); + first->set_target_station("middle"); + auto* second = command->add_path(); + second->set_source_station("middle"); + second->set_target_station("finish"); + return step; +} + +api::ActionStep* addDelayStep( + api::ActionQueueCommand_Request& request, + const std::string& step_id, + const std::uint32_t duration_ms) +{ + auto* step = request.add_steps(); + step->set_step_id(step_id); + step->mutable_delay()->set_duration_ms(duration_ms); + return step; +} + std::uint64_t currentUnixTimeMs() { const auto elapsed = std::chrono::duration_cast( @@ -124,6 +738,9 @@ protected: void TearDown() override { service_.reset(); + action_agv_.reset(); + action_arm_.reset(); + action_trace_.reset(); owned_devices_.clear(); device::DeviceManager::destroyInstance(); control::ControlAuthorityManager::instance().clear(); @@ -148,8 +765,50 @@ protected: owned_devices_.push_back(std::move(device)); } + void initializeActionDevices() + { + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + action_trace_ = std::make_shared(); + action_arm_ = std::make_shared( + "action-arm", action_trace_); + action_agv_ = std::make_shared( + "action-agv", action_trace_); + manager.registerDevice(action_arm_); + manager.registerDevice(action_agv_); + service_ = std::make_unique(); + api::GetSystemInfoCommand_Request info_request; + api::GetSystemInfoCommand_Feedback info_response; + grpc::ServerContext info_context; + const auto info_status = service_->GetSystemInfo( + &info_context, &info_request, &info_response); + ASSERT_TRUE(info_status.ok()) << info_status.error_message(); + service_instance_id_ = info_response.action_service_instance_id(); + ASSERT_FALSE(service_instance_id_.empty()); + } + + api::ActionQueueCommand_Feedback executeAction( + const api::ActionQueueCommand_Request& request) + { + auto bound_request = request; + if (bound_request.expected_service_instance_id().empty()) { + bound_request.set_expected_service_instance_id( + service_instance_id_); + } + api::ActionQueueCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->ExecuteActionQueue( + &context, &bound_request, &response); + EXPECT_TRUE(status.ok()) << status.error_message(); + return response; + } + std::unique_ptr service_; std::vector> owned_devices_; + std::shared_ptr action_trace_; + std::shared_ptr action_arm_; + std::shared_ptr action_agv_; + std::string service_instance_id_; }; TEST_F(GrpcSystemServiceTest, @@ -340,6 +999,784 @@ TEST_F(GrpcSystemServiceTest, MapsEveryKnownDeviceKind) } } +TEST_F(GrpcSystemServiceTest, ActionQueueExecutesFourMoveLStepsSerially) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request request; + request.set_action_id("four-movel"); + for (int marker = 1; marker <= 4; ++marker) { + addMoveLStep( + request, + "move-" + std::to_string(marker), + action_arm_->id(), + static_cast(marker)); + } + + const auto response = executeAction(request); + + EXPECT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(response.completed_steps(), 4U); + EXPECT_FALSE(response.has_failed_step_index()); + EXPECT_EQ(action_arm_->motionCalls(), 4); + EXPECT_EQ(action_arm_->maxActiveMotions(), 1); + EXPECT_EQ( + action_trace_->names(), + (std::vector{ + "arm:L:1", "arm:L:2", "arm:L:3", "arm:L:4"})); +} + +TEST_F(GrpcSystemServiceTest, ActionQueuePreservesArmDelayAgvOrder) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request request; + request.set_action_id("arm-delay-agv"); + addMoveJStep(request, "arm", action_arm_->id()); + addDelayStep(request, "settle", 25U); + addAgvStationStep( + request, "agv", action_agv_->id(), "dock"); + + const auto response = executeAction(request); + const auto arm_time = action_trace_->firstTime("arm:J"); + const auto agv_time = action_trace_->firstTime("agv:station:dock"); + + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(response.completed_steps(), 3U); + EXPECT_EQ( + action_trace_->names(), + (std::vector{"arm:J", "agv:station:dock"})); + ASSERT_TRUE(arm_time.has_value()); + ASSERT_TRUE(agv_time.has_value()); + EXPECT_GE( + std::chrono::duration_cast( + *agv_time - *arm_time), + std::chrono::milliseconds(15)); +} + +TEST_F(GrpcSystemServiceTest, ActionQueueExecutesAgvPoseAndPathSteps) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request request; + request.set_action_id("agv-pose-and-path"); + addAgvPoseStep( + request, "pose", action_agv_->id(), 1.0, 2.0, 0.5); + addAgvPathStep(request, "path", action_agv_->id()); + + const auto response = executeAction(request); + + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(response.completed_steps(), 2U); + EXPECT_EQ(action_agv_->navigationCalls(), 2); + const auto pose = action_agv_->lastPose(); + EXPECT_DOUBLE_EQ(pose.x, 1.0); + EXPECT_DOUBLE_EQ(pose.y, 2.0); + EXPECT_DOUBLE_EQ(pose.theta, 0.5); + const auto path = action_agv_->lastPath(); + ASSERT_EQ(path.size(), 2U); + EXPECT_EQ(path[0].source_station, "start"); + EXPECT_EQ(path[0].target_station, "middle"); + EXPECT_EQ(path[1].source_station, "middle"); + EXPECT_EQ(path[1].target_station, "finish"); + EXPECT_EQ( + action_trace_->names(), + (std::vector{"agv:pose", "agv:path"})); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueuePrevalidatesAllStepsBeforeAnyDispatch) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request request; + request.set_action_id("prevalidate-all"); + addMoveLStep(request, "valid-first", action_arm_->id(), 1.0); + addAgvStationStep( + request, "missing-second", "missing-agv", "dock"); + + const auto response = executeAction(request); + + EXPECT_FALSE(response.header().success()); + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ(response.completed_steps(), 0U); + ASSERT_TRUE(response.has_failed_step_index()); + EXPECT_EQ(response.failed_step_index(), 1U); + EXPECT_EQ(action_arm_->motionCalls(), 0); + EXPECT_EQ(action_agv_->navigationCalls(), 0); + EXPECT_TRUE(action_trace_->names().empty()); +} + +TEST_F(GrpcSystemServiceTest, ActionQueueRejectsAsynchronousMotion) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request arm_request; + arm_request.set_action_id("async-arm"); + addMoveLStep( + arm_request, "arm", action_arm_->id(), 1.0, 0U, true); + const auto arm_response = executeAction(arm_request); + + api::ActionQueueCommand_Request agv_request; + agv_request.set_action_id("async-agv"); + addAgvStationStep( + agv_request, "agv", action_agv_->id(), "dock", true); + const auto agv_response = executeAction(agv_request); + + EXPECT_EQ(arm_response.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ(agv_response.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ(action_arm_->motionCalls(), 0); + EXPECT_EQ(action_agv_->navigationCalls(), 0); + EXPECT_TRUE(action_trace_->names().empty()); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueStopsAfterFailureAndReportsFailedStep) +{ + initializeActionDevices(); + action_arm_->failOnMotionCall(2); + api::ActionQueueCommand_Request request; + request.set_action_id("fail-fast"); + addMoveLStep(request, "first", action_arm_->id(), 1.0); + addMoveLStep(request, "fails", action_arm_->id(), 2.0); + addMoveLStep(request, "must-not-run", action_arm_->id(), 3.0); + + const auto response = executeAction(request); + + EXPECT_FALSE(response.header().success()); + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_FAILED); + EXPECT_EQ(response.completed_steps(), 1U); + ASSERT_TRUE(response.has_failed_step_index()); + EXPECT_EQ(response.failed_step_index(), 1U); + EXPECT_EQ(action_arm_->motionCalls(), 2); + const auto trace = action_trace_->names(); + ASSERT_GE(trace.size(), 2U); + EXPECT_EQ(trace[0], "arm:L:1"); + EXPECT_EQ(trace[1], "arm:L:2"); + EXPECT_GE(action_arm_->stopMotionCalls(), 1); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueQuarantinesAgvAfterUnconfirmedFailureStop) +{ + initializeActionDevices(); + action_agv_->setNavigationFailure(true); + action_agv_->setStoppedConfirmed(false); + api::ActionQueueCommand_Request request; + request.set_action_id("agv-unconfirmed-stop"); + addAgvStationStep( + request, "fails", action_agv_->id(), "dock"); + + const auto response = executeAction(request); + + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_FAILED); + ASSERT_TRUE(response.has_failed_step_index()); + EXPECT_EQ(response.failed_step_index(), 0U); + EXPECT_NE( + response.header().error_message().find("remain quarantined"), + std::string::npos); + const auto lease = + control::ControlAuthorityManager::instance().tryAcquire( + action_agv_->id(), + "normal-control-after-unconfirmed-action-stop", + std::chrono::hours(1)); + EXPECT_FALSE(lease.acquired); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueIsIdempotentAndRejectsConflictingPayload) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request request; + request.set_action_id("idempotent-action"); + addMoveLStep(request, "only-step", action_arm_->id(), 1.0); + + const auto first = executeAction(request); + const auto retry = executeAction(request); + auto conflicting = request; + conflicting.mutable_steps(0) + ->mutable_arm_move_l()->mutable_target()->set_x(2.0); + const auto conflict = executeAction(conflicting); + + EXPECT_EQ(first.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ( + first.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_ACCEPTED_NEW); + EXPECT_EQ(retry.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ( + retry.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_CACHED_RESULT); + EXPECT_EQ(retry.completed_steps(), 1U); + EXPECT_EQ(conflict.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ( + conflict.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_ACTION_ID_CONFLICT); + EXPECT_EQ(action_arm_->motionCalls(), 1); + EXPECT_EQ( + action_trace_->names(), + (std::vector{"arm:L:1"})); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueRetryAfterTerminalCacheChurnDoesNotRedispatch) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request original; + original.set_action_id("idempotent-after-cache-churn"); + addMoveLStep(original, "only-step", action_arm_->id(), 1.0); + + const auto first = executeAction(original); + ASSERT_EQ(first.result(), api::ACTION_RESULT_CODE_COMPLETED); + ASSERT_EQ(action_arm_->motionCalls(), 1); + + constexpr int kActionsBeyondTerminalCacheCapacity = 257; + for (int index = 0; index < kActionsBeyondTerminalCacheCapacity; ++index) { + SCOPED_TRACE(index); + api::ActionQueueCommand_Request filler; + filler.set_action_id("terminal-cache-filler-" + std::to_string(index)); + addDelayStep(filler, "delay", 0U); + + const auto response = executeAction(filler); + ASSERT_EQ(response.result(), api::ACTION_RESULT_CODE_COMPLETED) + << response.header().error_message(); + } + + const auto retry = executeAction(original); + + EXPECT_EQ(retry.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(retry.completed_steps(), 1U); + EXPECT_EQ(action_arm_->motionCalls(), 1); + + // submitAndWait() may wake before the worker completes terminal-cache + // rotation for the final filler. Advance one additional terminal action so + // the original ID is deterministically present in the retired-ID filter. + constexpr int kTotalActionsToRetireOriginal = 4353; + for (int index = kActionsBeyondTerminalCacheCapacity; + index < kTotalActionsToRetireOriginal; ++index) { + SCOPED_TRACE(index); + api::ActionQueueCommand_Request filler; + filler.set_action_id("terminal-cache-filler-" + std::to_string(index)); + addDelayStep(filler, "delay", 0U); + + const auto response = executeAction(filler); + ASSERT_EQ(response.result(), api::ACTION_RESULT_CODE_COMPLETED) + << response.header().error_message(); + } + + const auto retired_retry = executeAction(original); + + EXPECT_EQ(retired_retry.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ( + retired_retry.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_RESULT_EVICTED); + EXPECT_NE( + retired_retry.header().error_message().find("no longer cached"), + std::string::npos); + EXPECT_EQ(action_arm_->motionCalls(), 1); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueRejectsMissingOrStaleServiceInstance) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request request; + request.set_action_id("service-instance-check"); + addMoveLStep(request, "move", action_arm_->id(), 1.0); + + api::ActionQueueCommand_Feedback missing_response; + grpc::ServerContext missing_context; + const auto missing_status = service_->ExecuteActionQueue( + &missing_context, &request, &missing_response); + + request.set_expected_service_instance_id("stale-instance"); + api::ActionQueueCommand_Feedback stale_response; + grpc::ServerContext stale_context; + const auto stale_status = service_->ExecuteActionQueue( + &stale_context, &request, &stale_response); + + EXPECT_TRUE(missing_status.ok()) << missing_status.error_message(); + EXPECT_EQ( + missing_response.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ(missing_response.service_instance_id(), service_instance_id_); + EXPECT_TRUE(stale_status.ok()) << stale_status.error_message(); + EXPECT_EQ(stale_response.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ( + stale_response.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_SERVICE_INSTANCE_MISMATCH); + EXPECT_EQ(stale_response.service_instance_id(), service_instance_id_); + EXPECT_EQ(action_arm_->motionCalls(), 0); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueServiceRestartRejectsRetryBoundToPriorInstance) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request request; + request.set_action_id("prior-instance-retry"); + request.set_expected_service_instance_id(service_instance_id_); + addMoveLStep(request, "move", action_arm_->id(), 1.0); + const std::string prior_instance = service_instance_id_; + + const auto first = executeAction(request); + ASSERT_EQ(first.result(), api::ACTION_RESULT_CODE_COMPLETED) + << first.header().error_message(); + ASSERT_EQ(action_arm_->motionCalls(), 1); + + service_.reset(); + service_ = std::make_unique(); + api::GetSystemInfoCommand_Request info_request; + api::GetSystemInfoCommand_Feedback info_response; + grpc::ServerContext info_context; + ASSERT_TRUE(service_->GetSystemInfo( + &info_context, &info_request, &info_response).ok()); + service_instance_id_ = info_response.action_service_instance_id(); + ASSERT_FALSE(service_instance_id_.empty()); + ASSERT_NE(service_instance_id_, prior_instance); + + api::ActionQueueCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->ExecuteActionQueue( + &context, &request, &response); + + EXPECT_TRUE(status.ok()) << status.error_message(); + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ( + response.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_SERVICE_INSTANCE_MISMATCH); + EXPECT_EQ(response.service_instance_id(), service_instance_id_); + EXPECT_EQ(action_arm_->motionCalls(), 1); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueLedgerCapacityRejectsOnlyNewIds) +{ + initializeActionDevices(); + ActionQueueExecutor executor( + device::DeviceManager::getInstance(), 2U); + const auto make_request = [&executor](const std::string& action_id) { + api::ActionQueueCommand_Request request; + request.set_action_id(action_id); + request.set_expected_service_instance_id(executor.instanceId()); + addDelayStep(request, "delay", 0U); + return request; + }; + const auto first_request = make_request("ledger-first"); + const auto second_request = make_request("ledger-second"); + const auto rejected_request = make_request("ledger-third"); + api::ActionQueueCommand_Feedback first; + api::ActionQueueCommand_Feedback second; + api::ActionQueueCommand_Feedback rejected; + api::ActionQueueCommand_Feedback retry; + + EXPECT_EQ( + executor.submitAndWait(first_request, first), + ActionQueueExecutor::WaitResult::Terminal); + EXPECT_EQ( + executor.submitAndWait(second_request, second), + ActionQueueExecutor::WaitResult::Terminal); + EXPECT_EQ( + executor.submitAndWait(rejected_request, rejected), + ActionQueueExecutor::WaitResult::Terminal); + EXPECT_EQ( + executor.submitAndWait(first_request, retry), + ActionQueueExecutor::WaitResult::Terminal); + + EXPECT_EQ(first.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(second.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(rejected.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ( + rejected.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_LEDGER_EXHAUSTED); + EXPECT_EQ(retry.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ( + retry.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_CACHED_RESULT); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueCanceledWaiterLeavesAcceptedActionRunning) +{ + initializeActionDevices(); + ActionQueueExecutor executor(device::DeviceManager::getInstance()); + action_arm_->blockNextMotion(); + + api::ActionQueueCommand_Request request; + request.set_action_id("canceled-waiter-action-continues"); + request.set_expected_service_instance_id(executor.instanceId()); + request.set_total_timeout_ms(1000U); + addMoveLStep(request, "move", action_arm_->id(), 1.0); + + std::atomic waiter_canceled{false}; + api::ActionQueueCommand_Feedback abandoned_feedback; + auto waiter = std::async( + std::launch::async, + [&executor, &request, &abandoned_feedback, &waiter_canceled]() { + return executor.submitAndWait( + request, + abandoned_feedback, + [&waiter_canceled]() { + return waiter_canceled.load(std::memory_order_acquire); + }); + }); + ASSERT_TRUE(action_arm_->waitForMotionCalls( + 1, std::chrono::milliseconds(500))); + + waiter_canceled.store(true, std::memory_order_release); + const auto waiter_status = waiter.wait_for(std::chrono::milliseconds(500)); + if (waiter_status != std::future_status::ready) { + action_arm_->releaseBlockedMotion(); + } + ASSERT_EQ(waiter_status, std::future_status::ready); + EXPECT_EQ( + waiter.get(), + ActionQueueExecutor::WaitResult::CanceledAfterAdmission); + EXPECT_EQ(action_arm_->motionCalls(), 1); + + action_arm_->releaseBlockedMotion(); + ASSERT_TRUE(executor.waitForIdle(std::chrono::milliseconds(500))); + + api::ActionQueueCommand_Feedback retry_feedback; + const auto retry_result = executor.submitAndWait(request, retry_feedback); + + EXPECT_EQ(retry_result, ActionQueueExecutor::WaitResult::Terminal); + EXPECT_TRUE(retry_feedback.header().success()) + << retry_feedback.header().error_message(); + EXPECT_EQ( + retry_feedback.result(), + api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(retry_feedback.completed_steps(), 1U); + EXPECT_EQ(action_arm_->motionCalls(), 1); +} + +TEST_F(GrpcSystemServiceTest, + ConcurrentIdenticalActionIdJoinsInFlightExecution) +{ + initializeActionDevices(); + ActionQueueExecutor executor(device::DeviceManager::getInstance()); + action_arm_->blockNextMotion(); + + api::ActionQueueCommand_Request request; + request.set_action_id("join-identical-in-flight-action"); + request.set_expected_service_instance_id(executor.instanceId()); + request.set_total_timeout_ms(1000U); + addMoveLStep(request, "move", action_arm_->id(), 1.0); + + api::ActionQueueCommand_Feedback first_feedback; + api::ActionQueueCommand_Feedback joined_feedback; + auto first = std::async( + std::launch::async, + [&executor, &request, &first_feedback]() { + return executor.submitAndWait(request, first_feedback); + }); + const bool motion_started = action_arm_->waitForMotionCalls( + 1, std::chrono::milliseconds(500)); + + std::atomic joined_wait_polls{0}; + auto joined = std::async( + std::launch::async, + [&executor, &request, &joined_feedback, &joined_wait_polls]() { + return executor.submitAndWait( + request, + joined_feedback, + [&joined_wait_polls]() { + joined_wait_polls.fetch_add( + 1, std::memory_order_acq_rel); + return false; + }); + }); + const auto poll_deadline = + std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (joined_wait_polls.load(std::memory_order_acquire) < 2 && + std::chrono::steady_clock::now() < poll_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + const bool joined_is_waiting = + joined_wait_polls.load(std::memory_order_acquire) >= 2; + action_arm_->releaseBlockedMotion(); + + const auto first_result = first.get(); + const auto joined_result = joined.get(); + + ASSERT_TRUE(motion_started); + ASSERT_TRUE(joined_is_waiting); + EXPECT_EQ(first_result, ActionQueueExecutor::WaitResult::Terminal); + EXPECT_EQ(joined_result, ActionQueueExecutor::WaitResult::Terminal); + EXPECT_EQ(first_feedback.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(joined_feedback.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ( + first_feedback.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_ACCEPTED_NEW); + EXPECT_EQ( + joined_feedback.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_JOINED_IN_FLIGHT); + EXPECT_EQ(action_arm_->motionCalls(), 1); +} + +TEST_F(GrpcSystemServiceTest, + ConcurrentActionQueuesRemainFifoAndHoldTheControlLease) +{ + initializeActionDevices(); + action_arm_->blockNextMotion(); + + api::ActionQueueCommand_Request first_request; + first_request.set_action_id("fifo-first"); + addMoveLStep(first_request, "first-1", action_arm_->id(), 10.0); + addMoveLStep(first_request, "first-2", action_arm_->id(), 11.0); + api::ActionQueueCommand_Request second_request; + second_request.set_action_id("fifo-second"); + addMoveLStep(second_request, "second-1", action_arm_->id(), 20.0); + + auto first = std::async( + std::launch::async, + [this, first_request]() { return executeAction(first_request); }); + const bool started = action_arm_->waitForMotionCalls( + 1, std::chrono::milliseconds(500)); + auto second = std::async( + std::launch::async, + [this, second_request]() { return executeAction(second_request); }); + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + + const auto competing = + control::ControlAuthorityManager::instance().tryAcquire( + action_arm_->id(), "ordinary-control", + std::chrono::seconds(1)); + if (competing.acquired) { + // Keep a failed assertion from stranding the worker behind this test + // lease and turning the diagnostic into a long timeout. + control::ControlAuthorityManager::instance().release( + competing.token); + } + action_arm_->releaseBlockedMotion(); + const auto first_response = first.get(); + const auto second_response = second.get(); + + EXPECT_TRUE(started); + EXPECT_FALSE(competing.acquired); + EXPECT_EQ(first_response.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(second_response.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(action_arm_->maxActiveMotions(), 1); + EXPECT_EQ( + action_trace_->names(), + (std::vector{ + "arm:L:10", "arm:L:11", "arm:L:20"})); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueTotalAndStepTimeoutsIssueTypedArmStop) +{ + initializeActionDevices(); + + action_arm_->blockNextMotion(); + api::ActionQueueCommand_Request total_timeout; + total_timeout.set_action_id("total-timeout"); + total_timeout.set_total_timeout_ms(40U); + addMoveLStep( + total_timeout, "total", action_arm_->id(), 1.0); + const auto total_response = executeAction(total_timeout); + const int stops_after_total = action_arm_->stopMotionCalls(); + + action_arm_->blockNextMotion(); + api::ActionQueueCommand_Request step_timeout; + step_timeout.set_action_id("step-timeout"); + step_timeout.set_total_timeout_ms(500U); + addMoveLStep( + step_timeout, "step", action_arm_->id(), 2.0, 30U); + const auto step_response = executeAction(step_timeout); + + EXPECT_EQ(total_response.result(), api::ACTION_RESULT_CODE_TIMED_OUT); + EXPECT_EQ(total_response.completed_steps(), 0U); + ASSERT_TRUE(total_response.has_failed_step_index()); + EXPECT_EQ(total_response.failed_step_index(), 0U); + EXPECT_GE(stops_after_total, 1); + EXPECT_EQ(step_response.result(), api::ACTION_RESULT_CODE_TIMED_OUT); + EXPECT_EQ(step_response.completed_steps(), 0U); + ASSERT_TRUE(step_response.has_failed_step_index()); + EXPECT_EQ(step_response.failed_step_index(), 0U); + EXPECT_GT(action_arm_->stopMotionCalls(), stops_after_total); +} + +TEST_F(GrpcSystemServiceTest, + DelayedActionCancellationCannotStopSuccessorControlLease) +{ + initializeActionDevices(); + action_arm_->blockNextMotion(); + action_arm_->blockCanceledMotionReturn(); + + api::ActionQueueCommand_Request request; + request.set_action_id("stale-action-stop-must-not-preempt-successor"); + request.set_total_timeout_ms(2000U); + addMoveLStep(request, "active", action_arm_->id(), 1.0); + + auto action = std::async( + std::launch::async, + [this, request]() { return executeAction(request); }); + const bool motion_started = action_arm_->waitForMotionCalls( + 1, std::chrono::milliseconds(500)); + + auto& authority = control::ControlAuthorityManager::instance(); + control::ControlAcquireResult direct_stop; + if (motion_started) { + direct_stop = authority.preemptAcquire( + action_arm_->id(), + "direct-stop-before-successor", + std::chrono::seconds(30)); + } + if (direct_stop.acquired) { + (void)action_arm_->stopMotion(); + } else { + action_arm_->releaseBlockedMotion(); + } + const bool old_driver_ready_to_return = + action_arm_->waitForCanceledMotionReturn( + std::chrono::milliseconds(500)); + if (direct_stop.acquired) { + authority.release(direct_stop.token); + } + + control::ControlAcquireResult successor; + if (old_driver_ready_to_return) { + successor = authority.tryAcquire( + action_arm_->id(), + "successor-move", + std::chrono::seconds(30)); + } + action_arm_->releaseCanceledMotionReturn(); + + const auto action_status = action.wait_for(std::chrono::seconds(1)); + if (action_status != std::future_status::ready) { + service_->prepareForShutdown(); + } + ASSERT_EQ(action_status, std::future_status::ready); + const auto response = action.get(); + const bool successor_still_current = + successor.acquired && authority.validate(successor.token); + if (successor.acquired) { + authority.release(successor.token); + } + + ASSERT_TRUE(motion_started); + ASSERT_TRUE(direct_stop.acquired) << direct_stop.detail; + ASSERT_TRUE(old_driver_ready_to_return); + ASSERT_TRUE(successor.acquired) << successor.detail; + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_CANCELED); + EXPECT_TRUE(successor_still_current); + EXPECT_EQ(action_arm_->stopMotionCalls(), 1); +} + +TEST_F(GrpcSystemServiceTest, + StopAllCancelsActiveActionAndSkipsRemainingSteps) +{ + initializeActionDevices(); + action_arm_->blockNextMotion(); + api::ActionQueueCommand_Request action_request; + action_request.set_action_id("stop-all-action"); + action_request.set_total_timeout_ms(500U); + addMoveLStep(action_request, "active", action_arm_->id(), 1.0); + addMoveLStep(action_request, "must-not-run", action_arm_->id(), 2.0); + + auto action = std::async( + std::launch::async, + [this, action_request]() { return executeAction(action_request); }); + const bool started = action_arm_->waitForMotionCalls( + 1, std::chrono::milliseconds(500)); + + api::StopAllCommand_Request stop_request; + api::StopAllCommand_Feedback stop_response; + grpc::ServerContext stop_context; + const auto stop_status = service_->StopAll( + &stop_context, &stop_request, &stop_response); + const auto action_response = action.get(); + + EXPECT_TRUE(started); + ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); + EXPECT_TRUE(stop_response.header().success()) + << stop_response.header().error_message(); + EXPECT_EQ(action_response.result(), api::ACTION_RESULT_CODE_CANCELED); + EXPECT_EQ(action_response.completed_steps(), 0U); + ASSERT_TRUE(action_response.has_failed_step_index()); + EXPECT_EQ(action_response.failed_step_index(), 0U); + EXPECT_EQ(action_arm_->motionCalls(), 1); + EXPECT_GE(action_arm_->stopMotionCalls(), 1); +} + +TEST_F(GrpcSystemServiceTest, + StopAllFailsClosedWhenAgvStoppedStateIsUnconfirmed) +{ + initializeActionDevices(); + action_agv_->setStoppedConfirmed(false); + + api::StopAllCommand_Request request; + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll( + &context, &request, &response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_FALSE(response.header().success()); + EXPECT_NE( + response.header().error_message().find("remain quarantined"), + std::string::npos); + const auto lease = + control::ControlAuthorityManager::instance().tryAcquire( + action_agv_->id(), + "normal-control-after-unconfirmed-stop-all", + std::chrono::hours(1)); + EXPECT_FALSE(lease.acquired); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueCancellationStopsActiveLaterArmBeforeEarlierArm) +{ + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + action_trace_ = std::make_shared(); + auto earlier_arm = std::make_shared( + "earlier-arm", action_trace_); + auto active_arm = std::make_shared( + "active-arm", action_trace_); + manager.registerDevice(earlier_arm); + manager.registerDevice(active_arm); + service_ = std::make_unique(); + api::GetSystemInfoCommand_Request info_request; + api::GetSystemInfoCommand_Feedback info_response; + grpc::ServerContext info_context; + ASSERT_TRUE(service_->GetSystemInfo( + &info_context, &info_request, &info_response).ok()); + service_instance_id_ = info_response.action_service_instance_id(); + ASSERT_FALSE(service_instance_id_.empty()); + + active_arm->blockNextMotion(); + api::ActionQueueCommand_Request request; + request.set_action_id("cancel-active-later-arm-first"); + request.set_total_timeout_ms(1000U); + addMoveLStep(request, "earlier-completes", earlier_arm->id(), 1.0); + addMoveLStep(request, "active-blocks", active_arm->id(), 2.0); + + auto action = std::async( + std::launch::async, + [this, request]() { return executeAction(request); }); + const bool active_started = active_arm->waitForMotionCalls( + 1, std::chrono::milliseconds(500)); + + service_->prepareForShutdown(); + const auto response = action.get(); + const auto trace = action_trace_->names(); + const auto active_stop = std::find( + trace.begin(), trace.end(), "arm:stop:active-arm"); + const auto earlier_stop = std::find( + trace.begin(), trace.end(), "arm:stop:earlier-arm"); + + ASSERT_TRUE(active_started); + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_CANCELED); + EXPECT_EQ(response.completed_steps(), 1U); + ASSERT_TRUE(response.has_failed_step_index()); + EXPECT_EQ(response.failed_step_index(), 1U); + ASSERT_NE(active_stop, trace.end()); + ASSERT_NE(earlier_stop, trace.end()); + EXPECT_LT(active_stop, earlier_stop); +} + TEST_F(GrpcSystemServiceTest, StopAllStopsRegisteredDevicesAndRevokesOnlyArmLease) { diff --git a/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp index 46a0bf61..74815d2a 100644 --- a/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp +++ b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp @@ -223,6 +223,11 @@ void GrpcServerTask::stop() { { std::lock_guard lock(mutex_); + if (auto* system_service = + dynamic_cast( + system_service_.get())) { + system_service->prepareForShutdown(); + } if (server_) { server_->Shutdown(); } diff --git a/protos/cmvr/api/system_command.proto b/protos/cmvr/api/system_command.proto index c61fa36d..b572d230 100644 --- a/protos/cmvr/api/system_command.proto +++ b/protos/cmvr/api/system_command.proto @@ -1,5 +1,7 @@ syntax = "proto3"; +import "cmvr/api/agv_command.proto"; +import "cmvr/api/arm_command.proto"; import "cmvr/api/common.proto"; package cmvr.api; @@ -103,6 +105,10 @@ message GetSystemInfoCommand { string os = 5; string kernel_version = 6; string architecture = 7; + + // Changes whenever the in-process ActionQueue idempotency ledger is + // recreated. Clients bind submissions and retries to this value. + string action_service_instance_id = 8; } } @@ -141,3 +147,97 @@ message StopAllCommand { CommandHeader.Feedback header = 1; } } + +// Final outcome of one ActionQueue execution. +enum ActionResultCode { + ACTION_RESULT_CODE_UNSPECIFIED = 0; + ACTION_RESULT_CODE_COMPLETED = 1; + ACTION_RESULT_CODE_FAILED = 2; + ACTION_RESULT_CODE_CANCELED = 3; + ACTION_RESULT_CODE_TIMED_OUT = 4; + ACTION_RESULT_CODE_REJECTED = 5; +} + +// Describes whether this RPC admitted a new action or observed an existing +// idempotency record. Clients must not infer this from an error string. +enum ActionDeduplicationStatus { + ACTION_DEDUPLICATION_STATUS_UNSPECIFIED = 0; + ACTION_DEDUPLICATION_STATUS_ACCEPTED_NEW = 1; + ACTION_DEDUPLICATION_STATUS_JOINED_IN_FLIGHT = 2; + ACTION_DEDUPLICATION_STATUS_CACHED_RESULT = 3; + ACTION_DEDUPLICATION_STATUS_RESULT_EVICTED = 4; + ACTION_DEDUPLICATION_STATUS_LEDGER_EXHAUSTED = 5; + ACTION_DEDUPLICATION_STATUS_ACTION_ID_CONFLICT = 6; + ACTION_DEDUPLICATION_STATUS_SERVICE_INSTANCE_MISMATCH = 7; +} + +// Edge-local delay between two device commands. +message DelayAction { + // Delay duration in milliseconds. The server applies a bounded maximum. + uint32 duration_ms = 1; +} + +// One finite, synchronous command in an ActionQueue request. +message ActionStep { + // Client-provided identifier used for diagnostics. It must be unique within + // one ActionQueue request. + string step_id = 1; + + // Per-step timeout in milliseconds. Zero inherits the remaining action + // timeout or the server default. + uint32 timeout_ms = 2; + + // Tags are grouped by domain so compatible commands can be added without + // renumbering existing alternatives: Arm 10-19, AGV 20-29, built-ins 90+. + oneof command { + MoveJ.Request arm_move_j = 10; + MoveL.Request arm_move_l = 11; + + AgvNavigateToPoseCommand.Request agv_navigate_to_pose = 20; + AgvNavigateToStationCommand.Request agv_navigate_to_station = 21; + AgvFollowPathCommand.Request agv_follow_path = 22; + + DelayAction delay = 90; + } +} + +// Atomically submits a complete command sequence for edge-local serial +// execution. Device motion alternatives must use synchronous execution. +message ActionQueueCommand { + message Request { + // Client-generated globally unique idempotency key. During one Action + // service instance, retrying an identical accepted request with the + // same action_id does not dispatch its steps a second time. Recent + // terminal results can be returned; older accepted IDs are rejected + // fail-closed after their result is evicted. Deduplication is not + // persisted across an edge-service restart; the required instance + // epoch below prevents an old retry from being replayed after restart. + string action_id = 1; + repeated ActionStep steps = 2; + + // Total queue-wait plus execution timeout in milliseconds. Zero uses a + // bounded server default. + uint32 total_timeout_ms = 3; + + // Required instance epoch obtained from GetSystemInfo. A mismatch + // means the process-local deduplication ledger was recreated, so the + // server rejects the request instead of risking a replay. + string expected_service_instance_id = 4; + } + + message Feedback { + CommandHeader.Feedback header = 1; + string action_id = 2; + + // Number of steps which completed successfully before the final result. + uint32 completed_steps = 3; + + // Present only when a particular step caused failure, cancellation, + // timeout, or rejection. Presence distinguishes index zero from no + // failed step. + optional uint32 failed_step_index = 4; + ActionResultCode result = 5; + string service_instance_id = 6; + ActionDeduplicationStatus deduplication_status = 7; + } +} diff --git a/protos/cmvr/api/system_service.proto b/protos/cmvr/api/system_service.proto index 83f19afd..bb6dc4b8 100644 --- a/protos/cmvr/api/system_service.proto +++ b/protos/cmvr/api/system_service.proto @@ -13,4 +13,6 @@ service SystemService { rpc UpdateParams(UpdateParamsCommand.Request) returns (UpdateParamsCommand.Feedback) {} rpc StopAll(StopAllCommand.Request) returns (StopAllCommand.Feedback) {} + + rpc ExecuteActionQueue(ActionQueueCommand.Request) returns (ActionQueueCommand.Feedback) {} }