add grpc action queue support

cmvr-es/common/types/arm/arm_types.h

@@ -2,6 +2,7 @@
 #define CMVR_ES_ARM_TYPES_H
 
 #include <cstdint>
+#include <functional>
 #include <string>
 #include <vector>
 
@@ -157,6 +158,9 @@ struct MotionOptions {
     double jerk{5.0};
     std::vector<double> joint_velocity_limits;
     bool asynchronous{false};
+    // Optional cooperative cancellation used by synchronous ActionQueue
+    // motion. Drivers must not retain this callback after the command returns.
+    std::function<bool()> cancellation_requested;
 };
 
 struct ServoOptions {

cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h

@@ -32,6 +32,7 @@ public:
     RobotMode getRobotMode() const override;
     SafetyMode getSafetyMode() const override;
     ControlMode getControlMode() const override { return ControlMode::Position; }
+    bool supportsActionQueueMotion() const noexcept override { return true; }
     Result listBaseFrame(std::vector<std::string>& frame_names) const override;
     Result listTCPFrame(std::vector<std::string>& frame_names) const override;
 

cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp

@@ -49,11 +49,40 @@ constexpr auto kAutoEnableReconnectInterval = std::chrono::milliseconds(500);
 constexpr auto kAutoEnableRetryInterval = std::chrono::seconds(1);
 constexpr auto kAutoEnableModeTimeout = std::chrono::seconds(10);
 
-int waitArrival(const arcs::aubo_sdk::RobotInterfacePtr& robot_interface)
+int waitArrival(
+    const arcs::aubo_sdk::RobotInterfacePtr& robot_interface,
+    const std::function<bool()>& cancellation_requested = {})
 {
+    const auto deadline =
+        std::chrono::steady_clock::now() + std::chrono::seconds(60);
+    const auto canceled = [&]() {
+        return cancellation_requested && cancellation_requested();
+    };
+    const auto hardwareEmergencyStopActive = [&]() {
+        const auto mode = robot_interface->getRobotState()->getSafetyModeType();
+        const int source = robot_interface->getRobotConfig()
+            ->getRobotEmergencyStopSource();
+        using arcs::common_interface::SafetyModeType;
+        return mode == SafetyModeType::RobotEmergencyStop ||
+               mode == SafetyModeType::SystemEmergencyStop || source != 0;
+    };
+    const auto stopMotion = [&]() {
+        try {
+            (void)robot_interface->getMotionControl()->stopMove(true, true);
+        } catch (...) {
+        }
+    };
+
     int retry_count = 0;
     int exec_id = robot_interface->getMotionControl()->getExecId();
     while (exec_id == -1 && retry_count++ < 5) {
+        if (canceled()) {
+            stopMotion();
+            return -2;
+        }
+        if (hardwareEmergencyStopActive()) {
+            return -3;
+        }
         std::this_thread::sleep_for(std::chrono::milliseconds(50));
         exec_id = robot_interface->getMotionControl()->getExecId();
     }
@@ -61,6 +90,17 @@ int waitArrival(const arcs::aubo_sdk::RobotInterfacePtr& robot_interface)
         return -1;
     }
     while (robot_interface->getMotionControl()->getExecId() != -1) {
+        if (canceled()) {
+            stopMotion();
+            return -2;
+        }
+        if (hardwareEmergencyStopActive()) {
+            return -3;
+        }
+        if (std::chrono::steady_clock::now() >= deadline) {
+            stopMotion();
+            return -4;
+        }
         std::this_thread::sleep_for(std::chrono::milliseconds(50));
     }
     return 0;
@@ -513,6 +553,10 @@ Result AuboArm::setSpeedScaling(const double scaling)
 
 Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& options)
 {
+    if (options.cancellation_requested && options.cancellation_requested()) {
+        return Result::failure(ArmErrorCode::CommandRejected,
+                               "[AuboArm] moveJ canceled before dispatch");
+    }
     std::string error;
     if (!validDof_(target.position.size(), error)) {
         return Result::failure(ArmErrorCode::InvalidDof, error);
@@ -548,7 +592,10 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o
             ret,
             arcs::common_interface::AUBO_OK,
             arcs::common_interface::AUBO_REQUEST_IGNORE,
-            [&robot_interface]() { return waitArrival(robot_interface); });
+            [&robot_interface, &options]() {
+                return waitArrival(
+                    robot_interface, options.cancellation_requested);
+            });
         if (outcome == aubo_internal::MotionCommandOutcome::CompletedWithoutMotion
             || outcome == aubo_internal::MotionCommandOutcome::CompletedAfterMotion) {
             return Result::success();
@@ -596,6 +643,10 @@ Result AuboArm::moveL(const CartesianPose& target,
                       const std::string& base_frame,
                       const std::string& tcp_frame)
 {
+    if (options.cancellation_requested && options.cancellation_requested()) {
+        return Result::failure(ArmErrorCode::CommandRejected,
+                               "[AuboArm] moveL canceled before dispatch");
+    }
     const auto ready = ensureMotionReady_("moveL");
     if (!ready.ok()) {
         return ready;
@@ -691,7 +742,10 @@ Result AuboArm::moveL(const CartesianPose& target,
             ret,
             arcs::common_interface::AUBO_OK,
             arcs::common_interface::AUBO_REQUEST_IGNORE,
-            [&robot_interface]() { return waitArrival(robot_interface); });
+            [&robot_interface, &options]() {
+                return waitArrival(
+                    robot_interface, options.cancellation_requested);
+            });
         if (outcome == aubo_internal::MotionCommandOutcome::CompletedWithoutMotion
             || outcome == aubo_internal::MotionCommandOutcome::CompletedAfterMotion) {
             return Result::success();

cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp

@@ -338,6 +338,10 @@ Result HuayanRobot::listTCPFrame(
 
 Result HuayanRobot::moveJ(const JointPositionCommand& target, const MotionOptions& options)
 {
+    if (options.cancellation_requested && options.cancellation_requested()) {
+        return Result::failure(ArmErrorCode::CommandRejected,
+                               "[HuayanRobot] moveJ canceled before dispatch");
+    }
     std::string error;
     if (!validDof_(target.position.size(), error)) {
         return Result::failure(ArmErrorCode::InvalidDof, error);
@@ -369,7 +373,8 @@ Result HuayanRobot::moveJ(const JointPositionCommand& target, const MotionOption
         busy_.store(false);
         return hrResult_(ret, "moveJ");
     }
-    const auto wait_result = waitMotionDone_("moveJ", 60000);
+    const auto wait_result = waitMotionDone_(
+        "moveJ", 60000, options.cancellation_requested);
     busy_.store(false);
     return wait_result;
 }
@@ -422,6 +427,10 @@ Result HuayanRobot::moveL(const CartesianPose& target,
                           const std::string& base_frame,
                           const std::string& tcp_frame)
 {
+    if (options.cancellation_requested && options.cancellation_requested()) {
+        return Result::failure(ArmErrorCode::CommandRejected,
+                               "[HuayanRobot] moveL canceled before dispatch");
+    }
     const auto ready = ensureMotionReady_("moveL");
     if (!ready.ok()) {
         return ready;
@@ -451,7 +460,8 @@ Result HuayanRobot::moveL(const CartesianPose& target,
         busy_.store(false);
         return hrResult_(ret, "moveL");
     }
-    const auto wait_result = waitMotionDone_("moveL", 60000);
+    const auto wait_result = waitMotionDone_(
+        "moveL", 60000, options.cancellation_requested);
     busy_.store(false);
     return wait_result;
 }
@@ -1095,10 +1105,19 @@ std::string HuayanRobot::nextCommandId_() const
     return id_ + "_" + std::to_string(++command_seq_);
 }
 
-Result HuayanRobot::waitMotionDone_(const std::string& context, const int timeout_ms) const
+Result HuayanRobot::waitMotionDone_(
+    const std::string& context,
+    const int timeout_ms,
+    const std::function<bool()>& cancellation_requested) const
 {
     const auto start = std::chrono::steady_clock::now();
     while (true) {
+        if (cancellation_requested && cancellation_requested()) {
+            (void)HRIF_GrpStop(box_id_, robot_id_);
+            return Result::failure(
+                ArmErrorCode::CommandRejected,
+                "[HuayanRobot] " + context + " canceled");
+        }
         bool done = false;
         const int ret = HRIF_IsMotionDone(box_id_, robot_id_, done);
         if (ret != 0) {
@@ -1146,7 +1165,7 @@ Result HuayanRobot::waitMotionDone_(const std::string& context, const int timeou
                 "[HuayanRobot] " + context + " timeout");
         }
 
-        std::this_thread::sleep_for(std::chrono::milliseconds(500));
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
     }
 }
 

cmvr-es/devices/arm/huayan_arm/huayan_arm.h

@@ -10,6 +10,7 @@
 
 #include <atomic>
 #include <condition_variable>
+#include <functional>
 #include <memory>
 #include <mutex>
 #include <string>
@@ -38,6 +39,7 @@ public:
     RobotMode getRobotMode() const override;
     SafetyMode getSafetyMode() const override;
     ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; }
+    bool supportsActionQueueMotion() const noexcept override { return true; }
     Result listBaseFrame(std::vector<std::string>& frame_names) const override;
     Result listTCPFrame(std::vector<std::string>& frame_names) const override;
 
@@ -130,7 +132,9 @@ private:
     CartesianVelocity readTcpVelocity_() const;
     std::vector<double> currentJointPositionDeg_() const;
     std::string nextCommandId_() const;
-    Result waitMotionDone_(const std::string& context, int timeout_ms) const;
+    Result waitMotionDone_(const std::string& context,
+                           int timeout_ms,
+                           const std::function<bool()>& cancellation_requested = {}) const;
     void startAutoEnableMonitor_();
     void stopAutoEnableMonitor_();
     void autoEnableMonitorLoop_();

cmvr-es/devices/arm/robot_arm.h

@@ -27,6 +27,9 @@ public:
     virtual RobotMode getRobotMode() const = 0;
     virtual SafetyMode getSafetyMode() const = 0;
     virtual ControlMode getControlMode() const = 0;
+    // ActionQueue requires synchronous motion and cooperative cancellation.
+    // Backends opt in only after both semantics are implemented.
+    virtual bool supportsActionQueueMotion() const noexcept { return false; }
     virtual Result listBaseFrame(std::vector<std::string>& frame_names) const
     {
         frame_names.clear();

cmvr-es/service/CMakeLists.txt

@@ -1,5 +1,7 @@
 
 add_library(service
+        grpc/action/src/action_queue.cpp
+        grpc/action/src/control_command_arbiter.cpp
         grpc/src/grpc_camera_service.cpp
         grpc/src/grpc_system_service.cpp
         grpc/src/grpc_speaker_service.cpp
@@ -27,6 +29,25 @@ target_link_libraries(service PRIVATE
 add_library(cmvr_es::service ALIAS service)
 install(TARGETS service LIBRARY DESTINATION lib)
 
+if(BUILD_TESTING)
+    enable_testing()
+    add_executable(action_queue_test
+            grpc/action/tests/action_queue_test.cpp
+            grpc/action/src/action_queue.cpp
+            grpc/action/src/control_command_arbiter.cpp)
+    target_link_libraries(action_queue_test PRIVATE
+            cmvr_es::proto
+            cmvr_es::logging
+            gtest
+            gtest_main
+            pthread)
+    set_target_properties(action_queue_test PROPERTIES
+            BUILD_RPATH "${CMAKE_BINARY_DIR};${CMAKE_SOURCE_DIR}/output/lib;${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/grpc/v1.76.0/lib")
+    add_test(NAME action_queue_test COMMAND action_queue_test)
+    set_tests_properties(action_queue_test PROPERTIES
+            ENVIRONMENT "LD_LIBRARY_PATH=${CMAKE_BINARY_DIR}:${CMAKE_SOURCE_DIR}/output/lib:${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/grpc/v1.76.0/lib")
+endif()
+
 # --------------------------------------------------------
 # Unit test
 # --------------------------------------------------------

cmvr-es/service/grpc/action/include/action_queue.h

@@ -0,0 +1,60 @@
+#ifndef CMVR_ES_ACTION_QUEUE_H
+#define CMVR_ES_ACTION_QUEUE_H
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include "cmvr/api/system_command.pb.h"
+
+namespace cmvr::device {
+class RobotArm;
+}
+
+namespace cmvr::service {
+
+// Executes one submitted sequence at a time. The RPC waits synchronously for
+// the terminal result, while StopAll may cancel the active sequence from a
+// different gRPC handler.
+class ActionQueue final {
+public:
+    using ArmResolver = std::function<std::shared_ptr<device::RobotArm>(
+        const std::string&)>;
+
+    enum class State {
+        Idle,
+        Validating,
+        Running,
+        Stopping,
+        Completed,
+        Failed,
+        Canceled,
+        TimedOut,
+        Rejected,
+        ShuttingDown,
+    };
+
+    explicit ActionQueue(ArmResolver arm_resolver);
+    ~ActionQueue();
+
+    ActionQueue(const ActionQueue&) = delete;
+    ActionQueue& operator=(const ActionQueue&) = delete;
+
+    void execute(const api::ActionQueueCommand_Request& request,
+                 api::ActionQueueCommand_Feedback& feedback);
+
+    // Immediately removes every not-yet-dispatched step and requests a typed
+    // stop for the active arm. No ActionQueue mutex is held across that call.
+    void cancelAndClear();
+    void shutdown();
+
+    State state() const;
+
+private:
+    struct Impl;
+    std::unique_ptr<Impl> impl_;
+};
+
+} // namespace cmvr::service
+
+#endif // CMVR_ES_ACTION_QUEUE_H

cmvr-es/service/grpc/action/include/control_command_arbiter.h

@@ -0,0 +1,66 @@
+#ifndef CMVR_ES_CONTROL_COMMAND_ARBITER_H
+#define CMVR_ES_CONTROL_COMMAND_ARBITER_H
+
+#include <cstddef>
+#include <mutex>
+
+namespace cmvr::service {
+
+// Process-local admission gate for gRPC control commands. Ordinary controls
+// may overlap each other, but ActionQueue owns the control domain exclusively.
+// Stop leases are preemptive: they never wait for the current owner and block
+// all new admission until the stop operation returns.
+class ControlCommandArbiter final {
+public:
+    enum class LeaseKind {
+        None,
+        Control,
+        ActionQueue,
+        Stop,
+    };
+
+    class Lease final {
+    public:
+        Lease() = default;
+        ~Lease();
+
+        Lease(const Lease&) = delete;
+        Lease& operator=(const Lease&) = delete;
+        Lease(Lease&& other) noexcept;
+        Lease& operator=(Lease&& other) noexcept;
+
+        explicit operator bool() const noexcept { return owner_ != nullptr; }
+        void reset() noexcept;
+
+    private:
+        friend class ControlCommandArbiter;
+        Lease(ControlCommandArbiter* owner, LeaseKind kind)
+            : owner_(owner), kind_(kind)
+        {
+        }
+
+        ControlCommandArbiter* owner_{nullptr};
+        LeaseKind kind_{LeaseKind::None};
+    };
+
+    static ControlCommandArbiter& instance();
+
+    Lease tryAcquireControl();
+    Lease tryAcquireActionQueue();
+    Lease beginStop();
+
+    bool actionQueueActive() const;
+    std::size_t activeControlCount() const;
+
+private:
+    void release(LeaseKind kind) noexcept;
+
+    mutable std::mutex mutex_;
+    std::size_t active_controls_{0};
+    std::size_t active_stops_{0};
+    bool action_queue_active_{false};
+};
+
+} // namespace cmvr::service
+
+#endif // CMVR_ES_CONTROL_COMMAND_ARBITER_H

cmvr-es/service/grpc/action/include/control_command_guard.h

@@ -0,0 +1,45 @@
+#ifndef CMVR_ES_CONTROL_COMMAND_GUARD_H
+#define CMVR_ES_CONTROL_COMMAND_GUARD_H
+
+#include <string>
+
+#include <grpcpp/grpcpp.h>
+
+#include "cmvr/api/common.pb.h"
+#include "common/base/grpc_utils.h"
+
+namespace cmvr::service {
+
+inline const std::string& controlCommandBusyMessage()
+{
+    static const std::string message =
+        "control command rejected while ActionQueue or StopAll is active";
+    return message;
+}
+
+inline grpc::Status rejectControlCommand(api::CommandHeader_Feedback* response)
+{
+    response->set_success(false);
+    response->set_error_message(controlCommandBusyMessage());
+    setCurrentTimestamp(response->mutable_timestamp());
+    return grpc::Status(
+        grpc::StatusCode::FAILED_PRECONDITION,
+        controlCommandBusyMessage());
+}
+
+template <typename Response>
+grpc::Status rejectControlCommand(Response* response)
+{
+    return rejectControlCommand(response->mutable_header());
+}
+
+inline grpc::Status rejectControlCommand()
+{
+    return grpc::Status(
+        grpc::StatusCode::FAILED_PRECONDITION,
+        controlCommandBusyMessage());
+}
+
+} // namespace cmvr::service
+
+#endif // CMVR_ES_CONTROL_COMMAND_GUARD_H

cmvr-es/service/grpc/action/src/action_queue.cpp

@@ -0,0 +1,575 @@
+#include "service/grpc/action/include/action_queue.h"
+
+#include <algorithm>
+#include <atomic>
+#include <chrono>
+#include <cmath>
+#include <cstddef>
+#include <cstdint>
+#include <deque>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include <google/protobuf/util/time_util.h>
+
+#include "common/base/logging/logger.h"
+#include "devices/arm/robot_arm.h"
+#include "service/grpc/action/include/control_command_arbiter.h"
+
+namespace cmvr::service {
+
+namespace {
+
+constexpr std::size_t kMaximumStepCount = 100;
+constexpr auto kDefaultTotalTimeout = std::chrono::minutes(5);
+constexpr auto kMaximumTimeout = std::chrono::minutes(30);
+constexpr auto kMaximumTimeoutMs =
+    std::chrono::duration_cast<std::chrono::milliseconds>(kMaximumTimeout)
+        .count();
+
+bool finiteNonNegative(const double value)
+{
+    return std::isfinite(value) && value >= 0.0;
+}
+
+device::JointPositionCommand toJointPosition(
+    const api::JointPositionCommand& source)
+{
+    device::JointPositionCommand target;
+    target.position.assign(source.position().begin(), source.position().end());
+    return target;
+}
+
+device::MotionOptions toMotionOptions(const api::MotionOptions& source)
+{
+    device::MotionOptions options;
+    options.velocity = source.velocity();
+    options.acceleration = source.acceleration();
+    options.blend_radius = source.blend_radius();
+    options.jerk = source.jerk() > 0.0 ? source.jerk() : 5.0;
+    options.joint_velocity_limits.assign(
+        source.joint_velocity_limits().begin(),
+        source.joint_velocity_limits().end());
+    options.asynchronous = source.asynchronous();
+    return options;
+}
+
+device::CartesianPose toCartesianPose(const api::CartesianPose& source)
+{
+    return {source.x(), source.y(), source.z(),
+            source.rx(), source.ry(), source.rz()};
+}
+
+device::FrameType toFrameType(const api::ArmFrameType frame)
+{
+    switch (frame) {
+    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;
+    case api::ARM_FRAME_BASE:
+    default:
+        return device::FrameType::Base;
+    }
+}
+
+bool validMotionOptions(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 = "motion options must be finite and non-negative";
+        return false;
+    }
+    for (const double limit : options.joint_velocity_limits()) {
+        if (!finiteNonNegative(limit)) {
+            error = "joint velocity limits must be finite and non-negative";
+            return false;
+        }
+    }
+    return true;
+}
+
+void finishFeedback(api::ActionQueueCommand_Feedback& feedback,
+                    const api::ActionResultCode result,
+                    const std::uint32_t completed_steps,
+                    const std::string& error,
+                    const int failed_step_index = -1)
+{
+    feedback.Clear();
+    feedback.set_result(result);
+    feedback.set_completed_steps(completed_steps);
+    if (failed_step_index >= 0) {
+        feedback.set_failed_step_index(
+            static_cast<std::uint32_t>(failed_step_index));
+    }
+    auto* header = feedback.mutable_header();
+    header->set_success(result == api::ACTION_RESULT_CODE_COMPLETED);
+    header->set_error_message(error);
+    *header->mutable_timestamp() =
+        google::protobuf::util::TimeUtil::GetCurrentTime();
+}
+
+} // namespace
+
+struct ActionQueue::Impl {
+    struct PreparedStep {
+        api::ActionStep source;
+        std::shared_ptr<device::RobotArm> arm;
+        std::size_t source_index{0};
+    };
+
+    struct CommandHandler {
+        std::function<bool(const api::ActionStep&,
+                           std::size_t,
+                           PreparedStep&,
+                           std::string&)> prepare;
+        std::function<device::Result(
+            const PreparedStep&,
+            const std::function<bool()>&)> execute;
+    };
+
+    explicit Impl(ActionQueue::ArmResolver resolver)
+        : arm_resolver(std::move(resolver))
+    {
+        registerCommands();
+    }
+
+    void registerCommands()
+    {
+        handlers.emplace(
+            api::ActionStep::kArmMoveJ,
+            CommandHandler{
+                [this](const api::ActionStep& step,
+                       const std::size_t index,
+                       PreparedStep& prepared,
+                       std::string& error) {
+                    const auto& command = step.arm_move_j();
+                    return prepareArmStep(
+                        step, command.header().device_id(), index,
+                        command.options(), prepared, error,
+                        [&command, &error](const device::RobotArm& arm) {
+                            if (command.target().position_size() !=
+                                static_cast<int>(arm.getDof())) {
+                                error = "MoveJ target size does not match arm DOF";
+                                return false;
+                            }
+                            for (const double value : command.target().position()) {
+                                if (!std::isfinite(value)) {
+                                    error = "MoveJ target must contain finite values";
+                                    return false;
+                                }
+                            }
+                            return true;
+                        });
+                },
+                [](const PreparedStep& prepared,
+                   const std::function<bool()>& canceled) {
+                    const auto& command = prepared.source.arm_move_j();
+                    auto options = toMotionOptions(command.options());
+                    options.cancellation_requested = canceled;
+                    return prepared.arm->moveJ(
+                        toJointPosition(command.target()), options);
+                }});
+
+        handlers.emplace(
+            api::ActionStep::kArmMoveL,
+            CommandHandler{
+                [this](const api::ActionStep& step,
+                       const std::size_t index,
+                       PreparedStep& prepared,
+                       std::string& error) {
+                    const auto& command = step.arm_move_l();
+                    return prepareArmStep(
+                        step, command.header().device_id(), index,
+                        command.options(), prepared, error,
+                        [&command, &error](const device::RobotArm&) {
+                            const auto& pose = command.target();
+                            if (!std::isfinite(pose.x()) ||
+                                !std::isfinite(pose.y()) ||
+                                !std::isfinite(pose.z()) ||
+                                !std::isfinite(pose.rx()) ||
+                                !std::isfinite(pose.ry()) ||
+                                !std::isfinite(pose.rz())) {
+                                error = "MoveL target must contain finite values";
+                                return false;
+                            }
+                            return true;
+                        });
+                },
+                [](const PreparedStep& prepared,
+                   const std::function<bool()>& canceled) {
+                    const auto& command = prepared.source.arm_move_l();
+                    auto options = toMotionOptions(command.options());
+                    options.cancellation_requested = canceled;
+                    const auto target = toCartesianPose(command.target());
+                    const bool named_frame =
+                        (command.has_base_frame() &&
+                         !command.base_frame().empty()) ||
+                        (command.has_tcp_frame() &&
+                         !command.tcp_frame().empty());
+                    if (named_frame) {
+                        return prepared.arm->moveL(
+                            target,
+                            options,
+                            command.has_base_frame()
+                                ? command.base_frame()
+                                : std::string{},
+                            command.has_tcp_frame()
+                                ? command.tcp_frame()
+                                : std::string{});
+                    }
+                    return prepared.arm->moveL(
+                        target, options, toFrameType(command.frame()));
+                }});
+    }
+
+    template <typename ExtraValidator>
+    bool prepareArmStep(const api::ActionStep& step,
+                        const std::string& device_id,
+                        const std::size_t index,
+                        const api::MotionOptions& options,
+                        PreparedStep& prepared,
+                        std::string& error,
+                        ExtraValidator&& validate)
+    {
+        if (device_id.empty()) {
+            error = "RobotArm device_id is required";
+            return false;
+        }
+        auto arm = arm_resolver(device_id);
+        if (!arm) {
+            error = "RobotArm device not found: " + device_id;
+            return false;
+        }
+        if (!arm->supportsActionQueueMotion()) {
+            error = "RobotArm backend does not support ActionQueue motion: " +
+                    device_id;
+            return false;
+        }
+        if (!validMotionOptions(options, error) || !validate(*arm)) {
+            return false;
+        }
+        prepared.source = step;
+        prepared.arm = std::move(arm);
+        prepared.source_index = index;
+        return true;
+    }
+
+    bool prepare(const api::ActionQueueCommand_Request& request,
+                 std::vector<PreparedStep>& prepared,
+                 std::string& error,
+                 int& failed_index)
+    {
+        if (request.steps_size() == 0) {
+            error = "ActionQueue requires at least one step";
+            return false;
+        }
+        if (request.steps_size() > static_cast<int>(kMaximumStepCount)) {
+            error = "ActionQueue step count exceeds the server limit";
+            return false;
+        }
+        if (request.total_timeout_ms() >
+            static_cast<std::uint64_t>(kMaximumTimeoutMs)) {
+            error = "ActionQueue total timeout exceeds the server limit";
+            return false;
+        }
+
+        prepared.reserve(static_cast<std::size_t>(request.steps_size()));
+        for (int index = 0; index < request.steps_size(); ++index) {
+            const auto& step = request.steps(index);
+            if (step.timeout_ms() >
+                static_cast<std::uint64_t>(kMaximumTimeoutMs)) {
+                error = "step timeout exceeds the server limit";
+                failed_index = index;
+                return false;
+            }
+            const auto handler = handlers.find(step.command_case());
+            if (handler == handlers.end()) {
+                error = "ActionQueue command type is not supported";
+                failed_index = index;
+                return false;
+            }
+            PreparedStep item;
+            if (!handler->second.prepare(
+                    step, static_cast<std::size_t>(index), item, error)) {
+                failed_index = index;
+                return false;
+            }
+            prepared.push_back(std::move(item));
+        }
+        return true;
+    }
+
+    void setState(const State next)
+    {
+        std::lock_guard lock(mutex);
+        state = next;
+    }
+
+    ActionQueue::ArmResolver arm_resolver;
+    std::unordered_map<int, CommandHandler> handlers;
+
+    mutable std::mutex mutex;
+    State state{State::Idle};
+    std::deque<PreparedStep> pending;
+    std::shared_ptr<device::RobotArm> active_arm;
+    std::atomic<bool> stop_requested{false};
+    std::atomic<bool> shutting_down{false};
+};
+
+ActionQueue::ActionQueue(ArmResolver arm_resolver)
+    : impl_(std::make_unique<Impl>(std::move(arm_resolver)))
+{
+}
+
+ActionQueue::~ActionQueue()
+{
+    shutdown();
+}
+
+void ActionQueue::execute(
+    const api::ActionQueueCommand_Request& request,
+    api::ActionQueueCommand_Feedback& feedback)
+{
+    if (impl_->shutting_down.load()) {
+        finishFeedback(feedback, api::ACTION_RESULT_CODE_REJECTED, 0,
+                       "ActionQueue is shutting down");
+        return;
+    }
+
+    auto action_lease =
+        ControlCommandArbiter::instance().tryAcquireActionQueue();
+    if (!action_lease) {
+        finishFeedback(
+            feedback, api::ACTION_RESULT_CODE_REJECTED, 0,
+            "ActionQueue cannot start while another control command is active");
+        return;
+    }
+
+    {
+        std::lock_guard lock(impl_->mutex);
+        impl_->stop_requested.store(false);
+        impl_->state = State::Validating;
+    }
+    std::vector<Impl::PreparedStep> prepared;
+    std::string error;
+    int failed_index = -1;
+    if (!impl_->prepare(request, prepared, error, failed_index)) {
+        if (impl_->stop_requested.load() || impl_->shutting_down.load()) {
+            impl_->setState(State::Canceled);
+            finishFeedback(feedback, api::ACTION_RESULT_CODE_CANCELED, 0,
+                           "ActionQueue was stopped");
+            impl_->setState(impl_->shutting_down.load()
+                ? State::ShuttingDown
+                : State::Idle);
+            return;
+        }
+        impl_->setState(State::Rejected);
+        finishFeedback(feedback, api::ACTION_RESULT_CODE_REJECTED, 0,
+                       error, failed_index);
+        impl_->setState(State::Idle);
+        return;
+    }
+
+    {
+        std::lock_guard lock(impl_->mutex);
+        if (impl_->stop_requested.load() || impl_->shutting_down.load()) {
+            impl_->state = impl_->shutting_down.load()
+                ? State::ShuttingDown
+                : State::Idle;
+            finishFeedback(feedback, api::ACTION_RESULT_CODE_CANCELED, 0,
+                           "ActionQueue was stopped");
+            return;
+        }
+        impl_->pending.assign(
+            std::make_move_iterator(prepared.begin()),
+            std::make_move_iterator(prepared.end()));
+        impl_->active_arm.reset();
+        impl_->state = State::Running;
+    }
+
+    const auto total_timeout = request.total_timeout_ms() == 0
+        ? kDefaultTotalTimeout
+        : std::chrono::milliseconds(request.total_timeout_ms());
+    const auto total_deadline = std::chrono::steady_clock::now() +
+                                total_timeout;
+    std::uint32_t completed_steps = 0;
+
+    while (true) {
+        Impl::PreparedStep step;
+        {
+            std::lock_guard lock(impl_->mutex);
+            if (impl_->stop_requested.load() || impl_->shutting_down.load()) {
+                impl_->state = State::Canceled;
+                finishFeedback(feedback, api::ACTION_RESULT_CODE_CANCELED,
+                               completed_steps, "ActionQueue was stopped");
+                impl_->active_arm.reset();
+                impl_->state = impl_->shutting_down.load()
+                    ? State::ShuttingDown
+                    : State::Idle;
+                return;
+            }
+            if (impl_->pending.empty()) {
+                impl_->state = State::Completed;
+                finishFeedback(feedback, api::ACTION_RESULT_CODE_COMPLETED,
+                               completed_steps, {});
+                impl_->active_arm.reset();
+                impl_->state = State::Idle;
+                return;
+            }
+            if (std::chrono::steady_clock::now() >= total_deadline) {
+                impl_->pending.clear();
+                impl_->state = State::TimedOut;
+                finishFeedback(feedback, api::ACTION_RESULT_CODE_TIMED_OUT,
+                               completed_steps,
+                               "ActionQueue total timeout expired");
+                impl_->state = State::Idle;
+                return;
+            }
+            step = std::move(impl_->pending.front());
+            impl_->pending.pop_front();
+            impl_->active_arm = step.arm;
+        }
+
+        auto step_deadline = total_deadline;
+        if (step.source.timeout_ms() != 0) {
+            step_deadline = std::min(
+                step_deadline,
+                std::chrono::steady_clock::now() +
+                    std::chrono::milliseconds(step.source.timeout_ms()));
+        }
+        const auto canceled = [this, step_deadline]() {
+            return impl_->stop_requested.load() ||
+                   impl_->shutting_down.load() ||
+                   std::chrono::steady_clock::now() >= step_deadline;
+        };
+
+        device::Result result;
+        try {
+            const auto handler = impl_->handlers.find(
+                step.source.command_case());
+            result = handler->second.execute(step, canceled);
+        } catch (const std::exception& exception) {
+            result = device::Result::failure(
+                device::ArmErrorCode::CommandFailed, exception.what());
+        } catch (...) {
+            result = device::Result::failure(
+                device::ArmErrorCode::CommandFailed,
+                "ActionQueue command threw an unknown exception");
+        }
+
+        {
+            std::lock_guard lock(impl_->mutex);
+            impl_->active_arm.reset();
+            if (impl_->stop_requested.load() || impl_->shutting_down.load()) {
+                impl_->pending.clear();
+                impl_->state = State::Canceled;
+                finishFeedback(
+                    feedback, api::ACTION_RESULT_CODE_CANCELED,
+                    completed_steps, "ActionQueue was stopped",
+                    static_cast<int>(step.source_index));
+                impl_->state = impl_->shutting_down.load()
+                    ? State::ShuttingDown
+                    : State::Idle;
+                return;
+            }
+            if (std::chrono::steady_clock::now() >= step_deadline) {
+                impl_->pending.clear();
+                impl_->state = State::TimedOut;
+                finishFeedback(
+                    feedback, api::ACTION_RESULT_CODE_TIMED_OUT,
+                    completed_steps, "ActionQueue step timeout expired",
+                    static_cast<int>(step.source_index));
+                impl_->state = State::Idle;
+                return;
+            }
+            if (!result.ok()) {
+                impl_->pending.clear();
+                impl_->state = State::Failed;
+                finishFeedback(
+                    feedback, api::ACTION_RESULT_CODE_FAILED,
+                    completed_steps, result.message,
+                    static_cast<int>(step.source_index));
+                impl_->state = State::Idle;
+                return;
+            }
+            ++completed_steps;
+        }
+    }
+}
+
+void ActionQueue::cancelAndClear()
+{
+    std::shared_ptr<device::RobotArm> active_arm;
+    {
+        std::lock_guard lock(impl_->mutex);
+        if (impl_->state == State::Idle ||
+            impl_->state == State::Rejected ||
+            impl_->state == State::Completed ||
+            impl_->state == State::Failed ||
+            impl_->state == State::Canceled ||
+            impl_->state == State::TimedOut) {
+            return;
+        }
+        impl_->stop_requested.store(true);
+        impl_->pending.clear();
+        impl_->state = impl_->shutting_down.load()
+            ? State::ShuttingDown
+            : State::Stopping;
+        active_arm = impl_->active_arm;
+    }
+
+    // Never call a device SDK while holding the ActionQueue mutex. This keeps
+    // hardware emergency-stop and auto-enable monitor callbacks from forming a
+    // lock cycle with StopAll.
+    if (active_arm) {
+        try {
+            const auto result = active_arm->stopMotion();
+            if (!result.ok()) {
+                CMVR_LOG(WARNING)
+                    << "[ActionQueue] active arm stop was not confirmed: "
+                    << result.message;
+            }
+        } catch (const std::exception& error) {
+            CMVR_LOG(WARNING)
+                << "[ActionQueue] active arm stop threw: " << error.what();
+        } catch (...) {
+            CMVR_LOG(WARNING)
+                << "[ActionQueue] active arm stop threw an unknown exception";
+        }
+    }
+}
+
+void ActionQueue::shutdown()
+{
+    if (!impl_) {
+        return;
+    }
+    impl_->shutting_down.store(true);
+    cancelAndClear();
+    std::lock_guard lock(impl_->mutex);
+    if (!impl_->active_arm) {
+        impl_->state = State::ShuttingDown;
+    }
+}
+
+ActionQueue::State ActionQueue::state() const
+{
+    std::lock_guard lock(impl_->mutex);
+    return impl_->state;
+}
+
+} // namespace cmvr::service

cmvr-es/service/grpc/action/src/control_command_arbiter.cpp

@@ -0,0 +1,105 @@
+#include "service/grpc/action/include/control_command_arbiter.h"
+
+#include <utility>
+
+namespace cmvr::service {
+
+ControlCommandArbiter::Lease::~Lease()
+{
+    reset();
+}
+
+ControlCommandArbiter::Lease::Lease(Lease&& other) noexcept
+    : owner_(std::exchange(other.owner_, nullptr)),
+      kind_(std::exchange(other.kind_, LeaseKind::None))
+{
+}
+
+ControlCommandArbiter::Lease& ControlCommandArbiter::Lease::operator=(
+    Lease&& other) noexcept
+{
+    if (this != &other) {
+        reset();
+        owner_ = std::exchange(other.owner_, nullptr);
+        kind_ = std::exchange(other.kind_, LeaseKind::None);
+    }
+    return *this;
+}
+
+void ControlCommandArbiter::Lease::reset() noexcept
+{
+    if (owner_) {
+        owner_->release(kind_);
+        owner_ = nullptr;
+        kind_ = LeaseKind::None;
+    }
+}
+
+ControlCommandArbiter& ControlCommandArbiter::instance()
+{
+    static ControlCommandArbiter arbiter;
+    return arbiter;
+}
+
+ControlCommandArbiter::Lease ControlCommandArbiter::tryAcquireControl()
+{
+    std::lock_guard lock(mutex_);
+    if (action_queue_active_ || active_stops_ != 0) {
+        return {};
+    }
+    ++active_controls_;
+    return {this, LeaseKind::Control};
+}
+
+ControlCommandArbiter::Lease ControlCommandArbiter::tryAcquireActionQueue()
+{
+    std::lock_guard lock(mutex_);
+    if (action_queue_active_ || active_controls_ != 0 || active_stops_ != 0) {
+        return {};
+    }
+    action_queue_active_ = true;
+    return {this, LeaseKind::ActionQueue};
+}
+
+ControlCommandArbiter::Lease ControlCommandArbiter::beginStop()
+{
+    std::lock_guard lock(mutex_);
+    ++active_stops_;
+    return {this, LeaseKind::Stop};
+}
+
+bool ControlCommandArbiter::actionQueueActive() const
+{
+    std::lock_guard lock(mutex_);
+    return action_queue_active_;
+}
+
+std::size_t ControlCommandArbiter::activeControlCount() const
+{
+    std::lock_guard lock(mutex_);
+    return active_controls_;
+}
+
+void ControlCommandArbiter::release(const LeaseKind kind) noexcept
+{
+    std::lock_guard lock(mutex_);
+    switch (kind) {
+    case LeaseKind::Control:
+        if (active_controls_ != 0) {
+            --active_controls_;
+        }
+        break;
+    case LeaseKind::ActionQueue:
+        action_queue_active_ = false;
+        break;
+    case LeaseKind::Stop:
+        if (active_stops_ != 0) {
+            --active_stops_;
+        }
+        break;
+    case LeaseKind::None:
+        break;
+    }
+}
+
+} // namespace cmvr::service

cmvr-es/service/grpc/action/tests/action_queue_test.cpp

@@ -0,0 +1,330 @@
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <future>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "devices/arm/robot_arm.h"
+#include "service/grpc/action/include/action_queue.h"
+#include "service/grpc/action/include/control_command_arbiter.h"
+
+namespace cmvr::service {
+namespace {
+
+using namespace std::chrono_literals;
+
+class FakeActionQueueArm final : public device::RobotArm {
+public:
+    explicit FakeActionQueueArm(std::string device_id)
+    {
+        id_ = std::move(device_id);
+        model_.dof = 6;
+        model_.joint_names = {"j1", "j2", "j3", "j4", "j5", "j6"};
+    }
+
+    std::string typeName() const override { return "FakeActionQueueArm"; }
+    bool stop() override { return stopMotion().ok(); }
+    device::RobotModel getRobotModel() const override { return model_; }
+    std::size_t getDof() const override { return model_.dof; }
+    device::ArmState getRobotState() const override { return {}; }
+    device::JointGroupState getJointState() const override { return {}; }
+    device::CartesianPose getTcpPose(device::FrameType) const override { return {}; }
+    device::RobotMode getRobotMode() const override { return device::RobotMode::Idle; }
+    device::SafetyMode getSafetyMode() const override {
+        return emergency_.load()
+            ? device::SafetyMode::EmergencyStop
+            : device::SafetyMode::Normal;
+    }
+    device::ControlMode getControlMode() const override {
+        return device::ControlMode::Position;
+    }
+    bool supportsActionQueueMotion() const noexcept override { return true; }
+
+    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 {
+        emergency_.store(true);
+        return stopMotion();
+    }
+    device::Result protectiveStop() override { return emergencyStop(); }
+    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 emergency_.load(); }
+    bool isFault() const override { return false; }
+
+    device::Result moveJ(const device::JointPositionCommand&,
+                         const device::MotionOptions& options) override
+    {
+        return runMotion("MoveJ", 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&,
+                         const device::MotionOptions& options,
+                         device::FrameType) override
+    {
+        return runMotion("MoveL", options);
+    }
+    device::Result moveL(const device::CartesianPose&,
+                         const device::MotionOptions& options,
+                         const std::string& base_frame,
+                         const std::string& tcp_frame) override
+    {
+        {
+            std::lock_guard lock(mutex_);
+            base_frame_ = base_frame;
+            tcp_frame_ = tcp_frame;
+        }
+        return runMotion("MoveL", options);
+    }
+    device::Result speedL(const device::CartesianVelocity&, double, double,
+                          device::FrameType) override {
+        return device::Result::success();
+    }
+    device::Result stopL(std::optional<double>) override { return stopMotion(); }
+    device::Result stopMotion() override
+    {
+        ++stop_count_;
+        cv_.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) override { return device::Result::success(); }
+    device::Result servoSpeedJ(const device::JointVelocityCommand&) override { return device::Result::success(); }
+    device::Result servoSpeedL(const device::CartesianVelocity&, device::FrameType) 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<double> ik(const std::string&, const std::string&,
+                           const device::CartesianPose&) override { return {}; }
+    std::shared_ptr<cmvr::IKSolver> kinematicsSolver() const override { return nullptr; }
+    device::CartesianPose fk(const std::string&, const std::string&) override { return {}; }
+    device::CartesianPose fk(bool) override { return {}; }
+    device::CartesianVelocity getSpeedLCommandTwistBase() const override { return {}; }
+    bool busy() const override { return busy_.load(); }
+
+    void blockMotion(bool value)
+    {
+        block_.store(value);
+        cv_.notify_all();
+    }
+
+    bool waitUntilStarted()
+    {
+        std::unique_lock lock(mutex_);
+        return cv_.wait_for(lock, 2s, [this] { return started_; });
+    }
+
+    std::vector<std::string> calls() const
+    {
+        std::lock_guard lock(mutex_);
+        return calls_;
+    }
+
+    std::string baseFrame() const
+    {
+        std::lock_guard lock(mutex_);
+        return base_frame_;
+    }
+
+    std::string tcpFrame() const
+    {
+        std::lock_guard lock(mutex_);
+        return tcp_frame_;
+    }
+
+    int stopCount() const { return stop_count_.load(); }
+
+    void triggerHardwareEstop()
+    {
+        emergency_.store(true);
+        cv_.notify_all();
+    }
+
+private:
+    device::Result runMotion(const std::string& name,
+                             const device::MotionOptions& options)
+    {
+        busy_.store(true);
+        {
+            std::lock_guard lock(mutex_);
+            calls_.push_back(name);
+            started_ = true;
+        }
+        cv_.notify_all();
+        while (block_.load()) {
+            if (emergency_.load()) {
+                busy_.store(false);
+                return device::Result::failure(
+                    device::ArmErrorCode::RobotInEmergencyStop,
+                    "hardware emergency stop");
+            }
+            if (options.cancellation_requested &&
+                options.cancellation_requested()) {
+                busy_.store(false);
+                return device::Result::failure(
+                    device::ArmErrorCode::CommandRejected, "canceled");
+            }
+            std::unique_lock lock(mutex_);
+            cv_.wait_for(lock, 10ms);
+        }
+        busy_.store(false);
+        return emergency_.load()
+            ? device::Result::failure(
+                  device::ArmErrorCode::RobotInEmergencyStop,
+                  "hardware emergency stop")
+            : device::Result::success();
+    }
+
+    device::RobotModel model_;
+    mutable std::mutex mutex_;
+    std::condition_variable cv_;
+    std::vector<std::string> calls_;
+    std::string base_frame_;
+    std::string tcp_frame_;
+    bool started_{false};
+    std::atomic<bool> block_{false};
+    std::atomic<bool> busy_{false};
+    std::atomic<bool> emergency_{false};
+    std::atomic<int> stop_count_{0};
+};
+
+ActionQueue makeQueue(const std::shared_ptr<FakeActionQueueArm>& arm)
+{
+    return ActionQueue([arm](const std::string& device_id) {
+        return device_id == arm->id()
+            ? std::static_pointer_cast<device::RobotArm>(arm)
+            : std::shared_ptr<device::RobotArm>{};
+    });
+}
+
+api::ActionQueueCommand_Request twoStepRequest(const std::string& device_id)
+{
+    api::ActionQueueCommand_Request request;
+    auto* move_j = request.add_steps()->mutable_arm_move_j();
+    move_j->mutable_header()->set_device_id(device_id);
+    for (int index = 0; index < 6; ++index) {
+        move_j->mutable_target()->add_position(0.1 * index);
+    }
+    auto* move_l = request.add_steps()->mutable_arm_move_l();
+    move_l->mutable_header()->set_device_id(device_id);
+    move_l->set_base_frame("workpiece");
+    move_l->set_tcp_frame("gripper");
+    return request;
+}
+
+TEST(ActionQueueTest, ExecutesMoveJAndNamedFrameMoveLInOrder)
+{
+    auto arm = std::make_shared<FakeActionQueueArm>("action_arm_order");
+    auto queue = makeQueue(arm);
+    api::ActionQueueCommand_Feedback feedback;
+
+    queue.execute(twoStepRequest(arm->id()), feedback);
+
+    EXPECT_TRUE(feedback.header().success())
+        << feedback.header().error_message();
+    EXPECT_EQ(feedback.result(), api::ACTION_RESULT_CODE_COMPLETED);
+    EXPECT_EQ(feedback.completed_steps(), 2U);
+    EXPECT_EQ(arm->calls(), (std::vector<std::string>{"MoveJ", "MoveL"}));
+    EXPECT_EQ(arm->baseFrame(), "workpiece");
+    EXPECT_EQ(arm->tcpFrame(), "gripper");
+}
+
+TEST(ActionQueueTest, StopClearsRemainingStepsAndReleasesControlDomain)
+{
+    auto arm = std::make_shared<FakeActionQueueArm>("action_arm_stop");
+    arm->blockMotion(true);
+    auto queue = makeQueue(arm);
+    api::ActionQueueCommand_Feedback feedback;
+
+    std::thread executor([&] {
+        queue.execute(twoStepRequest(arm->id()), feedback);
+    });
+    ASSERT_TRUE(arm->waitUntilStarted());
+    EXPECT_FALSE(ControlCommandArbiter::instance().tryAcquireControl());
+
+    queue.cancelAndClear();
+    executor.join();
+
+    EXPECT_EQ(feedback.result(), api::ACTION_RESULT_CODE_CANCELED);
+    EXPECT_EQ(feedback.completed_steps(), 0U);
+    EXPECT_EQ(arm->calls(), (std::vector<std::string>{"MoveJ"}));
+    EXPECT_GE(arm->stopCount(), 1);
+    EXPECT_TRUE(ControlCommandArbiter::instance().tryAcquireControl());
+}
+
+TEST(ActionQueueTest, RejectsSubmissionWhileOrdinaryControlIsActive)
+{
+    auto arm = std::make_shared<FakeActionQueueArm>("action_arm_gate");
+    auto queue = makeQueue(arm);
+    api::ActionQueueCommand_Feedback feedback;
+    auto control_lease =
+        ControlCommandArbiter::instance().tryAcquireControl();
+    ASSERT_TRUE(control_lease);
+
+    queue.execute(twoStepRequest(arm->id()), feedback);
+
+    EXPECT_EQ(feedback.result(), api::ACTION_RESULT_CODE_REJECTED);
+    EXPECT_TRUE(arm->calls().empty());
+}
+
+TEST(ActionQueueTest, HardwareEmergencyStopTerminatesWithoutHoldingTheGate)
+{
+    auto arm = std::make_shared<FakeActionQueueArm>("action_arm_estop");
+    arm->blockMotion(true);
+    auto queue = makeQueue(arm);
+    api::ActionQueueCommand_Feedback feedback;
+
+    auto execution = std::async(std::launch::async, [&] {
+        queue.execute(twoStepRequest(arm->id()), feedback);
+    });
+    ASSERT_TRUE(arm->waitUntilStarted());
+    arm->triggerHardwareEstop();
+
+    ASSERT_EQ(execution.wait_for(2s), std::future_status::ready);
+    execution.get();
+    EXPECT_EQ(feedback.result(), api::ACTION_RESULT_CODE_FAILED);
+    EXPECT_EQ(arm->calls(), (std::vector<std::string>{"MoveJ"}));
+    EXPECT_TRUE(ControlCommandArbiter::instance().tryAcquireControl());
+}
+
+TEST(ActionQueueTest, StepTimeoutStopsBlockedMotion)
+{
+    auto arm = std::make_shared<FakeActionQueueArm>("action_arm_timeout");
+    arm->blockMotion(true);
+    auto queue = makeQueue(arm);
+    auto request = twoStepRequest(arm->id());
+    request.mutable_steps(0)->set_timeout_ms(30);
+    api::ActionQueueCommand_Feedback feedback;
+
+    queue.execute(request, feedback);
+
+    EXPECT_EQ(feedback.result(), api::ACTION_RESULT_CODE_TIMED_OUT);
+    EXPECT_EQ(feedback.completed_steps(), 0U);
+    EXPECT_EQ(arm->calls(), (std::vector<std::string>{"MoveJ"}));
+}
+
+} // namespace
+} // namespace cmvr::service

cmvr-es/service/grpc/include/grpc_system_service.h

@@ -5,22 +5,31 @@
 #ifndef GRPC_SYSTEM_SERVICE_H
 #define GRPC_SYSTEM_SERVICE_H
 
+#include <memory>
+#include <mutex>
+
 #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 ActionQueue;
+
     class gRPCSystemServiceImpl: public api::SystemService::Service {
     public:
         gRPCSystemServiceImpl();
-        ~gRPCSystemServiceImpl() override = default;
+        ~gRPCSystemServiceImpl() override;
+        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 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<ActionQueue> action_queue_;
+        std::mutex stop_all_mutex_;
     };
 }
 

cmvr-es/service/grpc/src/grpc_agv_service.cpp

@@ -7,6 +7,9 @@
 
 #include <google/protobuf/util/time_util.h>
 
+#include "service/grpc/action/include/control_command_arbiter.h"
+#include "service/grpc/action/include/control_command_guard.h"
+
 using google::protobuf::util::TimeUtil;
 
 namespace cmvr::service {
@@ -427,6 +430,8 @@ grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext*,
                                             const api::CommandHeader_Request* request,
                                             api::CommandHeader_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
         if (!agv) {
@@ -444,6 +449,8 @@ grpc::Status gRPCAgvServiceImpl::relocalize(
     const api::AgvRelocalizeCommand_Request* request,
     api::AgvRelocalizeCommand_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         const std::string device_id = request->header().device_id();
         auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
@@ -461,6 +468,8 @@ grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext* context,
                                                 const api::AgvNavigateToPoseCommand_Request* request,
                                                 api::AgvNavigateToPoseCommand_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         if (context && context->IsCancelled()) {
             return setNavigationRequestCanceled(response);
@@ -484,6 +493,8 @@ grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext* context,
                                                    const api::AgvNavigateToStationCommand_Request* request,
                                                    api::AgvNavigateToStationCommand_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         if (context && context->IsCancelled()) {
             return setNavigationRequestCanceled(response);
@@ -507,6 +518,8 @@ grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext* context,
                                             const api::AgvFollowPathCommand_Request* request,
                                             api::AgvFollowPathCommand_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         if (context && context->IsCancelled()) {
             return setNavigationRequestCanceled(response);
@@ -536,6 +549,8 @@ grpc::Status gRPCAgvServiceImpl::pauseNavigation(grpc::ServerContext*,
                                                  const api::CommandHeader_Request* request,
                                                  api::CommandHeader_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
         if (!agv) {
@@ -552,6 +567,8 @@ grpc::Status gRPCAgvServiceImpl::resumeNavigation(grpc::ServerContext*,
                                                   const api::CommandHeader_Request* request,
                                                   api::CommandHeader_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
         if (!agv) {
@@ -584,6 +601,8 @@ grpc::Status gRPCAgvServiceImpl::setVelocity(grpc::ServerContext*,
                                              const api::AgvSetVelocityCommand_Request* request,
                                              api::AgvSetVelocityCommand_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         const std::string device_id = request->header().device_id();
         auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
@@ -602,6 +621,8 @@ grpc::Status gRPCAgvServiceImpl::translate(
     const api::AgvTranslateCommand_Request* request,
     api::AgvTranslateCommand_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         const std::string device_id = request->header().device_id();
         auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
@@ -683,6 +704,8 @@ grpc::Status gRPCAgvServiceImpl::switchMap(grpc::ServerContext*,
                                            const api::AgvMapCommand_Request* request,
                                            api::AgvMapCommand_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         const std::string device_id = request->header().device_id();
         auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
@@ -700,6 +723,8 @@ grpc::Status gRPCAgvServiceImpl::uploadMap(grpc::ServerContext*,
                                            const api::AgvMapCommand_Request* request,
                                            api::AgvMapCommand_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         const std::string device_id = request->header().device_id();
         auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
@@ -739,6 +764,8 @@ grpc::Status gRPCAgvServiceImpl::startMapping(grpc::ServerContext*,
                                               const api::AgvStartMappingCommand_Request* request,
                                               api::AgvStartMappingCommand_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         const std::string device_id = request->header().device_id();
         auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);

cmvr-es/service/grpc/src/grpc_arm_service.cpp

@@ -3,6 +3,7 @@
 #include <google/protobuf/util/time_util.h>
 
 #include "common/base/logging/logger.h"
+#include "service/grpc/action/include/control_command_arbiter.h"
 
 using google::protobuf::util::TimeUtil;
 
@@ -119,6 +120,23 @@ grpc::Status setDeviceNotFound(Response* response, const std::string& device_id)
     return grpc::Status(grpc::StatusCode::NOT_FOUND, message);
 }
 
+grpc::Status setControlBusy(api::CommandHeader_Feedback* response)
+{
+    const std::string message =
+        "control command rejected while ActionQueue or StopAll is active";
+    fillFeedback(response, false, message);
+    return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, message);
+}
+
+template <typename Response>
+grpc::Status setControlBusy(Response* response)
+{
+    const std::string message =
+        "control command rejected while ActionQueue or StopAll is active";
+    fillFeedback(response->mutable_header(), false, message);
+    return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, message);
+}
+
 } // namespace
 
 gRPCArmServiceImpl::gRPCArmServiceImpl()
@@ -152,6 +170,11 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*,
                                           const api::CommandHeader_Request* request,
                                           api::CommandHeader_Feedback* response)
 {
+    auto control_lease =
+        ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) {
+        return setControlBusy(response);
+    }
     try {
         const std::string device_id = request->device_id();
         auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
@@ -175,6 +198,11 @@ grpc::Status gRPCArmServiceImpl::clearFault(
     const api::CommandHeader_Request* request,
     api::CommandHeader_Feedback* response)
 {
+    auto control_lease =
+        ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) {
+        return setControlBusy(response);
+    }
     try {
         const std::string device_id = request->device_id();
         auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
@@ -197,6 +225,11 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*,
                                        const api::MoveJ_Request* request,
                                        api::MoveJ_Response* response)
 {
+    auto control_lease =
+        ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) {
+        return setControlBusy(response);
+    }
     try {
         const std::string device_id = request->header().device_id();
         auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
@@ -220,6 +253,11 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*,
                                        const api::MoveL_Request* request,
                                        api::MoveL_Response* response)
 {
+    auto control_lease =
+        ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) {
+        return setControlBusy(response);
+    }
     try {
         const std::string device_id = request->header().device_id();
         auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
@@ -320,6 +358,11 @@ grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*,
                                         const api::SpeedJ_Request* request,
                                         api::SpeedJ_Response* response)
 {
+    auto control_lease =
+        ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) {
+        return setControlBusy(response);
+    }
     try {
         const std::string device_id = request->header().device_id();
         auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
@@ -346,6 +389,11 @@ grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*,
                                         const api::SpeedL_Request* request,
                                         api::SpeedL_Response* response)
 {
+    auto control_lease =
+        ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) {
+        return setControlBusy(response);
+    }
     try {
         const std::string device_id = request->header().device_id();
         auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
@@ -373,6 +421,11 @@ grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext*,
                                         const api::ServoJ_Request* request,
                                         api::ServoJ_Response* response)
 {
+    auto control_lease =
+        ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) {
+        return setControlBusy(response);
+    }
     try {
         const std::string device_id = request->header().device_id();
         auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
@@ -472,6 +525,11 @@ grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*,
                                                 const api::CalibrateZeroQ_Request* request,
                                                 api::CalibrateZeroQ_Response* response)
 {
+    auto control_lease =
+        ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) {
+        return setControlBusy(response);
+    }
     try {
         const std::string device_id = request->header().device_id();
         auto arm = dmgr_.getDevice<device::RobotArm>(device_id);

cmvr-es/service/grpc/src/grpc_dexhand_service.cpp

@@ -12,6 +12,8 @@
 #include <vector>
 
 #include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h"
+#include "service/grpc/action/include/control_command_arbiter.h"
+#include "service/grpc/action/include/control_command_guard.h"
 
 using namespace std;
 using namespace cmvr::service;
@@ -227,6 +229,8 @@ grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context,
 grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context
     , const cmvr::api::SetDexHandPositionsCommand_Request* request
     , cmvr::api::SetDexHandPositionsCommand_Feedback* response) {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         string dev_id = request->header().device_id();
         CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPos): id=" << dev_id;
@@ -264,6 +268,8 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context
 grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* context
     , const cmvr::api::SetDexHandAnglesCommand_Request* request
     , cmvr::api::SetDexHandAnglesCommand_Feedback* response) {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         string dev_id = request->header().device_id();
         CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandAngle): id=" << dev_id;
@@ -305,6 +311,8 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* contex
 grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* context
     , const cmvr::api::SetDexHandForceCommand_Request* request
     , cmvr::api::SetDexHandForceCommand_Feedback* response) {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         string dev_id = request->header().device_id();
         CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandForce): id=" << dev_id;
@@ -342,6 +350,8 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* contex
 grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* context
     , const cmvr::api::SetDexHandSpeedCommand_Request* request
     , cmvr::api::SetDexHandSpeedCommand_Feedback* response) {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         string dev_id = request->header().device_id();
         CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandSpeed): id=" << dev_id;
@@ -379,6 +389,8 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* contex
 grpc::Status gRPCDexHandServiceImpl::SetDexHandPresetAct(grpc::ServerContext* context
     , const cmvr::api::SetDexHandPresetActCommand_Request* request
     , cmvr::api::SetDexHandPresetActCommand_Feedback* response) {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         string dev_id = request->header().device_id();
         CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPresetAct): id=" << dev_id;

cmvr-es/service/grpc/src/grpc_head_service.cpp

@@ -5,6 +5,8 @@
 #include "manager/device_manager/include/device_manager.h"
 #include "common/base/grpc_utils.h"
 #include "biohead/biohead_esp32/include/biohead_esp32.h"
+#include "service/grpc/action/include/control_command_arbiter.h"
+#include "service/grpc/action/include/control_command_guard.h"
 #include <chrono>
 #include <algorithm>
 #include <iostream>
@@ -40,6 +42,9 @@ grpc::Status gRPCMBioHeadServiceImpl::SetExpression(
     const SetFacialExpression_Request* request,
     SetFacialExpression_Feedback* response) {
 
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
+
     try {
         std::string dev_id = request->header().device_id();
         auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
@@ -90,6 +95,8 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression(
     grpc::ServerContext* context,
     grpc::ServerReaderWriter<StreamFacialExpression_Feedback, StreamFacialExpression_Request>* stream)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand();
     StreamFacialExpression_Feedback feedback_msg;
     std::string dev_id;
     std::shared_ptr<AbstractBiohead> robot;
@@ -268,6 +275,9 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop(
 grpc::Status gRPCMBioHeadServiceImpl::SpeakStart(grpc::ServerContext* context, const cmvr::api::SpeakStart_Request* request, cmvr::api::SpeakStart_Feedback* response)
 {
 
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
+
     {
 
         try {
@@ -326,6 +336,8 @@ grpc::Status gRPCMBioHeadServiceImpl::SpeakStop(grpc::ServerContext* context, co
 
 grpc::Status gRPCMBioHeadServiceImpl::Happy(grpc::ServerContext* context, const cmvr::api::Happy_Request* request, cmvr::api::Happy_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         string dev_id = request->header().device_id();
         auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
@@ -350,6 +362,8 @@ grpc::Status gRPCMBioHeadServiceImpl::Happy(grpc::ServerContext* context, const
 
 grpc::Status gRPCMBioHeadServiceImpl::Surprise(grpc::ServerContext* context, const cmvr::api::Surprise_Request* request, cmvr::api::Surprise_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         string dev_id = request->header().device_id();
         auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
@@ -375,6 +389,8 @@ grpc::Status gRPCMBioHeadServiceImpl::Surprise(grpc::ServerContext* context, con
 
 grpc::Status gRPCMBioHeadServiceImpl::ExpressionTired(grpc::ServerContext* context, const cmvr::api::ExpressionTired_Request* request, cmvr::api::ExpressionTired_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         string dev_id = request->header().device_id();
         auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
@@ -401,6 +417,8 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionTired(grpc::ServerContext* conte
 
 grpc::Status gRPCMBioHeadServiceImpl::ExpressionAngry(grpc::ServerContext* context, const cmvr::api::ExpressionAngry_Request* request, cmvr::api::ExpressionAngry_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         string dev_id = request->header().device_id();
         auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
@@ -427,6 +445,8 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionAngry(grpc::ServerContext* conte
 
 grpc::Status gRPCMBioHeadServiceImpl::ExpressionSadness(grpc::ServerContext* context, const cmvr::api::ExpressionSadness_Request* request, cmvr::api::ExpressionSadness_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         string dev_id = request->header().device_id();
         auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
@@ -452,6 +472,8 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionSadness(grpc::ServerContext* con
 
 grpc::Status gRPCMBioHeadServiceImpl::ExpressionYawn(grpc::ServerContext* context, const cmvr::api::ExpressionYawn_Request* request, cmvr::api::ExpressionYawn_Feedback* response)
 {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         string dev_id = request->header().device_id();
         auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);

cmvr-es/service/grpc/src/grpc_hlc_service.cpp

@@ -14,6 +14,8 @@
 #include "common/base/logging/logger.h"
 #include "manager/task_manager/include/task_manager.h"
 #include "task/touch_screen_task/include/touch_screen_task.h"
+#include "service/grpc/action/include/control_command_arbiter.h"
+#include "service/grpc/action/include/control_command_guard.h"
 
 
 using namespace cmvr::service;
@@ -43,6 +45,8 @@ void fillTouchResponse(Touch_Response* response,
 gRPCHlcServiceImpl::gRPCHlcServiceImpl() = default;
 
 grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) {
+    auto control_lease = ControlCommandArbiter::instance().tryAcquireControl();
+    if (!control_lease) return rejectControlCommand(response);
     try {
         auto touch_task = task::TaskManager::getInstance().getTouchScreenTask();
         if (!touch_task) {

cmvr-es/service/grpc/src/grpc_system_service.cpp

@@ -5,12 +5,32 @@
 #include "../include/grpc_system_service.h"
 
 #include "common/base/logging/logger.h"
+#include "service/grpc/action/include/action_queue.h"
+#include "service/grpc/action/include/control_command_arbiter.h"
 
-using namespace cmvr::device;
 using namespace cmvr::device;
 using namespace cmvr::service;
 
-gRPCSystemServiceImpl::gRPCSystemServiceImpl(): dmgr_(DeviceManager::getInstance()) {}
+gRPCSystemServiceImpl::gRPCSystemServiceImpl()
+    : dmgr_(DeviceManager::getInstance()),
+      action_queue_(std::make_unique<ActionQueue>(
+          [this](const std::string& device_id) {
+              return dmgr_.getDevice<device::RobotArm>(device_id);
+          }))
+{
+}
+
+gRPCSystemServiceImpl::~gRPCSystemServiceImpl()
+{
+    prepareForShutdown();
+}
+
+void gRPCSystemServiceImpl::prepareForShutdown()
+{
+    if (action_queue_) {
+        action_queue_->shutdown();
+    }
+}
 
 grpc::Status gRPCSystemServiceImpl::GetSystemInfo(grpc::ServerContext* context,
     const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response)
@@ -95,6 +115,17 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context,
     const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response)
 {
     try {
+        std::unique_lock stop_all_lock(stop_all_mutex_, std::try_to_lock);
+        if (!stop_all_lock.owns_lock()) {
+            response->mutable_header()->set_success(false);
+            response->mutable_header()->set_error_message(
+                "another StopAll request is already running");
+            setCurrentTimestamp(
+                response->mutable_header()->mutable_timestamp());
+            return grpc::Status::OK;
+        }
+        auto stop_lease = ControlCommandArbiter::instance().beginStop();
+        action_queue_->cancelAndClear();
         dmgr_.stop();
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@@ -108,3 +139,34 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context,
         return grpc::Status::OK;
     }
 }
+
+grpc::Status gRPCSystemServiceImpl::ExecuteActionQueue(
+    grpc::ServerContext*,
+    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 {
+        action_queue_->execute(*request, *response);
+        return grpc::Status::OK;
+    } catch (const std::exception& error) {
+        response->Clear();
+        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;
+    } catch (...) {
+        response->Clear();
+        response->set_result(api::ACTION_RESULT_CODE_FAILED);
+        response->mutable_header()->set_success(false);
+        response->mutable_header()->set_error_message(
+            "ActionQueue failed with an unknown exception");
+        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        return grpc::Status::OK;
+    }
+}

cmvr-es/task/grpc_server_task/include/grpc_server_task.h

@@ -11,6 +11,10 @@
 #include "cmvr/config/grpc_server_config/grpc_server_config.pb.h"
 #include "task/task.h"
 
+namespace cmvr::service {
+class gRPCSystemServiceImpl;
+}
+
 namespace cmvr::task {
 
 class GrpcServerTask final : public Task {
@@ -49,7 +53,7 @@ private:
     std::thread wait_thread_;
 
     std::unique_ptr<grpc::Service> camera_service_;
-    std::unique_ptr<grpc::Service> system_service_;
+    std::unique_ptr<service::gRPCSystemServiceImpl> system_service_;
     std::unique_ptr<grpc::Service> speaker_service_;
     std::unique_ptr<grpc::Service> microphone_service_;
     std::unique_ptr<grpc::Service> dexhand_service_;

cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp

@@ -161,6 +161,9 @@ void GrpcServerTask::stop()
 {
     {
         std::lock_guard lock(mutex_);
+        if (system_service_) {
+            system_service_->prepareForShutdown();
+        }
         if (server_) {
             server_->Shutdown();
         }

protos/cmvr/api/system_command.proto

@@ -1,5 +1,6 @@
 syntax = "proto3";
 
+import "cmvr/api/arm_command.proto";
 import "cmvr/api/common.proto";
 
 package cmvr.api;
@@ -69,4 +70,48 @@ message StopAllCommand {
     message Feedback {
         CommandHeader.Feedback header = 1;
     }
-}
+}
+
+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;
+}
+
+message ActionStep {
+    reserved 1;
+    reserved "step_id";
+
+    // Zero inherits the remaining ActionQueue timeout.
+    uint32 timeout_ms = 2;
+
+    oneof command {
+        MoveJ.Request arm_move_j = 10;
+        MoveL.Request arm_move_l = 11;
+    }
+}
+
+message ActionQueueCommand {
+    message Request {
+        reserved 1;
+        reserved "action_id";
+
+        repeated ActionStep steps = 2;
+        // Zero uses the server default.
+        uint32 total_timeout_ms = 3;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+
+        reserved 2;
+        reserved "action_id";
+
+        uint32 completed_steps = 3;
+        optional uint32 failed_step_index = 4;
+        ActionResultCode result = 5;
+    }
+}

protos/cmvr/api/system_service.proto

@@ -8,8 +8,7 @@ package cmvr.api;
 service SystemService {
     rpc GetSystemInfo(GetSystemInfoCommand.Request) returns (GetSystemInfoCommand.Feedback) {}
     rpc GetSystemStatus(GetSystemStatusCommand.Request) returns (GetSystemStatusCommand.Feedback) {}
-
     rpc UpdateParams(UpdateParamsCommand.Request) returns (UpdateParamsCommand.Feedback) {}
-
     rpc StopAll(StopAllCommand.Request) returns (StopAllCommand.Feedback) {}
-}
+    rpc ExecuteActionQueue(ActionQueueCommand.Request) returns (ActionQueueCommand.Feedback) {}
+}