SmartBench/cmvr-es
8
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix aubo stop all motion cancellation

Browse Source
This commit is contained in:
xtkuang 2026-09-17 17:42:20 +08:00
parent 8ac589d607
commit c3c58f4563
3 changed files with 254 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h
View File

@ -3,6 +3,7 @@
#include <atomic> #include <atomic>
#include <condition_variable> #include <condition_variable>
#include <cstdint>
#include <memory> #include <memory>
#include <mutex> #include <mutex>
#include <optional> #include <optional>
@ -92,7 +93,7 @@ public:
bool busy() const override { return busy_.load(); } bool busy() const override { return busy_.load(); }
private: private:
enum class ActiveSpeedMotion { enum class ActiveMotion {
None, None,
Joint, Joint,
Linear, Linear,
@ -102,6 +103,11 @@ private:
bool validDof_(std::size_t size, std::string& error) const; bool validDof_(std::size_t size, std::string& error) const;
Result ensureConnected_(const std::string& context) const; Result ensureConnected_(const std::string& context) const;
Result ensureMotionReady_(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<ActiveMotion> requested_kind,
double acceleration);
#if defined(CMVR_HAS_AUBO_SDK) #if defined(CMVR_HAS_AUBO_SDK)
struct SdkState; struct SdkState;
@ -120,8 +126,8 @@ private:
double speed_scaling_{1.0}; double speed_scaling_{1.0};
std::atomic<bool> connected_{false}; std::atomic<bool> connected_{false};
std::atomic<bool> busy_{false}; std::atomic<bool> busy_{false};
std::atomic<ActiveSpeedMotion> active_speed_motion_{ std::atomic<ActiveMotion> active_motion_{ActiveMotion::None};
ActiveSpeedMotion::None}; std::atomic<std::uint64_t> motion_epoch_{0};
std::atomic<bool> emergency_stopped_{false}; std::atomic<bool> emergency_stopped_{false};
std::atomic<bool> hardware_emergency_stopped_{false}; std::atomic<bool> hardware_emergency_stopped_{false};
std::atomic<int> hardware_safety_mode_{0}; std::atomic<int> hardware_safety_mode_{0};

319
cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp
View File

@ -7,6 +7,7 @@
#include <set> #include <set>
#include <stdexcept> #include <stdexcept>
#include <thread> #include <thread>
#include <utility>
#include "common/base/logging/logger.h" #include "common/base/logging/logger.h"
@ -17,9 +18,27 @@
namespace cmvr::device { namespace cmvr::device {
namespace { namespace {
struct BusyGuard { class ScopeExit final {
std::atomic<bool>& busy; public:
~BusyGuard() { busy.store(false); } explicit ScopeExit(std::function<void()> 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<void()> callback_;
}; };
std::vector<std::string> defaultJointNames(const std::size_t dof) std::vector<std::string> defaultJointNames(const std::size_t dof)
@ -51,7 +70,8 @@ constexpr auto kAutoEnableModeTimeout = std::chrono::seconds(10);
int waitArrival( int waitArrival(
const arcs::aubo_sdk::RobotInterfacePtr& robot_interface, const arcs::aubo_sdk::RobotInterfacePtr& robot_interface,
const std::function<bool()>& cancellation_requested = {}) const std::function<bool()>& cancellation_requested = {},
const std::function<void()>& stop_on_cancel = {})
{ {
const auto deadline = const auto deadline =
std::chrono::steady_clock::now() + std::chrono::seconds(60); std::chrono::steady_clock::now() + std::chrono::seconds(60);
@ -67,6 +87,10 @@ int waitArrival(
mode == SafetyModeType::SystemEmergencyStop || source != 0; mode == SafetyModeType::SystemEmergencyStop || source != 0;
}; };
const auto stopMotion = [&]() { const auto stopMotion = [&]() {
if (stop_on_cancel) {
stop_on_cancel();
return;
}
try { try {
(void)robot_interface->getMotionControl()->stopMove(true, true); (void)robot_interface->getMotionControl()->stopMove(true, true);
} catch (...) { } catch (...) {
@ -74,36 +98,48 @@ int waitArrival(
}; };
int retry_count = 0; int retry_count = 0;
int exec_id = robot_interface->getMotionControl()->getExecId(); int exec_id = -1;
while (exec_id == -1 && retry_count++ < 5) { while (retry_count++ < 5) {
if (hardwareEmergencyStopActive()) {
return -3;
}
if (canceled()) { if (canceled()) {
stopMotion(); stopMotion();
return -2; return -2;
} }
if (hardwareEmergencyStopActive()) { exec_id = robot_interface->getMotionControl()->getExecId();
return -3; if (exec_id != -1) {
break;
} }
std::this_thread::sleep_for(std::chrono::milliseconds(50)); std::this_thread::sleep_for(std::chrono::milliseconds(50));
exec_id = robot_interface->getMotionControl()->getExecId();
} }
if (exec_id == -1) { if (exec_id == -1) {
return -1; if (hardwareEmergencyStopActive()) {
} return -3;
while (robot_interface->getMotionControl()->getExecId() != -1) { }
if (canceled()) { if (canceled()) {
stopMotion(); stopMotion();
return -2; return -2;
} }
return -1;
}
while (true) {
if (hardwareEmergencyStopActive()) { if (hardwareEmergencyStopActive()) {
return -3; return -3;
} }
if (canceled()) {
stopMotion();
return -2;
}
if (std::chrono::steady_clock::now() >= deadline) { if (std::chrono::steady_clock::now() >= deadline) {
stopMotion(); stopMotion();
return -4; return -4;
} }
if (robot_interface->getMotionControl()->getExecId() == -1) {
return 0;
}
std::this_thread::sleep_for(std::chrono::milliseconds(50)); std::this_thread::sleep_for(std::chrono::milliseconds(50));
} }
return 0;
} }
bool isHardwareEmergencyStop( bool isHardwareEmergencyStop(
@ -274,6 +310,33 @@ AuboArm::~AuboArm()
(void)disconnect(); (void)disconnect();
} }
bool AuboArm::tryBeginMotion_(const ActiveMotion kind, std::uint64_t& token)
{
std::lock_guard<std::mutex> 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<std::mutex> lock(mutex_);
if (motion_epoch_.load() == token && active_motion_.load() == kind) {
active_motion_.store(ActiveMotion::None);
busy_.store(false);
}
}
bool AuboArm::init() bool AuboArm::init()
{ {
if (ip_.empty()) { if (ip_.empty()) {
@ -639,10 +702,13 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o
if (!ready.ok()) { if (!ready.ok()) {
return ready; 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_); 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) #if defined(CMVR_HAS_AUBO_SDK)
try { try {
@ -656,6 +722,16 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o
} }
auto motion_control = robot_interface->getMotionControl(); auto motion_control = robot_interface->getMotionControl();
motion_control->setSpeedFraction(speed_scaling_); 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( const int ret = motion_control->moveJoint(
target.position, target.position,
options.acceleration > 0.0 ? options.acceleration : 0.5, options.acceleration > 0.0 ? options.acceleration : 0.5,
@ -666,10 +742,20 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o
ret, ret,
arcs::common_interface::AUBO_OK, arcs::common_interface::AUBO_OK,
arcs::common_interface::AUBO_REQUEST_IGNORE, arcs::common_interface::AUBO_REQUEST_IGNORE,
[&robot_interface, &options]() { [&robot_interface, &motion_control, &canceled]() {
return waitArrival( return waitArrival(robot_interface, canceled,
robot_interface, options.cancellation_requested); [&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 if (outcome == aubo_internal::MotionCommandOutcome::CompletedWithoutMotion
|| outcome == aubo_internal::MotionCommandOutcome::CompletedAfterMotion) { || outcome == aubo_internal::MotionCommandOutcome::CompletedAfterMotion) {
return Result::success(); return Result::success();
@ -700,25 +786,20 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration
if (!ready.ok()) { if (!ready.ok()) {
return ready; return ready;
} }
if (busy_.exchange(true)) { std::uint64_t motion_token = 0;
if (!tryBeginMotion_(ActiveMotion::Joint, motion_token)) {
return Result::failure( return Result::failure(
ArmErrorCode::RobotNotReady, ArmErrorCode::RobotNotReady,
"[AuboArm] arm is busy: " + id_); "[AuboArm] arm is busy: " + id_);
} }
active_speed_motion_.store(ActiveSpeedMotion::Joint); ScopeExit motion_owner([this, motion_token]() {
const auto clear_owned_motion = [this]() { releaseMotion_(ActiveMotion::Joint, motion_token);
auto expected = ActiveSpeedMotion::Joint; });
if (active_speed_motion_.compare_exchange_strong(
expected, ActiveSpeedMotion::None)) {
busy_.store(false);
}
};
#if defined(CMVR_HAS_AUBO_SDK) #if defined(CMVR_HAS_AUBO_SDK)
try { try {
const auto robot_names = sdk_->rpc_client->getRobotNames(); const auto robot_names = sdk_->rpc_client->getRobotNames();
if (robot_names.empty()) { if (robot_names.empty()) {
clear_owned_motion();
return Result::failure( return Result::failure(
ArmErrorCode::RobotNotReady, ArmErrorCode::RobotNotReady,
"[AuboArm] robot name list is empty"); "[AuboArm] robot name list is empty");
@ -726,7 +807,6 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration
const auto robot_interface = const auto robot_interface =
sdk_->rpc_client->getRobotInterface(robot_names.front()); sdk_->rpc_client->getRobotInterface(robot_names.front());
if (!robot_interface || !robot_interface->getMotionControl()) { if (!robot_interface || !robot_interface->getMotionControl()) {
clear_owned_motion();
return Result::failure( return Result::failure(
ArmErrorCode::RobotNotReady, ArmErrorCode::RobotNotReady,
"[AuboArm] motion control interface is unavailable"); "[AuboArm] motion control interface is unavailable");
@ -740,7 +820,7 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration
const int ret = motion_control->speedJoint( const int ret = motion_control->speedJoint(
velocity.velocity, resolved_acceleration, resolved_duration); 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 // StopAll/stopMotion may race with the blocking SDK call. Stop a
// second time after it returns so a late submission cannot leave // second time after it returns so a late submission cannot leave
// the robot moving. // the robot moving.
@ -750,7 +830,6 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration
"[AuboArm] speedJ canceled by stopMotion or hardware safety event"); "[AuboArm] speedJ canceled by stopMotion or hardware safety event");
} }
if (ret != arcs::common_interface::AUBO_OK) { if (ret != arcs::common_interface::AUBO_OK) {
clear_owned_motion();
return Result::failure( return Result::failure(
ArmErrorCode::CommandFailed, ArmErrorCode::CommandFailed,
"[AuboArm] speedJ failed: sdk ret=" + "[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 // Aubo speedJoint keeps following the accepted velocity after this
// call returns. Keep busy_ and the motion kind latched until an // call returns. Keep busy_ and the motion kind latched until an
// explicit stopJ/stopMotion request clears them. // explicit stopJ/stopMotion request clears them.
motion_owner.dismiss();
return Result::success(); return Result::success();
} catch (const std::exception& e) { } catch (const std::exception& e) {
clear_owned_motion();
return Result::failure( return Result::failure(
ArmErrorCode::CommandFailed, ArmErrorCode::CommandFailed,
std::string{"[AuboArm] speedJ failed: "} + e.what()); std::string{"[AuboArm] speedJ failed: "} + e.what());
} }
#else #else
clear_owned_motion();
return unsupported_("speedJ"); return unsupported_("speedJ");
#endif #endif
} }
Result AuboArm::stopJ(double acceleration) Result AuboArm::stopJ(double acceleration)
{ {
(void)acceleration; return stopMotion_(ActiveMotion::Joint, acceleration);
return stopMotion();
} }
Result AuboArm::moveL(const CartesianPose& target, Result AuboArm::moveL(const CartesianPose& target,
@ -800,10 +877,13 @@ Result AuboArm::moveL(const CartesianPose& target,
if (!ready.ok()) { if (!ready.ok()) {
return ready; 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_); 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) #if defined(CMVR_HAS_AUBO_SDK)
try { try {
@ -881,6 +961,16 @@ Result AuboArm::moveL(const CartesianPose& target,
"base_frame: " + selected_base_frame); "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( const int ret = motion_control->moveLine(
pose, pose,
options.acceleration > 0.0 ? options.acceleration : 0.5, options.acceleration > 0.0 ? options.acceleration : 0.5,
@ -891,10 +981,17 @@ Result AuboArm::moveL(const CartesianPose& target,
ret, ret,
arcs::common_interface::AUBO_OK, arcs::common_interface::AUBO_OK,
arcs::common_interface::AUBO_REQUEST_IGNORE, arcs::common_interface::AUBO_REQUEST_IGNORE,
[&robot_interface, &options]() { [&robot_interface, &motion_control, &canceled]() {
return waitArrival( return waitArrival(robot_interface, canceled,
robot_interface, options.cancellation_requested); [&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 if (outcome == aubo_internal::MotionCommandOutcome::CompletedWithoutMotion
|| outcome == aubo_internal::MotionCommandOutcome::CompletedAfterMotion) { || outcome == aubo_internal::MotionCommandOutcome::CompletedAfterMotion) {
return Result::success(); return Result::success();
@ -926,25 +1023,20 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
if (!ready.ok()) { if (!ready.ok()) {
return ready; return ready;
} }
if (busy_.exchange(true)) { std::uint64_t motion_token = 0;
if (!tryBeginMotion_(ActiveMotion::Linear, motion_token)) {
return Result::failure( return Result::failure(
ArmErrorCode::RobotNotReady, ArmErrorCode::RobotNotReady,
"[AuboArm] arm is busy: " + id_); "[AuboArm] arm is busy: " + id_);
} }
active_speed_motion_.store(ActiveSpeedMotion::Linear); ScopeExit motion_owner([this, motion_token]() {
const auto clear_owned_motion = [this]() { releaseMotion_(ActiveMotion::Linear, motion_token);
auto expected = ActiveSpeedMotion::Linear; });
if (active_speed_motion_.compare_exchange_strong(
expected, ActiveSpeedMotion::None)) {
busy_.store(false);
}
};
#if defined(CMVR_HAS_AUBO_SDK) #if defined(CMVR_HAS_AUBO_SDK)
try { try {
const auto robot_names = sdk_->rpc_client->getRobotNames(); const auto robot_names = sdk_->rpc_client->getRobotNames();
if (robot_names.empty()) { if (robot_names.empty()) {
clear_owned_motion();
return Result::failure( return Result::failure(
ArmErrorCode::RobotNotReady, ArmErrorCode::RobotNotReady,
"[AuboArm] robot name list is empty"); "[AuboArm] robot name list is empty");
@ -952,7 +1044,6 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
const auto robot_interface = const auto robot_interface =
sdk_->rpc_client->getRobotInterface(robot_names.front()); sdk_->rpc_client->getRobotInterface(robot_names.front());
if (!robot_interface || !robot_interface->getMotionControl()) { if (!robot_interface || !robot_interface->getMotionControl()) {
clear_owned_motion();
return Result::failure( return Result::failure(
ArmErrorCode::RobotNotReady, ArmErrorCode::RobotNotReady,
"[AuboArm] motion control interface is unavailable"); "[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}; velocity.wx, velocity.wy, velocity.wz, 0.0, 0.0, 0.0};
if (frame == FrameType::Tool) { if (frame == FrameType::Tool) {
if (!robot_interface->getRobotState()) { if (!robot_interface->getRobotState()) {
clear_owned_motion();
return Result::failure( return Result::failure(
ArmErrorCode::RobotNotReady, ArmErrorCode::RobotNotReady,
"[AuboArm] robot state interface is unavailable"); "[AuboArm] robot state interface is unavailable");
@ -972,7 +1062,6 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
auto tool_frame = auto tool_frame =
robot_interface->getRobotState()->getTcpPose(); robot_interface->getRobotState()->getTcpPose();
if (tool_frame.size() < 6) { if (tool_frame.size() < 6) {
clear_owned_motion();
return Result::failure( return Result::failure(
ArmErrorCode::CommandFailed, ArmErrorCode::CommandFailed,
"[AuboArm] speedL failed: tcp pose size is less than 6"); "[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; tool_frame[2] = 0.0;
const auto math = sdk_->rpc_client->getMath(); const auto math = sdk_->rpc_client->getMath();
if (!math) { if (!math) {
clear_owned_motion();
return Result::failure( return Result::failure(
ArmErrorCode::RobotNotReady, ArmErrorCode::RobotNotReady,
"[AuboArm] math interface is unavailable"); "[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); angular_velocity = math->poseTrans(tool_frame, angular_velocity);
if (linear_velocity.size() < 3 || if (linear_velocity.size() < 3 ||
angular_velocity.size() < 3) { angular_velocity.size() < 3) {
clear_owned_motion();
return Result::failure( return Result::failure(
ArmErrorCode::CommandFailed, ArmErrorCode::CommandFailed,
"[AuboArm] speedL failed: frame conversion returned an invalid velocity"); "[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( const int ret = motion_control->speedLine(
speed, resolved_acceleration, resolved_duration); speed, resolved_acceleration, resolved_duration);
if (active_speed_motion_.load() != ActiveSpeedMotion::Linear) { if (motionCanceled_(motion_token)) {
(void)motion_control->stopLine( (void)motion_control->stopLine(
resolved_acceleration, resolved_acceleration); resolved_acceleration, resolved_acceleration);
return Result::failure( 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"); "[AuboArm] speedL canceled by stopMotion or hardware safety event");
} }
if (ret != arcs::common_interface::AUBO_OK) { if (ret != arcs::common_interface::AUBO_OK) {
clear_owned_motion();
return Result::failure( return Result::failure(
ArmErrorCode::CommandFailed, ArmErrorCode::CommandFailed,
"[AuboArm] speedL failed: sdk ret=" + "[AuboArm] speedL failed: sdk ret=" +
std::to_string(ret)); std::to_string(ret));
} }
motion_owner.dismiss();
return Result::success(); return Result::success();
} catch (const std::exception& e) { } catch (const std::exception& e) {
clear_owned_motion();
return Result::failure( return Result::failure(
ArmErrorCode::CommandFailed, ArmErrorCode::CommandFailed,
std::string{"[AuboArm] speedL failed: "} + e.what()); std::string{"[AuboArm] speedL failed: "} + e.what());
} }
#else #else
clear_owned_motion();
return unsupported_("speedL"); return unsupported_("speedL");
#endif #endif
} }
Result AuboArm::stopL(std::optional<double> acceleration) Result AuboArm::stopL(std::optional<double> acceleration)
{ {
(void)acceleration; return stopMotion_(
return stopMotion(); ActiveMotion::Linear, acceleration.has_value() ? *acceleration : 0.0);
} }
Result AuboArm::stopMotion() Result AuboArm::stopMotion()
{
return stopMotion_(std::nullopt, 0.0);
}
Result AuboArm::stopMotion_(const std::optional<ActiveMotion> requested_kind,
const double acceleration)
{ {
if (hardware_estop_latched_.load()) { if (hardware_estop_latched_.load()) {
auto_recovery_suppressed_.store(true); auto_recovery_suppressed_.store(true);
} }
#if defined(CMVR_HAS_AUBO_SDK) ActiveMotion active_motion = ActiveMotion::None;
if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { {
active_speed_motion_.store(ActiveSpeedMotion::None); std::lock_guard<std::mutex> lock(mutex_);
busy_.store(false); 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(); return Result::success();
} }
const auto active_speed =
active_speed_motion_.exchange(ActiveSpeedMotion::None);
busy_.store(false);
try { try {
const auto robot_names = sdk_->rpc_client->getRobotNames(); const auto robot_names = sdk_->rpc_client->getRobotNames();
if (robot_names.empty()) { if (robot_names.empty()) {
@ -1068,31 +1170,61 @@ Result AuboArm::stopMotion()
return Result::success(); return Result::success();
} }
const auto motion_control = robot_interface->getMotionControl(); 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; int ret = arcs::common_interface::AUBO_OK;
switch (active_speed) { switch (stop_kind) {
case ActiveSpeedMotion::Joint: case ActiveMotion::Joint:
ret = motion_control->stopJoint(1.5); ret = motion_control->stopJoint(
acceleration > 0.0 ? acceleration : 31.0);
break; break;
case ActiveSpeedMotion::Linear: case ActiveMotion::Linear: {
ret = motion_control->stopLine(1.2, 1.2); const double resolved_acceleration =
acceleration > 0.0 ? acceleration : 10.0;
ret = motion_control->stopLine(
resolved_acceleration, resolved_acceleration);
break; 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); ret = motion_control->stopMove(true, true);
break; 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( return Result::failure(
ArmErrorCode::CommandFailed, ArmErrorCode::CommandFailed,
"[AuboArm] stopMotion failed: sdk ret=" + "[AuboArm] stopMotion failed: sdk ret=" +
std::to_string(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) { } catch (const std::exception& e) {
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopMotion failed: ") + e.what()); return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopMotion failed: ") + e.what());
} }
#else #else
active_speed_motion_.store(ActiveSpeedMotion::None);
busy_.store(false);
return Result::success(); return Result::success();
#endif #endif
} }
@ -1152,8 +1284,12 @@ Result AuboArm::connect(const std::string& ip, const int port)
sdk_->rpc_client->login(username_, password_); sdk_->rpc_client->login(username_, password_);
ip_ = ip; ip_ = ip;
port_ = port > 0 ? port : 30004; port_ = port > 0 ? port : 30004;
active_speed_motion_.store(ActiveSpeedMotion::None); {
busy_.store(false); std::lock_guard<std::mutex> lock(mutex_);
motion_epoch_.fetch_add(1);
active_motion_.store(ActiveMotion::None);
busy_.store(false);
}
connected_.store(true); connected_.store(true);
hardware_safety_mode_.store(static_cast<int>(SafetyMode::Unknown)); hardware_safety_mode_.store(static_cast<int>(SafetyMode::Unknown));
hardware_emergency_stopped_.store(false); hardware_emergency_stopped_.store(false);
@ -1200,8 +1336,12 @@ Result AuboArm::disconnect()
sdk_.reset(); sdk_.reset();
#endif #endif
connected_.store(false); connected_.store(false);
active_speed_motion_.store(ActiveSpeedMotion::None); {
busy_.store(false); std::lock_guard<std::mutex> lock(mutex_);
motion_epoch_.fetch_add(1);
active_motion_.store(ActiveMotion::None);
busy_.store(false);
}
hardware_emergency_stopped_.store(false); hardware_emergency_stopped_.store(false);
hardware_estop_latched_.store(false); hardware_estop_latched_.store(false);
hardware_safety_mode_.store(static_cast<int>(SafetyMode::Unknown)); hardware_safety_mode_.store(static_cast<int>(SafetyMode::Unknown));
@ -1277,12 +1417,15 @@ void AuboArm::autoEnableMonitorLoop_()
<< ", source=" << emergency_source; << ", source=" << emergency_source;
} }
// The controller has already stopped the physical motion. // The controller has already stopped the physical motion.
// Clear the retained direct-speed command as well so a // Invalidate every tracked direct motion so a blocking
// blocking speedJoint/speedLine call observes cancellation // MoveJ/MoveL/speed call observes cancellation when it
// when it returns, and recovery does not leave the arm // returns, and recovery cannot leave the arm busy.
// permanently busy. {
active_speed_motion_.store(ActiveSpeedMotion::None); std::lock_guard<std::mutex> lock(mutex_);
busy_.store(false); motion_epoch_.fetch_add(1);
active_motion_.store(ActiveMotion::None);
busy_.store(false);
}
} else if (hardware_estop_latched_.load() && } else if (hardware_estop_latched_.load() &&
isHardwareEmergencyStopReleased( isHardwareEmergencyStopReleased(
safety_mode, emergency_source) && safety_mode, emergency_source) &&

20
cmvr-es/service/grpc/src/grpc_arm_service.cpp
View File

@ -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, const api::MoveJ_Request* request,
api::MoveJ_Response* response) api::MoveJ_Response* response)
{ {
@ -236,8 +236,12 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*,
if (!arm) { if (!arm) {
return setDeviceNotFound(response, device_id); return setDeviceNotFound(response, device_id);
} }
const auto result = arm->moveJ(toJointPositionCommand(request->target()), auto options = toMotionOptions(request->options());
toMotionOptions(request->options())); options.cancellation_requested = [context]() {
return context && context->IsCancelled();
};
const auto result = arm->moveJ(
toJointPositionCommand(request->target()), options);
if (result.ok()) { if (result.ok()) {
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveJ): success, id=" << device_id CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveJ): success, id=" << device_id
<< ", positions=" << request->target().position_size(); << ", 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, const api::MoveL_Request* request,
api::MoveL_Response* response) api::MoveL_Response* response)
{ {
@ -267,9 +271,13 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*,
const bool has_named_frame = const bool has_named_frame =
(request->has_base_frame() && !request->base_frame().empty()) || (request->has_base_frame() && !request->base_frame().empty()) ||
(request->has_tcp_frame() && !request->tcp_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 const auto result = has_named_frame
? arm->moveL(toCartesianPose(request->target()), ? arm->moveL(toCartesianPose(request->target()),
toMotionOptions(request->options()), options,
request->has_base_frame() request->has_base_frame()
? request->base_frame() ? request->base_frame()
: std::string{}, : std::string{},
@ -277,7 +285,7 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*,
? request->tcp_frame() ? request->tcp_frame()
: std::string{}) : std::string{})
: arm->moveL(toCartesianPose(request->target()), : arm->moveL(toCartesianPose(request->target()),
toMotionOptions(request->options()), options,
toFrameType(request->frame())); toFrameType(request->frame()));
if (result.ok()) { if (result.ok()) {
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveL): success, id=" << device_id CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveL): success, id=" << device_id