fix aubo stop all motion cancellation
This commit is contained in:
parent
8ac589d607
commit
c3c58f4563
@ -3,6 +3,7 @@
|
||||
|
||||
#include <atomic>
|
||||
#include <condition_variable>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
@ -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<ActiveMotion> requested_kind,
|
||||
double acceleration);
|
||||
|
||||
#if defined(CMVR_HAS_AUBO_SDK)
|
||||
struct SdkState;
|
||||
@ -120,8 +126,8 @@ private:
|
||||
double speed_scaling_{1.0};
|
||||
std::atomic<bool> connected_{false};
|
||||
std::atomic<bool> busy_{false};
|
||||
std::atomic<ActiveSpeedMotion> active_speed_motion_{
|
||||
ActiveSpeedMotion::None};
|
||||
std::atomic<ActiveMotion> active_motion_{ActiveMotion::None};
|
||||
std::atomic<std::uint64_t> motion_epoch_{0};
|
||||
std::atomic<bool> emergency_stopped_{false};
|
||||
std::atomic<bool> hardware_emergency_stopped_{false};
|
||||
std::atomic<int> hardware_safety_mode_{0};
|
||||
|
||||
@ -7,6 +7,7 @@
|
||||
#include <set>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
|
||||
#include "common/base/logging/logger.h"
|
||||
|
||||
@ -17,9 +18,27 @@
|
||||
namespace cmvr::device {
|
||||
namespace {
|
||||
|
||||
struct BusyGuard {
|
||||
std::atomic<bool>& busy;
|
||||
~BusyGuard() { busy.store(false); }
|
||||
class ScopeExit final {
|
||||
public:
|
||||
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)
|
||||
@ -51,7 +70,8 @@ constexpr auto kAutoEnableModeTimeout = std::chrono::seconds(10);
|
||||
|
||||
int waitArrival(
|
||||
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 =
|
||||
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;
|
||||
if (hardwareEmergencyStopActive()) {
|
||||
return -3;
|
||||
}
|
||||
while (robot_interface->getMotionControl()->getExecId() != -1) {
|
||||
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<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()
|
||||
{
|
||||
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<double> 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<ActiveMotion> 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<std::mutex> 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));
|
||||
}
|
||||
|
||||
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);
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
motion_epoch_.fetch_add(1);
|
||||
active_motion_.store(ActiveMotion::None);
|
||||
busy_.store(false);
|
||||
}
|
||||
connected_.store(true);
|
||||
hardware_safety_mode_.store(static_cast<int>(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);
|
||||
{
|
||||
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_estop_latched_.store(false);
|
||||
hardware_safety_mode_.store(static_cast<int>(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);
|
||||
// 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<std::mutex> 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) &&
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user