From c3c58f45630e44e0c9956d525cb6b3e36a056db6 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Thu, 17 Sep 2026 17:42:20 +0800 Subject: [PATCH] fix aubo stop all motion cancellation --- .../devices/arm/aubo_arm/include/aubo_arm.h | 12 +- cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp | 319 +++++++++++++----- cmvr-es/service/grpc/src/grpc_arm_service.cpp | 20 +- 3 files changed, 254 insertions(+), 97 deletions(-) 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 44c41971..6e276870 100644 --- a/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -92,7 +93,7 @@ public: bool busy() const override { return busy_.load(); } private: - enum class ActiveSpeedMotion { + enum class ActiveMotion { None, Joint, Linear, @@ -102,6 +103,11 @@ private: bool validDof_(std::size_t size, std::string& error) const; Result ensureConnected_(const std::string& context) const; Result ensureMotionReady_(const std::string& context) const; + bool tryBeginMotion_(ActiveMotion kind, std::uint64_t& token); + bool motionCanceled_(std::uint64_t token) const noexcept; + void releaseMotion_(ActiveMotion kind, std::uint64_t token) noexcept; + Result stopMotion_(std::optional requested_kind, + double acceleration); #if defined(CMVR_HAS_AUBO_SDK) struct SdkState; @@ -120,8 +126,8 @@ private: double speed_scaling_{1.0}; std::atomic connected_{false}; std::atomic busy_{false}; - std::atomic active_speed_motion_{ - ActiveSpeedMotion::None}; + std::atomic active_motion_{ActiveMotion::None}; + std::atomic motion_epoch_{0}; std::atomic emergency_stopped_{false}; std::atomic hardware_emergency_stopped_{false}; std::atomic hardware_safety_mode_{0}; 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 092f7125..ff436ca6 100644 --- a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include "common/base/logging/logger.h" @@ -17,9 +18,27 @@ namespace cmvr::device { namespace { -struct BusyGuard { - std::atomic& busy; - ~BusyGuard() { busy.store(false); } +class ScopeExit final { +public: + explicit ScopeExit(std::function callback) + : callback_(std::move(callback)) + { + } + + ~ScopeExit() + { + if (callback_) { + callback_(); + } + } + + ScopeExit(const ScopeExit&) = delete; + ScopeExit& operator=(const ScopeExit&) = delete; + + void dismiss() noexcept { callback_ = {}; } + +private: + std::function callback_; }; std::vector defaultJointNames(const std::size_t dof) @@ -51,7 +70,8 @@ constexpr auto kAutoEnableModeTimeout = std::chrono::seconds(10); int waitArrival( const arcs::aubo_sdk::RobotInterfacePtr& robot_interface, - const std::function& cancellation_requested = {}) + const std::function& cancellation_requested = {}, + const std::function& stop_on_cancel = {}) { const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(60); @@ -67,6 +87,10 @@ int waitArrival( mode == SafetyModeType::SystemEmergencyStop || source != 0; }; const auto stopMotion = [&]() { + if (stop_on_cancel) { + stop_on_cancel(); + return; + } try { (void)robot_interface->getMotionControl()->stopMove(true, true); } catch (...) { @@ -74,36 +98,48 @@ int waitArrival( }; int retry_count = 0; - int exec_id = robot_interface->getMotionControl()->getExecId(); - while (exec_id == -1 && retry_count++ < 5) { + int exec_id = -1; + while (retry_count++ < 5) { + if (hardwareEmergencyStopActive()) { + return -3; + } if (canceled()) { stopMotion(); return -2; } - if (hardwareEmergencyStopActive()) { - return -3; + exec_id = robot_interface->getMotionControl()->getExecId(); + if (exec_id != -1) { + break; } std::this_thread::sleep_for(std::chrono::milliseconds(50)); - exec_id = robot_interface->getMotionControl()->getExecId(); } if (exec_id == -1) { - return -1; - } - while (robot_interface->getMotionControl()->getExecId() != -1) { + if (hardwareEmergencyStopActive()) { + return -3; + } if (canceled()) { stopMotion(); return -2; } + return -1; + } + while (true) { if (hardwareEmergencyStopActive()) { return -3; } + if (canceled()) { + stopMotion(); + return -2; + } if (std::chrono::steady_clock::now() >= deadline) { stopMotion(); return -4; } + if (robot_interface->getMotionControl()->getExecId() == -1) { + return 0; + } std::this_thread::sleep_for(std::chrono::milliseconds(50)); } - return 0; } bool isHardwareEmergencyStop( @@ -274,6 +310,33 @@ AuboArm::~AuboArm() (void)disconnect(); } +bool AuboArm::tryBeginMotion_(const ActiveMotion kind, std::uint64_t& token) +{ + std::lock_guard lock(mutex_); + if (busy_.load()) { + return false; + } + token = motion_epoch_.fetch_add(1) + 1; + active_motion_.store(kind); + busy_.store(true); + return true; +} + +bool AuboArm::motionCanceled_(const std::uint64_t token) const noexcept +{ + return motion_epoch_.load() != token; +} + +void AuboArm::releaseMotion_(const ActiveMotion kind, + const std::uint64_t token) noexcept +{ + std::lock_guard lock(mutex_); + if (motion_epoch_.load() == token && active_motion_.load() == kind) { + active_motion_.store(ActiveMotion::None); + busy_.store(false); + } +} + bool AuboArm::init() { if (ip_.empty()) { @@ -639,10 +702,13 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o if (!ready.ok()) { return ready; } - if (busy_.exchange(true)) { + std::uint64_t motion_token = 0; + if (!tryBeginMotion_(ActiveMotion::Joint, motion_token)) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); } - BusyGuard busy_guard{busy_}; + ScopeExit motion_owner([this, motion_token]() { + releaseMotion_(ActiveMotion::Joint, motion_token); + }); #if defined(CMVR_HAS_AUBO_SDK) try { @@ -656,6 +722,16 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o } auto motion_control = robot_interface->getMotionControl(); motion_control->setSpeedFraction(speed_scaling_); + const auto canceled = [this, motion_token, + external = options.cancellation_requested]() { + return motionCanceled_(motion_token) || + (external && external()); + }; + if (canceled()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveJ canceled before submission"); + } const int ret = motion_control->moveJoint( target.position, options.acceleration > 0.0 ? options.acceleration : 0.5, @@ -666,10 +742,20 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o ret, arcs::common_interface::AUBO_OK, arcs::common_interface::AUBO_REQUEST_IGNORE, - [&robot_interface, &options]() { - return waitArrival( - robot_interface, options.cancellation_requested); + [&robot_interface, &motion_control, &canceled]() { + return waitArrival(robot_interface, canceled, + [&motion_control]() { + // Stop again even when StopAll invalidated the token: + // the motion submission may have raced with the first + // stop request and arrived just after it. + (void)motion_control->stopJoint(31.0); + }); }); + if (canceled()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveJ canceled"); + } if (outcome == aubo_internal::MotionCommandOutcome::CompletedWithoutMotion || outcome == aubo_internal::MotionCommandOutcome::CompletedAfterMotion) { return Result::success(); @@ -700,25 +786,20 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration if (!ready.ok()) { return ready; } - if (busy_.exchange(true)) { + std::uint64_t motion_token = 0; + if (!tryBeginMotion_(ActiveMotion::Joint, motion_token)) { return Result::failure( ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); } - active_speed_motion_.store(ActiveSpeedMotion::Joint); - const auto clear_owned_motion = [this]() { - auto expected = ActiveSpeedMotion::Joint; - if (active_speed_motion_.compare_exchange_strong( - expected, ActiveSpeedMotion::None)) { - busy_.store(false); - } - }; + ScopeExit motion_owner([this, motion_token]() { + releaseMotion_(ActiveMotion::Joint, motion_token); + }); #if defined(CMVR_HAS_AUBO_SDK) try { const auto robot_names = sdk_->rpc_client->getRobotNames(); if (robot_names.empty()) { - clear_owned_motion(); return Result::failure( ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); @@ -726,7 +807,6 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration const auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); if (!robot_interface || !robot_interface->getMotionControl()) { - clear_owned_motion(); return Result::failure( ArmErrorCode::RobotNotReady, "[AuboArm] motion control interface is unavailable"); @@ -740,7 +820,7 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration const int ret = motion_control->speedJoint( velocity.velocity, resolved_acceleration, resolved_duration); - if (active_speed_motion_.load() != ActiveSpeedMotion::Joint) { + if (motionCanceled_(motion_token)) { // StopAll/stopMotion may race with the blocking SDK call. Stop a // second time after it returns so a late submission cannot leave // the robot moving. @@ -750,7 +830,6 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration "[AuboArm] speedJ canceled by stopMotion or hardware safety event"); } if (ret != arcs::common_interface::AUBO_OK) { - clear_owned_motion(); return Result::failure( ArmErrorCode::CommandFailed, "[AuboArm] speedJ failed: sdk ret=" + @@ -760,23 +839,21 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration // Aubo speedJoint keeps following the accepted velocity after this // call returns. Keep busy_ and the motion kind latched until an // explicit stopJ/stopMotion request clears them. + motion_owner.dismiss(); return Result::success(); } catch (const std::exception& e) { - clear_owned_motion(); return Result::failure( ArmErrorCode::CommandFailed, std::string{"[AuboArm] speedJ failed: "} + e.what()); } #else - clear_owned_motion(); return unsupported_("speedJ"); #endif } Result AuboArm::stopJ(double acceleration) { - (void)acceleration; - return stopMotion(); + return stopMotion_(ActiveMotion::Joint, acceleration); } Result AuboArm::moveL(const CartesianPose& target, @@ -800,10 +877,13 @@ Result AuboArm::moveL(const CartesianPose& target, if (!ready.ok()) { return ready; } - if (busy_.exchange(true)) { + std::uint64_t motion_token = 0; + if (!tryBeginMotion_(ActiveMotion::Linear, motion_token)) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); } - BusyGuard busy_guard{busy_}; + ScopeExit motion_owner([this, motion_token]() { + releaseMotion_(ActiveMotion::Linear, motion_token); + }); #if defined(CMVR_HAS_AUBO_SDK) try { @@ -881,6 +961,16 @@ Result AuboArm::moveL(const CartesianPose& target, "base_frame: " + selected_base_frame); } } + const auto canceled = [this, motion_token, + external = options.cancellation_requested]() { + return motionCanceled_(motion_token) || + (external && external()); + }; + if (canceled()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveL canceled before submission"); + } const int ret = motion_control->moveLine( pose, options.acceleration > 0.0 ? options.acceleration : 0.5, @@ -891,10 +981,17 @@ Result AuboArm::moveL(const CartesianPose& target, ret, arcs::common_interface::AUBO_OK, arcs::common_interface::AUBO_REQUEST_IGNORE, - [&robot_interface, &options]() { - return waitArrival( - robot_interface, options.cancellation_requested); + [&robot_interface, &motion_control, &canceled]() { + return waitArrival(robot_interface, canceled, + [&motion_control]() { + (void)motion_control->stopLine(10.0, 10.0); + }); }); + if (canceled()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveL canceled"); + } if (outcome == aubo_internal::MotionCommandOutcome::CompletedWithoutMotion || outcome == aubo_internal::MotionCommandOutcome::CompletedAfterMotion) { return Result::success(); @@ -926,25 +1023,20 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d if (!ready.ok()) { return ready; } - if (busy_.exchange(true)) { + std::uint64_t motion_token = 0; + if (!tryBeginMotion_(ActiveMotion::Linear, motion_token)) { return Result::failure( ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); } - active_speed_motion_.store(ActiveSpeedMotion::Linear); - const auto clear_owned_motion = [this]() { - auto expected = ActiveSpeedMotion::Linear; - if (active_speed_motion_.compare_exchange_strong( - expected, ActiveSpeedMotion::None)) { - busy_.store(false); - } - }; + ScopeExit motion_owner([this, motion_token]() { + releaseMotion_(ActiveMotion::Linear, motion_token); + }); #if defined(CMVR_HAS_AUBO_SDK) try { const auto robot_names = sdk_->rpc_client->getRobotNames(); if (robot_names.empty()) { - clear_owned_motion(); return Result::failure( ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); @@ -952,7 +1044,6 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d const auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); if (!robot_interface || !robot_interface->getMotionControl()) { - clear_owned_motion(); return Result::failure( ArmErrorCode::RobotNotReady, "[AuboArm] motion control interface is unavailable"); @@ -964,7 +1055,6 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d velocity.wx, velocity.wy, velocity.wz, 0.0, 0.0, 0.0}; if (frame == FrameType::Tool) { if (!robot_interface->getRobotState()) { - clear_owned_motion(); return Result::failure( ArmErrorCode::RobotNotReady, "[AuboArm] robot state interface is unavailable"); @@ -972,7 +1062,6 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d auto tool_frame = robot_interface->getRobotState()->getTcpPose(); if (tool_frame.size() < 6) { - clear_owned_motion(); return Result::failure( ArmErrorCode::CommandFailed, "[AuboArm] speedL failed: tcp pose size is less than 6"); @@ -982,7 +1071,6 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d tool_frame[2] = 0.0; const auto math = sdk_->rpc_client->getMath(); if (!math) { - clear_owned_motion(); return Result::failure( ArmErrorCode::RobotNotReady, "[AuboArm] math interface is unavailable"); @@ -991,7 +1079,6 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d angular_velocity = math->poseTrans(tool_frame, angular_velocity); if (linear_velocity.size() < 3 || angular_velocity.size() < 3) { - clear_owned_motion(); return Result::failure( ArmErrorCode::CommandFailed, "[AuboArm] speedL failed: frame conversion returned an invalid velocity"); @@ -1010,7 +1097,7 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d const int ret = motion_control->speedLine( speed, resolved_acceleration, resolved_duration); - if (active_speed_motion_.load() != ActiveSpeedMotion::Linear) { + if (motionCanceled_(motion_token)) { (void)motion_control->stopLine( resolved_acceleration, resolved_acceleration); return Result::failure( @@ -1018,46 +1105,61 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d "[AuboArm] speedL canceled by stopMotion or hardware safety event"); } if (ret != arcs::common_interface::AUBO_OK) { - clear_owned_motion(); return Result::failure( ArmErrorCode::CommandFailed, "[AuboArm] speedL failed: sdk ret=" + std::to_string(ret)); } + motion_owner.dismiss(); return Result::success(); } catch (const std::exception& e) { - clear_owned_motion(); return Result::failure( ArmErrorCode::CommandFailed, std::string{"[AuboArm] speedL failed: "} + e.what()); } #else - clear_owned_motion(); return unsupported_("speedL"); #endif } Result AuboArm::stopL(std::optional acceleration) { - (void)acceleration; - return stopMotion(); + return stopMotion_( + ActiveMotion::Linear, acceleration.has_value() ? *acceleration : 0.0); } Result AuboArm::stopMotion() +{ + return stopMotion_(std::nullopt, 0.0); +} + +Result AuboArm::stopMotion_(const std::optional requested_kind, + const double acceleration) { if (hardware_estop_latched_.load()) { auto_recovery_suppressed_.store(true); } -#if defined(CMVR_HAS_AUBO_SDK) - if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { - active_speed_motion_.store(ActiveSpeedMotion::None); - busy_.store(false); + ActiveMotion active_motion = ActiveMotion::None; + { + std::lock_guard lock(mutex_); + motion_epoch_.fetch_add(1); + active_motion = active_motion_.exchange(ActiveMotion::None); + // Keep new device-level commands out until the vendor stop has been + // issued and the controller has been observed idle. + busy_.store(true); + } + ScopeExit stop_owner([this]() { busy_.store(false); }); + + // A hardware emergency stop has already stopped the physical robot. Avoid + // issuing motion RPCs while the controller is in an emergency safety mode. + if (hardware_estop_latched_.load()) { + return Result::success(); + } +#if defined(CMVR_HAS_AUBO_SDK) + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { return Result::success(); } - const auto active_speed = - active_speed_motion_.exchange(ActiveSpeedMotion::None); - busy_.store(false); try { const auto robot_names = sdk_->rpc_client->getRobotNames(); if (robot_names.empty()) { @@ -1068,31 +1170,61 @@ Result AuboArm::stopMotion() return Result::success(); } const auto motion_control = robot_interface->getMotionControl(); + const auto robot_state = robot_interface->getRobotState(); + const ActiveMotion stop_kind = + requested_kind.has_value() ? *requested_kind : active_motion; int ret = arcs::common_interface::AUBO_OK; - switch (active_speed) { - case ActiveSpeedMotion::Joint: - ret = motion_control->stopJoint(1.5); + switch (stop_kind) { + case ActiveMotion::Joint: + ret = motion_control->stopJoint( + acceleration > 0.0 ? acceleration : 31.0); break; - case ActiveSpeedMotion::Linear: - ret = motion_control->stopLine(1.2, 1.2); + case ActiveMotion::Linear: { + const double resolved_acceleration = + acceleration > 0.0 ? acceleration : 10.0; + ret = motion_control->stopLine( + resolved_acceleration, resolved_acceleration); break; - case ActiveSpeedMotion::None: + } + case ActiveMotion::None: + if (robot_state && robot_state->isSteady() && + motion_control->getExecId() == -1) { + return Result::success(); + } ret = motion_control->stopMove(true, true); break; } - if (ret != arcs::common_interface::AUBO_OK) { + if (ret != arcs::common_interface::AUBO_OK && + ret != arcs::common_interface::AUBO_REQUEST_IGNORE) { return Result::failure( ArmErrorCode::CommandFailed, "[AuboArm] stopMotion failed: sdk ret=" + std::to_string(ret)); } - return Result::success(); + + constexpr auto kStopTimeout = std::chrono::seconds(5); + constexpr auto kPollInterval = std::chrono::milliseconds(50); + const auto deadline = std::chrono::steady_clock::now() + kStopTimeout; + int stable_samples = 0; + while (std::chrono::steady_clock::now() < deadline) { + const bool idle = robot_state && robot_state->isSteady() && + motion_control->getExecId() == -1; + if (idle) { + if (++stable_samples >= 3) { + return Result::success(); + } + } else { + stable_samples = 0; + } + std::this_thread::sleep_for(kPollInterval); + } + return Result::failure( + ArmErrorCode::Timeout, + "[AuboArm] stopMotion timed out waiting for controller idle"); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopMotion failed: ") + e.what()); } #else - active_speed_motion_.store(ActiveSpeedMotion::None); - busy_.store(false); return Result::success(); #endif } @@ -1152,8 +1284,12 @@ Result AuboArm::connect(const std::string& ip, const int port) sdk_->rpc_client->login(username_, password_); ip_ = ip; port_ = port > 0 ? port : 30004; - active_speed_motion_.store(ActiveSpeedMotion::None); - busy_.store(false); + { + std::lock_guard lock(mutex_); + motion_epoch_.fetch_add(1); + active_motion_.store(ActiveMotion::None); + busy_.store(false); + } connected_.store(true); hardware_safety_mode_.store(static_cast(SafetyMode::Unknown)); hardware_emergency_stopped_.store(false); @@ -1200,8 +1336,12 @@ Result AuboArm::disconnect() sdk_.reset(); #endif connected_.store(false); - active_speed_motion_.store(ActiveSpeedMotion::None); - busy_.store(false); + { + std::lock_guard lock(mutex_); + motion_epoch_.fetch_add(1); + active_motion_.store(ActiveMotion::None); + busy_.store(false); + } hardware_emergency_stopped_.store(false); hardware_estop_latched_.store(false); hardware_safety_mode_.store(static_cast(SafetyMode::Unknown)); @@ -1277,12 +1417,15 @@ void AuboArm::autoEnableMonitorLoop_() << ", source=" << emergency_source; } // The controller has already stopped the physical motion. - // Clear the retained direct-speed command as well so a - // blocking speedJoint/speedLine call observes cancellation - // when it returns, and recovery does not leave the arm - // permanently busy. - active_speed_motion_.store(ActiveSpeedMotion::None); - busy_.store(false); + // Invalidate every tracked direct motion so a blocking + // MoveJ/MoveL/speed call observes cancellation when it + // returns, and recovery cannot leave the arm busy. + { + std::lock_guard lock(mutex_); + motion_epoch_.fetch_add(1); + active_motion_.store(ActiveMotion::None); + busy_.store(false); + } } else if (hardware_estop_latched_.load() && isHardwareEmergencyStopReleased( safety_mode, emergency_source) && diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index 27072722..f65aa39c 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -221,7 +221,7 @@ grpc::Status gRPCArmServiceImpl::clearFault( } } -grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*, +grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext* context, const api::MoveJ_Request* request, api::MoveJ_Response* response) { @@ -236,8 +236,12 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } - const auto result = arm->moveJ(toJointPositionCommand(request->target()), - toMotionOptions(request->options())); + auto options = toMotionOptions(request->options()); + options.cancellation_requested = [context]() { + return context && context->IsCancelled(); + }; + const auto result = arm->moveJ( + toJointPositionCommand(request->target()), options); if (result.ok()) { CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveJ): success, id=" << device_id << ", positions=" << request->target().position_size(); @@ -249,7 +253,7 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*, } } -grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*, +grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext* context, const api::MoveL_Request* request, api::MoveL_Response* response) { @@ -267,9 +271,13 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*, const bool has_named_frame = (request->has_base_frame() && !request->base_frame().empty()) || (request->has_tcp_frame() && !request->tcp_frame().empty()); + auto options = toMotionOptions(request->options()); + options.cancellation_requested = [context]() { + return context && context->IsCancelled(); + }; const auto result = has_named_frame ? arm->moveL(toCartesianPose(request->target()), - toMotionOptions(request->options()), + options, request->has_base_frame() ? request->base_frame() : std::string{}, @@ -277,7 +285,7 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*, ? request->tcp_frame() : std::string{}) : arm->moveL(toCartesianPose(request->target()), - toMotionOptions(request->options()), + options, toFrameType(request->frame())); if (result.ok()) { CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveL): success, id=" << device_id