From d15d381b1d8a633f3ffb3531679a4956f105ce7d Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Wed, 16 Sep 2026 14:33:49 +0800 Subject: [PATCH] add grpc action queue support --- cmvr-es/common/types/arm/arm_types.h | 4 + .../devices/arm/aubo_arm/include/aubo_arm.h | 1 + cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp | 60 +- cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp | 27 +- cmvr-es/devices/arm/huayan_arm/huayan_arm.h | 6 +- cmvr-es/devices/arm/robot_arm.h | 3 + cmvr-es/service/CMakeLists.txt | 21 + .../grpc/action/include/action_queue.h | 60 ++ .../action/include/control_command_arbiter.h | 66 ++ .../action/include/control_command_guard.h | 45 ++ .../service/grpc/action/src/action_queue.cpp | 575 ++++++++++++++++++ .../action/src/control_command_arbiter.cpp | 105 ++++ .../grpc/action/tests/action_queue_test.cpp | 330 ++++++++++ .../grpc/include/grpc_system_service.h | 11 +- cmvr-es/service/grpc/src/grpc_agv_service.cpp | 27 + cmvr-es/service/grpc/src/grpc_arm_service.cpp | 58 ++ .../service/grpc/src/grpc_dexhand_service.cpp | 12 + .../service/grpc/src/grpc_head_service.cpp | 22 + cmvr-es/service/grpc/src/grpc_hlc_service.cpp | 4 + .../service/grpc/src/grpc_system_service.cpp | 66 +- .../include/grpc_server_task.h | 6 +- .../grpc_server_task/src/grpc_server_task.cpp | 3 + protos/cmvr/api/system_command.proto | 47 +- protos/cmvr/api/system_service.proto | 5 +- 24 files changed, 1548 insertions(+), 16 deletions(-) create mode 100644 cmvr-es/service/grpc/action/include/action_queue.h create mode 100644 cmvr-es/service/grpc/action/include/control_command_arbiter.h create mode 100644 cmvr-es/service/grpc/action/include/control_command_guard.h create mode 100644 cmvr-es/service/grpc/action/src/action_queue.cpp create mode 100644 cmvr-es/service/grpc/action/src/control_command_arbiter.cpp create mode 100644 cmvr-es/service/grpc/action/tests/action_queue_test.cpp diff --git a/cmvr-es/common/types/arm/arm_types.h b/cmvr-es/common/types/arm/arm_types.h index 567b503c..4edf3f0d 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 @@ -157,6 +158,9 @@ struct MotionOptions { double jerk{5.0}; std::vector 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 cancellation_requested; }; struct ServoOptions { diff --git a/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h index d43d92c9..1544b7cd 100644 --- a/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h +++ b/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& frame_names) const override; Result listTCPFrame(std::vector& frame_names) const override; diff --git a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp index 6159d8b8..29540094 100644 --- a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp +++ b/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& 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(); diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp index b59dfdf6..672896a4 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp +++ b/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& 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)); } } diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h index 8d3edd41..1ea0da0e 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h @@ -10,6 +10,7 @@ #include #include +#include #include #include #include @@ -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& frame_names) const override; Result listTCPFrame(std::vector& frame_names) const override; @@ -130,7 +132,9 @@ private: CartesianVelocity readTcpVelocity_() const; std::vector 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& cancellation_requested = {}) const; void startAutoEnableMonitor_(); void stopAutoEnableMonitor_(); void autoEnableMonitorLoop_(); diff --git a/cmvr-es/devices/arm/robot_arm.h b/cmvr-es/devices/arm/robot_arm.h index 85d19bb3..36d9a8a9 100644 --- a/cmvr-es/devices/arm/robot_arm.h +++ b/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& frame_names) const { frame_names.clear(); diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index ea8b5ec2..a46330be 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/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 # -------------------------------------------------------- diff --git a/cmvr-es/service/grpc/action/include/action_queue.h b/cmvr-es/service/grpc/action/include/action_queue.h new file mode 100644 index 00000000..87b3511c --- /dev/null +++ b/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 +#include +#include + +#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( + 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_; +}; + +} // namespace cmvr::service + +#endif // CMVR_ES_ACTION_QUEUE_H diff --git a/cmvr-es/service/grpc/action/include/control_command_arbiter.h b/cmvr-es/service/grpc/action/include/control_command_arbiter.h new file mode 100644 index 00000000..2b09133e --- /dev/null +++ b/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 +#include + +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 diff --git a/cmvr-es/service/grpc/action/include/control_command_guard.h b/cmvr-es/service/grpc/action/include/control_command_guard.h new file mode 100644 index 00000000..d9978fa9 --- /dev/null +++ b/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 + +#include + +#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 +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 diff --git a/cmvr-es/service/grpc/action/src/action_queue.cpp b/cmvr-es/service/grpc/action/src/action_queue.cpp new file mode 100644 index 00000000..b43e5904 --- /dev/null +++ b/cmvr-es/service/grpc/action/src/action_queue.cpp @@ -0,0 +1,575 @@ +#include "service/grpc/action/include/action_queue.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#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(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(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 arm; + std::size_t source_index{0}; + }; + + struct CommandHandler { + std::function prepare; + std::function&)> 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(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& 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& 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 + 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& 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(kMaximumStepCount)) { + error = "ActionQueue step count exceeds the server limit"; + return false; + } + if (request.total_timeout_ms() > + static_cast(kMaximumTimeoutMs)) { + error = "ActionQueue total timeout exceeds the server limit"; + return false; + } + + prepared.reserve(static_cast(request.steps_size())); + for (int index = 0; index < request.steps_size(); ++index) { + const auto& step = request.steps(index); + if (step.timeout_ms() > + static_cast(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(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 handlers; + + mutable std::mutex mutex; + State state{State::Idle}; + std::deque pending; + std::shared_ptr active_arm; + std::atomic stop_requested{false}; + std::atomic shutting_down{false}; +}; + +ActionQueue::ActionQueue(ArmResolver arm_resolver) + : impl_(std::make_unique(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 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(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(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(step.source_index)); + impl_->state = State::Idle; + return; + } + ++completed_steps; + } + } +} + +void ActionQueue::cancelAndClear() +{ + std::shared_ptr 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 diff --git a/cmvr-es/service/grpc/action/src/control_command_arbiter.cpp b/cmvr-es/service/grpc/action/src/control_command_arbiter.cpp new file mode 100644 index 00000000..b1d6eb33 --- /dev/null +++ b/cmvr-es/service/grpc/action/src/control_command_arbiter.cpp @@ -0,0 +1,105 @@ +#include "service/grpc/action/include/control_command_arbiter.h" + +#include + +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 diff --git a/cmvr-es/service/grpc/action/tests/action_queue_test.cpp b/cmvr-es/service/grpc/action/tests/action_queue_test.cpp new file mode 100644 index 00000000..51399205 --- /dev/null +++ b/cmvr-es/service/grpc/action/tests/action_queue_test.cpp @@ -0,0 +1,330 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#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) 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 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) 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 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 calls_; + std::string base_frame_; + std::string tcp_frame_; + bool started_{false}; + std::atomic block_{false}; + std::atomic busy_{false}; + std::atomic emergency_{false}; + std::atomic stop_count_{0}; +}; + +ActionQueue makeQueue(const std::shared_ptr& arm) +{ + return ActionQueue([arm](const std::string& device_id) { + return device_id == arm->id() + ? std::static_pointer_cast(arm) + : std::shared_ptr{}; + }); +} + +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("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{"MoveJ", "MoveL"})); + EXPECT_EQ(arm->baseFrame(), "workpiece"); + EXPECT_EQ(arm->tcpFrame(), "gripper"); +} + +TEST(ActionQueueTest, StopClearsRemainingStepsAndReleasesControlDomain) +{ + auto arm = std::make_shared("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{"MoveJ"})); + EXPECT_GE(arm->stopCount(), 1); + EXPECT_TRUE(ControlCommandArbiter::instance().tryAcquireControl()); +} + +TEST(ActionQueueTest, RejectsSubmissionWhileOrdinaryControlIsActive) +{ + auto arm = std::make_shared("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("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{"MoveJ"})); + EXPECT_TRUE(ControlCommandArbiter::instance().tryAcquireControl()); +} + +TEST(ActionQueueTest, StepTimeoutStopsBlockedMotion) +{ + auto arm = std::make_shared("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{"MoveJ"})); +} + +} // namespace +} // 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 17028a92..1047f504 100644 --- a/cmvr-es/service/grpc/include/grpc_system_service.h +++ b/cmvr-es/service/grpc/include/grpc_system_service.h @@ -5,22 +5,31 @@ #ifndef GRPC_SYSTEM_SERVICE_H #define GRPC_SYSTEM_SERVICE_H +#include +#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 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 action_queue_; + std::mutex stop_all_mutex_; }; } diff --git a/cmvr-es/service/grpc/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/src/grpc_agv_service.cpp index e9ddb71a..d4c47d15 100644 --- a/cmvr-es/service/grpc/src/grpc_agv_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_agv_service.cpp @@ -7,6 +7,9 @@ #include +#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(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_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(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(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_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_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_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_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_id); diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index 36873dc0..27072722 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -3,6 +3,7 @@ #include #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 +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_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_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_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_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_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_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_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_id); diff --git a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp b/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp index 84bf5003..b672e233 100644 --- a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp @@ -12,6 +12,8 @@ #include #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; diff --git a/cmvr-es/service/grpc/src/grpc_head_service.cpp b/cmvr-es/service/grpc/src/grpc_head_service.cpp index 6ad3aff8..26be6d69 100644 --- a/cmvr-es/service/grpc/src/grpc_head_service.cpp +++ b/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 #include #include @@ -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(dev_id); @@ -90,6 +95,8 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression( grpc::ServerContext* context, grpc::ServerReaderWriter* stream) { + auto control_lease = ControlCommandArbiter::instance().tryAcquireControl(); + if (!control_lease) return rejectControlCommand(); StreamFacialExpression_Feedback feedback_msg; std::string dev_id; std::shared_ptr 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(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(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(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(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(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(dev_id); diff --git a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp index 5edc18b0..cfa40d37 100644 --- a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp +++ b/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) { diff --git a/cmvr-es/service/grpc/src/grpc_system_service.cpp b/cmvr-es/service/grpc/src/grpc_system_service.cpp index cbeeed60..b79c036f 100644 --- a/cmvr-es/service/grpc/src/grpc_system_service.cpp +++ b/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( + [this](const std::string& device_id) { + return dmgr_.getDevice(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; + } +} diff --git a/cmvr-es/task/grpc_server_task/include/grpc_server_task.h b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h index 055e2eed..8a24bf61 100644 --- a/cmvr-es/task/grpc_server_task/include/grpc_server_task.h +++ b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h @@ -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 camera_service_; - std::unique_ptr system_service_; + std::unique_ptr system_service_; std::unique_ptr speaker_service_; std::unique_ptr microphone_service_; std::unique_ptr dexhand_service_; 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 dd48753a..65c91dd4 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 @@ -161,6 +161,9 @@ void GrpcServerTask::stop() { { std::lock_guard lock(mutex_); + if (system_service_) { + system_service_->prepareForShutdown(); + } if (server_) { server_->Shutdown(); } diff --git a/protos/cmvr/api/system_command.proto b/protos/cmvr/api/system_command.proto index 52b99221..ac91ae03 100644 --- a/protos/cmvr/api/system_command.proto +++ b/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; } -} \ No newline at end of file +} + +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; + } +} diff --git a/protos/cmvr/api/system_service.proto b/protos/cmvr/api/system_service.proto index c8aee52e..4dbb72df 100644 --- a/protos/cmvr/api/system_service.proto +++ b/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) {} -} \ No newline at end of file + rpc ExecuteActionQueue(ActionQueueCommand.Request) returns (ActionQueueCommand.Feedback) {} +}