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

fix(aubo): make stop release control safely

Browse Source
This commit is contained in:
xtkuang 2026-08-05 15:36:49 +08:00
parent ab4bbfac50
commit e77c2cfea8
14 changed files with 1589 additions and 142 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
cmvr-es/devices/arm/aubo_arm/CMakeLists.txt
View File

@ -67,6 +67,19 @@ if(BUILD_TESTING)
)
set_tests_properties(aubo_arm_motion_result_test PROPERTIES TIMEOUT 10)
add_executable(aubo_motion_state_test
tests/aubo_motion_state_test.cpp
)
target_include_directories(aubo_motion_state_test
PRIVATE
${CMAKE_SOURCE_DIR}/cmvr-es
)
add_test(
NAME aubo_motion_state_test
COMMAND aubo_motion_state_test
)
set_tests_properties(aubo_motion_state_test PROPERTIES TIMEOUT 10)
add_executable(aubo_arm_json_command_test
tests/aubo_arm_json_command_test.cpp
)

580
cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp
View File

@ -1,6 +1,7 @@
#include "devices/arm/aubo_arm/aubo_arm.h"
#include "devices/arm/aubo_arm/aubo_motion_result.h"
#include "devices/arm/aubo_arm/aubo_motion_state.h"
#include <algorithm>
#include <cctype>
@ -18,11 +19,133 @@
namespace cmvr::device {
namespace {
struct BusyGuard {
std::atomic<bool>& busy;
~BusyGuard() { busy.store(false); }
class MotionOwnerGuard final {
public:
MotionOwnerGuard(
aubo_internal::MotionState& state,
std::atomic<bool>& busy,
const aubo_internal::MotionToken token)
: state_(state), busy_(busy), token_(token)
{
}
~MotionOwnerGuard() noexcept
{
try {
if (requires_settlement_ && !settled_) {
state_.failMotion(token_);
} else {
state_.finish(token_, finish_mode_);
}
busy_.store(state_.busy());
} catch (...) {
// A failed state lock must not terminate an RPC unwind. Preserve
// the conservative externally visible state instead.
busy_.store(true);
}
}
MotionOwnerGuard(const MotionOwnerGuard&) = delete;
MotionOwnerGuard& operator=(const MotionOwnerGuard&) = delete;
void requireExplicitSettlement() noexcept
{
requires_settlement_ = true;
}
void settle() noexcept { settled_ = true; }
void clearOnFinish() noexcept
{
finish_mode_ = aubo_internal::MotionFinishMode::Clear;
}
void retainKind() noexcept
{
finish_mode_ = aubo_internal::MotionFinishMode::Retain;
}
private:
aubo_internal::MotionState& state_;
std::atomic<bool>& busy_;
aubo_internal::MotionToken token_;
aubo_internal::MotionFinishMode finish_mode_{
aubo_internal::MotionFinishMode::RestorePrevious};
bool requires_settlement_{false};
bool settled_{false};
};
class StopStateGuard final {
public:
StopStateGuard(
aubo_internal::MotionState& state,
std::atomic<bool>& busy)
: state_(state), busy_(busy)
{
}
~StopStateGuard() noexcept
{
if (!completed_) {
try {
state_.failStop();
} catch (...) {
// Keep the facade fail-closed even if state cleanup fails.
}
busy_.store(true);
}
}
StopStateGuard(const StopStateGuard&) = delete;
StopStateGuard& operator=(const StopStateGuard&) = delete;
bool complete()
{
if (!state_.completeStop()) {
return false;
}
busy_.store(false);
completed_ = true;
return true;
}
private:
aubo_internal::MotionState& state_;
std::atomic<bool>& busy_;
bool completed_{false};
};
const char* motionStartFailure(
const aubo_internal::MotionStartStatus status) noexcept
{
switch (status) {
case aubo_internal::MotionStartStatus::Invalid:
return "invalid motion type";
case aubo_internal::MotionStartStatus::Busy:
return "another motion is active";
case aubo_internal::MotionStartStatus::Stopping:
return "a stop operation is in progress";
case aubo_internal::MotionStartStatus::Blocked:
return "the previous stop did not complete; retry stopMotion or reconnect";
case aubo_internal::MotionStartStatus::Started:
break;
}
return "unknown motion state";
}
const char* motionKindName(const aubo_internal::MotionKind kind) noexcept
{
switch (kind) {
case aubo_internal::MotionKind::Joint:
return "joint";
case aubo_internal::MotionKind::Linear:
return "linear";
case aubo_internal::MotionKind::None:
break;
}
return "unknown";
}
std::vector<std::string> defaultJointNames(const std::size_t dof)
{
std::vector<std::string> names;
@ -182,21 +305,40 @@ bool waitForRobotMode(const RobotInterfacePtr& robot_interface,
return false;
}
int waitArrival(const RobotInterfacePtr& robot_interface)
template <typename IsCancelled>
aubo_internal::MotionWaitResult waitArrival(
const RobotInterfacePtr& robot_interface,
IsCancelled&& is_cancelled)
{
int retry_count = 0;
if (is_cancelled()) {
return aubo_internal::MotionWaitResult::Cancelled;
}
int exec_id = robot_interface->getMotionControl()->getExecId();
while (exec_id == -1 && retry_count++ < 5) {
if (is_cancelled()) {
return aubo_internal::MotionWaitResult::Cancelled;
}
std::this_thread::sleep_for(std::chrono::milliseconds(50));
if (is_cancelled()) {
return aubo_internal::MotionWaitResult::Cancelled;
}
exec_id = robot_interface->getMotionControl()->getExecId();
}
if (exec_id == -1) {
return -1;
return is_cancelled()
? aubo_internal::MotionWaitResult::Cancelled
: aubo_internal::MotionWaitResult::Failed;
}
while (robot_interface->getMotionControl()->getExecId() != -1) {
if (is_cancelled()) {
return aubo_internal::MotionWaitResult::Cancelled;
}
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
return 0;
return is_cancelled()
? aubo_internal::MotionWaitResult::Cancelled
: aubo_internal::MotionWaitResult::Completed;
}
bool waitServoModeSelect(const RobotInterfacePtr& robot_interface, const int mode)
@ -228,6 +370,8 @@ CartesianPose poseFromVector(const std::vector<double>& values)
struct AuboArm::SdkState {
std::shared_ptr<arcs::aubo_sdk::RpcClient> rpc_client;
std::shared_ptr<aubo_internal::MotionState> motion_state{
std::make_shared<aubo_internal::MotionState>()};
};
AuboArm::AuboArm(const config::RobotArmConfig& cfg)
@ -423,7 +567,7 @@ ArmState AuboArm::getRobotState() const
state.connected = connected_.load();
state.powered_on = state.connected;
state.brake_released = state.connected;
state.moving = busy_.load();
state.moving = busy();
state.robot_mode = getRobotMode();
state.safety_mode = getSafetyMode();
state.control_mode = getControlMode();
@ -515,7 +659,16 @@ RobotMode AuboArm::getRobotMode() const
if (emergency_stopped_) {
return RobotMode::Stopped;
}
return busy_.load() ? RobotMode::Running : RobotMode::Idle;
return busy() ? RobotMode::Running : RobotMode::Idle;
}
bool AuboArm::busy() const
{
std::lock_guard lock(mutex_);
if (sdk_) {
return sdk_->motion_state->busy();
}
return false;
}
Result AuboArm::torqueOn()
@ -612,37 +765,60 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o
if (!validDof_(target.position.size(), error)) {
return Result::failure(ArmErrorCode::InvalidDof, error);
}
const auto ready = ensureConnected_("moveJ");
if (!ready.ok()) {
return ready;
}
if (busy_.exchange(true)) {
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_);
}
BusyGuard busy_guard{busy_};
try {
const auto robot_names = sdk_->rpc_client->getRobotNames();
std::unique_lock submit_lock(mutex_);
const auto locked_ready = ensureConnected_("moveJ");
if (!locked_ready.ok()) {
return locked_ready;
}
const auto rpc_client = sdk_->rpc_client;
const auto motion_state = sdk_->motion_state;
const auto motion = motion_state->begin(
aubo_internal::MotionKind::Joint);
if (!motion.started()) {
return Result::failure(
ArmErrorCode::RobotNotReady,
"[AuboArm] moveJ rejected: " +
std::string(motionStartFailure(motion.status)) +
", id=" + id_);
}
busy_.store(true);
MotionOwnerGuard motion_owner{
*motion_state, busy_, motion.token};
const auto robot_names = rpc_client->getRobotNames();
if (robot_names.empty()) {
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
}
auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
auto robot_interface = rpc_client->getRobotInterface(robot_names.front());
if (!robot_interface) {
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
}
auto motion_control = robot_interface->getMotionControl();
motion_control->setSpeedFraction(speed_scaling_);
motion_owner.requireExplicitSettlement();
const int ret = motion_control->moveJoint(
target.position,
options.acceleration > 0.0 ? options.acceleration : 0.5,
options.velocity > 0.0 ? options.velocity : 0.5,
options.blend_radius,
0);
if (ret == arcs::common_interface::AUBO_OK) {
submit_lock.unlock();
} else {
motion_owner.settle();
}
const auto outcome = aubo_internal::resolveMotionCommand(
ret,
arcs::common_interface::AUBO_OK,
arcs::common_interface::AUBO_REQUEST_IGNORE,
[&robot_interface]() { return waitArrival(robot_interface); });
[&robot_interface, motion_state, token = motion.token]() {
return waitArrival(
robot_interface,
[motion_state, token]() {
return motion_state->cancelled(token);
});
});
switch (outcome) {
case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion:
CMVR_LOG(DEBUG) << "[AuboArm] moveJ completed without motion: sdk ret="
@ -650,7 +826,14 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o
<< arcs::common_interface::returnValue2Str(ret) << ")";
return Result::success();
case aubo_internal::MotionCommandOutcome::CompletedAfterMotion:
motion_owner.clearOnFinish();
motion_owner.settle();
return Result::success();
case aubo_internal::MotionCommandOutcome::Cancelled:
motion_owner.settle();
return Result::failure(
ArmErrorCode::CommandRejected,
"[AuboArm] moveJ stopped by stopMotion");
case aubo_internal::MotionCommandOutcome::SubmitFailed:
return Result::failure(
ArmErrorCode::CommandFailed,
@ -671,18 +854,30 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration
if (!validDof_(velocity.velocity.size(), error)) {
return Result::failure(ArmErrorCode::InvalidDof, error);
}
const auto ready = ensureConnected_("speedJ");
if (!ready.ok()) {
return ready;
}
if (busy_.exchange(true)) {
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_);
}
BusyGuard busy_guard{busy_};
try {
std::unique_lock submit_lock(mutex_);
const auto locked_ready = ensureConnected_("speedJ");
if (!locked_ready.ok()) {
return locked_ready;
}
const auto rpc_client = sdk_->rpc_client;
const auto motion_state = sdk_->motion_state;
const auto motion = motion_state->begin(
aubo_internal::MotionKind::Joint, true);
if (!motion.started()) {
return Result::failure(
ArmErrorCode::RobotNotReady,
"[AuboArm] speedJ rejected: " +
std::string(motionStartFailure(motion.status)) +
", id=" + id_);
}
busy_.store(true);
MotionOwnerGuard motion_owner{
*motion_state, busy_, motion.token};
Result interface_result;
auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "speedJ", interface_result);
auto robot_interface = getPrimaryRobotInterface(
rpc_client, "speedJ", interface_result);
if (!interface_result.ok()) {
return interface_result;
}
@ -690,14 +885,31 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration
robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_);
const double resolved_acceleration = acceleration > 0.0 ? acceleration : 1.5;
const double resolved_duration = duration > 0.0 ? duration : 100.0;
motion_owner.requireExplicitSettlement();
submit_lock.unlock();
if (motion_state->cancelled(motion.token)) {
motion_owner.settle();
return Result::failure(
ArmErrorCode::CommandRejected,
"[AuboArm] speedJ stopped by stopMotion before submission");
}
const int ret = robot_interface->getMotionControl()->speedJoint(
velocity.velocity,
resolved_acceleration,
resolved_duration);
if (motion_state->cancelled(motion.token)) {
motion_owner.settle();
return Result::failure(
ArmErrorCode::CommandRejected,
"[AuboArm] speedJ stopped by stopMotion");
}
if (ret != 0) {
motion_owner.settle();
return Result::failure(ArmErrorCode::CommandFailed,
"[AuboArm] speedJ failed: ret=" + std::to_string(ret));
}
motion_owner.retainKind();
motion_owner.settle();
return Result::success();
} catch (const std::exception& e) {
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] speedJ failed: ") + e.what());
@ -706,46 +918,38 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration
Result AuboArm::stopJ(double acceleration)
{
if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
return Result::success();
}
try {
Result interface_result;
auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "stopJ", interface_result);
if (!interface_result.ok()) {
return interface_result;
}
const double resolved_acceleration = acceleration > 0.0 ? acceleration : 31.0;
const int ret = robot_interface->getMotionControl()->stopJoint(resolved_acceleration);
busy_.store(false);
if (ret != 0) {
return Result::failure(ArmErrorCode::CommandFailed,
"[AuboArm] stopJ failed: ret=" + std::to_string(ret));
}
return Result::success();
} catch (const std::exception& e) {
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopJ failed: ") + e.what());
}
return stopMotion_(MotionStopKind::Joint, acceleration);
}
Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame)
{
(void)frame;
const auto ready = ensureConnected_("moveL");
if (!ready.ok()) {
return ready;
}
if (busy_.exchange(true)) {
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_);
}
BusyGuard busy_guard{busy_};
try {
const auto robot_names = sdk_->rpc_client->getRobotNames();
std::unique_lock submit_lock(mutex_);
const auto locked_ready = ensureConnected_("moveL");
if (!locked_ready.ok()) {
return locked_ready;
}
const auto rpc_client = sdk_->rpc_client;
const auto motion_state = sdk_->motion_state;
const auto motion = motion_state->begin(
aubo_internal::MotionKind::Linear);
if (!motion.started()) {
return Result::failure(
ArmErrorCode::RobotNotReady,
"[AuboArm] moveL rejected: " +
std::string(motionStartFailure(motion.status)) +
", id=" + id_);
}
busy_.store(true);
MotionOwnerGuard motion_owner{
*motion_state, busy_, motion.token};
const auto robot_names = rpc_client->getRobotNames();
if (robot_names.empty()) {
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
}
auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
auto robot_interface = rpc_client->getRobotInterface(robot_names.front());
if (!robot_interface) {
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
}
@ -754,17 +958,29 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options,
std::vector<double> tcp_offset(6, 0.0);
robot_interface->getRobotConfig()->setTcpOffset(tcp_offset);
std::vector<double> pose{target.x, target.y, target.z, target.rx, target.ry, target.rz};
motion_owner.requireExplicitSettlement();
const int ret = motion_control->moveLine(
pose,
options.acceleration > 0.0 ? options.acceleration : 0.5,
options.velocity > 0.0 ? options.velocity : 0.25,
options.blend_radius,
0);
if (ret == arcs::common_interface::AUBO_OK) {
submit_lock.unlock();
} else {
motion_owner.settle();
}
const auto outcome = aubo_internal::resolveMotionCommand(
ret,
arcs::common_interface::AUBO_OK,
arcs::common_interface::AUBO_REQUEST_IGNORE,
[&robot_interface]() { return waitArrival(robot_interface); });
[&robot_interface, motion_state, token = motion.token]() {
return waitArrival(
robot_interface,
[motion_state, token]() {
return motion_state->cancelled(token);
});
});
switch (outcome) {
case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion:
CMVR_LOG(DEBUG) << "[AuboArm] moveL completed without motion: sdk ret="
@ -772,7 +988,14 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options,
<< arcs::common_interface::returnValue2Str(ret) << ")";
return Result::success();
case aubo_internal::MotionCommandOutcome::CompletedAfterMotion:
motion_owner.clearOnFinish();
motion_owner.settle();
return Result::success();
case aubo_internal::MotionCommandOutcome::Cancelled:
motion_owner.settle();
return Result::failure(
ArmErrorCode::CommandRejected,
"[AuboArm] moveL stopped by stopMotion");
case aubo_internal::MotionCommandOutcome::SubmitFailed:
return Result::failure(
ArmErrorCode::CommandFailed,
@ -789,18 +1012,30 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options,
Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame)
{
const auto ready = ensureConnected_("speedL");
if (!ready.ok()) {
return ready;
}
if (busy_.exchange(true)) {
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_);
}
BusyGuard busy_guard{busy_};
try {
std::unique_lock submit_lock(mutex_);
const auto locked_ready = ensureConnected_("speedL");
if (!locked_ready.ok()) {
return locked_ready;
}
const auto rpc_client = sdk_->rpc_client;
const auto motion_state = sdk_->motion_state;
const auto motion = motion_state->begin(
aubo_internal::MotionKind::Linear, true);
if (!motion.started()) {
return Result::failure(
ArmErrorCode::RobotNotReady,
"[AuboArm] speedL rejected: " +
std::string(motionStartFailure(motion.status)) +
", id=" + id_);
}
busy_.store(true);
MotionOwnerGuard motion_owner{
*motion_state, busy_, motion.token};
Result interface_result;
auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "speedL", interface_result);
auto robot_interface = getPrimaryRobotInterface(
rpc_client, "speedL", interface_result);
if (!interface_result.ok()) {
return interface_result;
}
@ -820,8 +1055,10 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
tool_frame[0] = 0.0;
tool_frame[1] = 0.0;
tool_frame[2] = 0.0;
line_speed = sdk_->rpc_client->getMath()->poseTrans(tool_frame, line_speed);
angular_speed = sdk_->rpc_client->getMath()->poseTrans(tool_frame, angular_speed);
line_speed = rpc_client->getMath()->poseTrans(
tool_frame, line_speed);
angular_speed = rpc_client->getMath()->poseTrans(
tool_frame, angular_speed);
} else if (frame == FrameType::User) {
return Result::failure(ArmErrorCode::UnsupportedCommand,
"[AuboArm] speedL User frame requires a configured user coordinate frame");
@ -838,14 +1075,31 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
const double resolved_acceleration = acceleration > 0.0 ? acceleration : 1.2;
const double resolved_duration = duration > 0.0 ? duration : 100.0;
motion_owner.requireExplicitSettlement();
submit_lock.unlock();
if (motion_state->cancelled(motion.token)) {
motion_owner.settle();
return Result::failure(
ArmErrorCode::CommandRejected,
"[AuboArm] speedL stopped by stopMotion before submission");
}
const int ret = robot_interface->getMotionControl()->speedLine(
speed,
resolved_acceleration,
resolved_duration);
if (motion_state->cancelled(motion.token)) {
motion_owner.settle();
return Result::failure(
ArmErrorCode::CommandRejected,
"[AuboArm] speedL stopped by stopMotion");
}
if (ret != 0) {
motion_owner.settle();
return Result::failure(ArmErrorCode::CommandFailed,
"[AuboArm] speedL failed: ret=" + std::to_string(ret));
}
motion_owner.retainKind();
motion_owner.settle();
return Result::success();
} catch (const std::exception& e) {
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] speedL failed: ") + e.what());
@ -854,44 +1108,165 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
Result AuboArm::stopL(std::optional<double> acceleration)
{
if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
return Result::success();
}
try {
Result interface_result;
auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "stopL", interface_result);
if (!interface_result.ok()) {
return interface_result;
}
const double resolved_acceleration =
acceleration.has_value() && *acceleration > 0.0 ? *acceleration : 10.0;
const int ret = robot_interface->getMotionControl()->stopLine(resolved_acceleration, resolved_acceleration);
busy_.store(false);
if (ret != 0) {
return Result::failure(ArmErrorCode::CommandFailed,
"[AuboArm] stopL failed: ret=" + std::to_string(ret));
}
return Result::success();
} catch (const std::exception& e) {
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopL failed: ") + e.what());
}
return stopMotion_(
MotionStopKind::Linear,
acceleration.has_value() ? *acceleration : 0.0);
}
Result AuboArm::stopMotion()
{
return stopMotion_(MotionStopKind::Automatic, 0.0);
}
Result AuboArm::stopMotion_(
const MotionStopKind requested_kind,
const double acceleration)
{
if (!connected_.load()) {
return Result::success();
}
try {
std::unique_lock submit_lock(mutex_);
if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
return Result::success();
}
try {
const auto robot_names = sdk_->rpc_client->getRobotNames();
if (robot_names.empty()) {
const auto motion_state = sdk_->motion_state;
aubo_internal::MotionKind forced_kind =
aubo_internal::MotionKind::None;
if (requested_kind == MotionStopKind::Joint) {
forced_kind = aubo_internal::MotionKind::Joint;
} else if (requested_kind == MotionStopKind::Linear) {
forced_kind = aubo_internal::MotionKind::Linear;
}
const auto stop_request =
motion_state->beginStop(forced_kind);
if (!stop_request.started()) {
return Result::failure(
ArmErrorCode::RobotNotReady,
"[AuboArm] stopMotion rejected: another stop operation is in progress");
}
busy_.store(true);
StopStateGuard stop_state_guard{
*motion_state, busy_};
Result interface_result;
auto robot_interface = getPrimaryRobotInterface(
sdk_->rpc_client, "stopMotion", interface_result);
if (!interface_result.ok()) {
return interface_result;
}
auto motion_control = robot_interface->getMotionControl();
auto robot_state = robot_interface->getRobotState();
int last_exec_id = motion_control->getExecId();
bool last_steady = robot_state->isSteady();
const bool requires_vendor_stop =
stop_request.tracked_motion ||
last_exec_id != -1 ||
!last_steady;
if (requires_vendor_stop &&
stop_request.kind == aubo_internal::MotionKind::None) {
return Result::failure(
ArmErrorCode::CommandFailed,
"[AuboArm] stopMotion failed: controller is moving but the "
"active direct-motion type is unknown");
}
const auto issue_vendor_stop = [&]() -> Result {
int ret = 0;
if (stop_request.kind == aubo_internal::MotionKind::Joint) {
const double resolved_acceleration =
acceleration > 0.0 ? acceleration : 31.0;
ret = motion_control->stopJoint(resolved_acceleration);
} else {
const double resolved_acceleration =
acceleration > 0.0 ? acceleration : 10.0;
ret = motion_control->stopLine(
resolved_acceleration, resolved_acceleration);
}
if (ret != arcs::common_interface::AUBO_OK) {
return Result::failure(
ArmErrorCode::CommandFailed,
"[AuboArm] stopMotion failed: " +
std::string(motionKindName(stop_request.kind)) +
" stop sdk ret=" + std::to_string(ret) +
" (" +
arcs::common_interface::returnValue2Str(ret) +
")");
}
return Result::success();
};
if (requires_vendor_stop) {
const auto stop_result = issue_vendor_stop();
if (!stop_result.ok()) {
return stop_result;
}
auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
if (robot_interface) {
robot_interface->getMotionControl()->stopMove(true, true);
}
busy_.store(false);
constexpr auto kStopTimeout = std::chrono::seconds(5);
constexpr auto kPollInterval = std::chrono::milliseconds(50);
constexpr int kStableSamples = 3;
const auto deadline =
std::chrono::steady_clock::now() + kStopTimeout;
int stable_samples = 0;
bool idle_since_stop = last_exec_id == -1 && last_steady;
bool owner_active =
motion_state->ownerActive(stop_request.active_token);
while (std::chrono::steady_clock::now() < deadline) {
last_exec_id = motion_control->getExecId();
last_steady = robot_state->isSteady();
owner_active =
motion_state->ownerActive(stop_request.active_token);
const bool physically_idle =
last_exec_id == -1 && last_steady;
if (!physically_idle && idle_since_stop) {
if (stop_request.kind ==
aubo_internal::MotionKind::None) {
return Result::failure(
ArmErrorCode::CommandFailed,
"[AuboArm] stopMotion failed: motion started "
"after an idle observation but its type is unknown");
}
const auto stop_result = issue_vendor_stop();
if (!stop_result.ok()) {
return stop_result;
}
idle_since_stop = false;
}
if (physically_idle && !owner_active) {
if (++stable_samples >= kStableSamples) {
break;
}
} else {
stable_samples = 0;
}
if (physically_idle) {
idle_since_stop = true;
}
std::this_thread::sleep_for(kPollInterval);
}
if (stable_samples < kStableSamples) {
return Result::failure(
ArmErrorCode::Timeout,
"[AuboArm] stopMotion failed: timeout waiting for " +
std::string(motionKindName(stop_request.kind)) +
" motion to stop, generation=" +
std::to_string(stop_request.active_token.generation) +
", exec_id=" + std::to_string(last_exec_id) +
", steady=" + (last_steady ? "true" : "false") +
", owner_active=" +
(owner_active ? "true" : "false"));
}
if (!stop_state_guard.complete()) {
return Result::failure(
ArmErrorCode::CommandFailed,
"[AuboArm] stopMotion failed: cancelled motion handler is still active");
}
return Result::success();
} catch (const std::exception& e) {
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopMotion failed: ") + e.what());
@ -1226,7 +1601,6 @@ Result AuboArm::stopProgram()
}
try {
const int ret = sdk_->rpc_client->getRuntimeMachine()->abort();
busy_.store(false);
if (ret != 0) {
return Result::failure(ArmErrorCode::CommandFailed,
"[AuboArm] stopProgram failed: ret=" + std::to_string(ret));

9
cmvr-es/devices/arm/aubo_arm/aubo_arm.h
View File

@ -80,12 +80,19 @@ public:
CartesianPose fk(const std::string& base_link, const std::string& ee_link) override;
CartesianPose fk(bool is_tcp = true) override;
CartesianVelocity getSpeedLCommandTwistBase() const override { return {}; }
bool busy() const override { return busy_.load(); }
bool busy() const override;
private:
enum class MotionStopKind {
Automatic,
Joint,
Linear,
};
Result unsupported_(const std::string& name) const;
bool validDof_(std::size_t size, std::string& error) const;
Result ensureConnected_(const std::string& context) const;
Result stopMotion_(MotionStopKind kind, double acceleration);
struct SdkState;

13
cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h
View File

@ -6,10 +6,17 @@ namespace cmvr::device::aubo_internal {
enum class MotionCommandOutcome {
CompletedWithoutMotion,
CompletedAfterMotion,
Cancelled,
SubmitFailed,
CompletionFailed,
};
enum class MotionWaitResult {
Completed,
Cancelled,
Failed,
};
template <typename WaitForCompletion>
MotionCommandOutcome resolveMotionCommand(
const int return_code,
@ -23,7 +30,11 @@ MotionCommandOutcome resolveMotionCommand(
if (return_code != success_code) {
return MotionCommandOutcome::SubmitFailed;
}
if (wait_for_completion() != 0) {
const auto wait_result = wait_for_completion();
if (wait_result == MotionWaitResult::Cancelled) {
return MotionCommandOutcome::Cancelled;
}
if (wait_result != MotionWaitResult::Completed) {
return MotionCommandOutcome::CompletionFailed;
}
return MotionCommandOutcome::CompletedAfterMotion;

265
cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h Normal file
View File

@ -0,0 +1,265 @@
#ifndef CMVR_ES_AUBO_MOTION_STATE_H
#define CMVR_ES_AUBO_MOTION_STATE_H
#include <algorithm>
#include <chrono>
#include <condition_variable>
#include <cstdint>
#include <mutex>
namespace cmvr::device::aubo_internal {
enum class MotionKind {
None,
Joint,
Linear,
};
struct MotionToken {
std::uint64_t generation{0};
MotionKind kind{MotionKind::None};
bool valid() const noexcept
{
return generation != 0 && kind != MotionKind::None;
}
};
enum class MotionStartStatus {
Started,
Invalid,
Busy,
Stopping,
Blocked,
};
struct MotionStartResult {
MotionStartStatus status{MotionStartStatus::Busy};
MotionToken token;
bool started() const noexcept
{
return status == MotionStartStatus::Started;
}
};
enum class MotionFinishMode {
RestorePrevious,
Clear,
Retain,
};
enum class StopStartStatus {
Started,
AlreadyStopping,
};
struct StopRequest {
StopStartStatus status{StopStartStatus::AlreadyStopping};
MotionKind kind{MotionKind::None};
MotionToken active_token;
bool tracked_motion{false};
bool started() const noexcept
{
return status == StopStartStatus::Started;
}
};
// Tracks one direct AUBO motion owner. MoveJ/MoveL submissions are serialized
// through the vendor call. Speed calls release the outer mutex before their
// potentially blocking SDK call, so the generation cancellation below also
// closes the stop-vs-speed-submission race.
class MotionState final {
public:
MotionStartResult begin(
const MotionKind kind,
const bool replace_retained_same_kind = false)
{
std::lock_guard lock(mutex_);
if (kind == MotionKind::None) {
return {MotionStartStatus::Invalid, {}};
}
if (stop_in_progress_) {
return {MotionStartStatus::Stopping, {}};
}
if (blocked_) {
return {MotionStartStatus::Blocked, {}};
}
if (owner_active_) {
return {MotionStartStatus::Busy, {}};
}
if (last_kind_ != MotionKind::None &&
(!replace_retained_same_kind || last_kind_ != kind)) {
return {MotionStartStatus::Busy, {}};
}
MotionToken token{++next_generation_, kind};
owner_active_ = true;
active_token_ = token;
previous_kind_ = last_kind_;
return {MotionStartStatus::Started, token};
}
void finish(
const MotionToken& token,
const MotionFinishMode mode = MotionFinishMode::RestorePrevious)
{
std::lock_guard lock(mutex_);
if (!owner_active_ ||
active_token_.generation != token.generation) {
return;
}
owner_active_ = false;
active_token_ = {};
if (!stop_in_progress_ && !blocked_) {
if (mode == MotionFinishMode::Retain) {
last_kind_ = token.kind;
} else if (mode == MotionFinishMode::Clear) {
last_kind_ = MotionKind::None;
} else {
last_kind_ = previous_kind_;
}
}
previous_kind_ = MotionKind::None;
owner_finished_cv_.notify_all();
}
void failMotion(const MotionToken& token)
{
std::lock_guard lock(mutex_);
if (!owner_active_ ||
active_token_.generation != token.generation) {
return;
}
owner_active_ = false;
active_token_ = {};
last_kind_ = token.kind;
previous_kind_ = MotionKind::None;
blocked_ = true;
owner_finished_cv_.notify_all();
}
StopRequest beginStop(
const MotionKind requested_kind = MotionKind::None)
{
std::lock_guard lock(mutex_);
if (stop_in_progress_) {
return {};
}
stop_in_progress_ = true;
const MotionToken active = owner_active_
? active_token_
: MotionToken{};
// A successful speedJoint/speedLine call may keep the controller in
// velocity mode after the SDK function returns, even when the target
// velocity is zero and the robot currently reports steady. Retain that
// motion kind until a typed stop has been acknowledged.
const bool tracked_motion =
active.valid() || last_kind_ != MotionKind::None;
if (active.valid()) {
cancelled_generation_ = std::max(
cancelled_generation_, active.generation);
}
MotionKind kind = MotionKind::None;
if (active.valid()) {
kind = active.kind;
} else if (last_kind_ != MotionKind::None) {
kind = last_kind_;
} else if (requested_kind != MotionKind::None) {
kind = requested_kind;
} else {
kind = last_kind_;
}
if (kind != MotionKind::None) {
last_kind_ = kind;
}
return {
StopStartStatus::Started,
kind,
active,
tracked_motion};
}
bool cancelled(const MotionToken& token) const
{
std::lock_guard lock(mutex_);
return token.valid() &&
token.generation <= cancelled_generation_;
}
bool waitForOwnerExit(
const MotionToken& token,
const std::chrono::milliseconds timeout)
{
if (!token.valid()) {
return true;
}
std::unique_lock lock(mutex_);
return owner_finished_cv_.wait_for(
lock,
timeout,
[this, &token]() {
return !owner_active_ ||
active_token_.generation != token.generation;
});
}
bool ownerActive(const MotionToken& token) const
{
if (!token.valid()) {
return false;
}
std::lock_guard lock(mutex_);
return owner_active_ &&
active_token_.generation == token.generation;
}
bool completeStop()
{
std::lock_guard lock(mutex_);
if (owner_active_) {
return false;
}
stop_in_progress_ = false;
blocked_ = false;
active_token_ = {};
last_kind_ = MotionKind::None;
previous_kind_ = MotionKind::None;
owner_finished_cv_.notify_all();
return true;
}
void failStop()
{
std::lock_guard lock(mutex_);
stop_in_progress_ = false;
blocked_ = true;
owner_finished_cv_.notify_all();
}
bool busy() const
{
std::lock_guard lock(mutex_);
return owner_active_ || stop_in_progress_ || blocked_ ||
last_kind_ != MotionKind::None;
}
private:
mutable std::mutex mutex_;
std::condition_variable owner_finished_cv_;
std::uint64_t next_generation_{0};
std::uint64_t cancelled_generation_{0};
MotionToken active_token_;
MotionKind last_kind_{MotionKind::None};
MotionKind previous_kind_{MotionKind::None};
bool owner_active_{false};
bool stop_in_progress_{false};
bool blocked_{false};
};
} // namespace cmvr::device::aubo_internal
#endif // CMVR_ES_AUBO_MOTION_STATE_H

17
cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp
View File

@ -18,6 +18,7 @@ namespace {
int main()
{
using cmvr::device::aubo_internal::MotionCommandOutcome;
using cmvr::device::aubo_internal::MotionWaitResult;
using cmvr::device::aubo_internal::resolveMotionCommand;
constexpr int success_code = 0;
@ -26,7 +27,7 @@ int main()
int wait_calls = 0;
const auto wait_succeeded = [&wait_calls]() {
++wait_calls;
return 0;
return MotionWaitResult::Completed;
};
CHECK_TRUE(resolveMotionCommand(
success_code,
@ -57,7 +58,7 @@ int main()
wait_calls = 0;
const auto wait_failed = [&wait_calls]() {
++wait_calls;
return -1;
return MotionWaitResult::Failed;
};
CHECK_TRUE(resolveMotionCommand(
success_code,
@ -66,5 +67,17 @@ int main()
wait_failed) == MotionCommandOutcome::CompletionFailed);
CHECK_TRUE(wait_calls == 1);
wait_calls = 0;
const auto wait_cancelled = [&wait_calls]() {
++wait_calls;
return MotionWaitResult::Cancelled;
};
CHECK_TRUE(resolveMotionCommand(
success_code,
success_code,
request_ignore_code,
wait_cancelled) == MotionCommandOutcome::Cancelled);
CHECK_TRUE(wait_calls == 1);
return 0;
}

127
cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp Normal file
View File

@ -0,0 +1,127 @@
#include "devices/arm/aubo_arm/aubo_motion_state.h"
#include <chrono>
#include <iostream>
namespace {
#define CHECK_TRUE(condition) \
do { \
if (!(condition)) { \
std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \
<< #condition << std::endl; \
return 1; \
} \
} while (false)
} // namespace
int main()
{
using namespace cmvr::device::aubo_internal;
MotionState state;
CHECK_TRUE(state.begin(MotionKind::None).status ==
MotionStartStatus::Invalid);
const auto joint = state.begin(MotionKind::Joint);
CHECK_TRUE(joint.started());
CHECK_TRUE(state.busy());
CHECK_TRUE(state.begin(MotionKind::Linear).status ==
MotionStartStatus::Busy);
const auto stop_joint = state.beginStop();
CHECK_TRUE(stop_joint.started());
CHECK_TRUE(stop_joint.kind == MotionKind::Joint);
CHECK_TRUE(stop_joint.active_token.generation ==
joint.token.generation);
CHECK_TRUE(stop_joint.tracked_motion);
CHECK_TRUE(state.cancelled(joint.token));
CHECK_TRUE(state.beginStop().status ==
StopStartStatus::AlreadyStopping);
CHECK_TRUE(state.begin(MotionKind::Linear).status ==
MotionStartStatus::Stopping);
CHECK_TRUE(!state.waitForOwnerExit(
joint.token, std::chrono::milliseconds(1)));
CHECK_TRUE(!state.completeStop());
state.finish(joint.token);
CHECK_TRUE(state.waitForOwnerExit(
joint.token, std::chrono::milliseconds(1)));
CHECK_TRUE(state.completeStop());
CHECK_TRUE(!state.busy());
const auto linear = state.begin(MotionKind::Linear);
CHECK_TRUE(linear.started());
CHECK_TRUE(!state.cancelled(linear.token));
// A delayed guard from the cancelled command must not release a newer one.
state.finish(joint.token);
CHECK_TRUE(state.busy());
state.finish(linear.token, MotionFinishMode::Clear);
CHECK_TRUE(!state.busy());
const auto speed_joint = state.begin(MotionKind::Joint);
CHECK_TRUE(speed_joint.started());
state.finish(speed_joint.token, MotionFinishMode::Retain);
CHECK_TRUE(state.busy());
CHECK_TRUE(state.begin(MotionKind::Linear).status ==
MotionStartStatus::Busy);
const auto rejected_speed_update =
state.begin(MotionKind::Joint, true);
CHECK_TRUE(rejected_speed_update.started());
state.finish(
rejected_speed_update.token,
MotionFinishMode::RestorePrevious);
const auto stop_speed = state.beginStop();
CHECK_TRUE(stop_speed.started());
CHECK_TRUE(stop_speed.kind == MotionKind::Joint);
CHECK_TRUE(stop_speed.tracked_motion);
CHECK_TRUE(state.completeStop());
CHECK_TRUE(!state.busy());
const auto idle_stop = state.beginStop();
CHECK_TRUE(idle_stop.kind == MotionKind::None);
CHECK_TRUE(!idle_stop.tracked_motion);
state.failStop();
const auto retry_idle_stop = state.beginStop();
CHECK_TRUE(retry_idle_stop.kind == MotionKind::None);
CHECK_TRUE(!retry_idle_stop.tracked_motion);
CHECK_TRUE(state.completeStop());
const auto mismatched_stop_motion = state.begin(MotionKind::Linear);
CHECK_TRUE(mismatched_stop_motion.started());
const auto mismatched_stop = state.beginStop(MotionKind::Joint);
CHECK_TRUE(mismatched_stop.kind == MotionKind::Linear);
state.finish(mismatched_stop_motion.token);
CHECK_TRUE(state.completeStop());
const auto uncertain_motion = state.begin(MotionKind::Joint);
CHECK_TRUE(uncertain_motion.started());
state.failMotion(uncertain_motion.token);
CHECK_TRUE(state.busy());
CHECK_TRUE(state.begin(MotionKind::Linear).status ==
MotionStartStatus::Blocked);
const auto stop_uncertain = state.beginStop();
CHECK_TRUE(stop_uncertain.kind == MotionKind::Joint);
CHECK_TRUE(stop_uncertain.tracked_motion);
CHECK_TRUE(state.completeStop());
const auto failed_stop_motion = state.begin(MotionKind::Linear);
CHECK_TRUE(failed_stop_motion.started());
const auto failed_stop = state.beginStop();
CHECK_TRUE(failed_stop.kind == MotionKind::Linear);
state.failStop();
CHECK_TRUE(state.busy());
CHECK_TRUE(state.begin(MotionKind::Joint).status ==
MotionStartStatus::Blocked);
state.finish(failed_stop_motion.token);
const auto retry = state.beginStop();
CHECK_TRUE(retry.started());
CHECK_TRUE(retry.kind == MotionKind::Linear);
CHECK_TRUE(state.completeStop());
const auto recovered = state.begin(MotionKind::Joint);
CHECK_TRUE(recovered.started());
state.finish(recovered.token, MotionFinishMode::Clear);
return 0;
}

10
cmvr-es/manager/control_authority/include/control_authority_manager.h
View File

@ -41,6 +41,14 @@ public:
const std::string& resource_id,
const std::string& owner_id,
Duration ttl);
// Atomically invalidates a normal control lease and joins a safety
// barrier. Each safety caller receives an independent token; normal
// control remains blocked until the last safety token is released.
ControlAcquireResult preemptAcquire(
const std::string& resource_id,
const std::string& owner_id,
Duration ttl);
bool renew(const ControlLeaseToken& token, Duration ttl);
bool validate(const ControlLeaseToken& token);
void release(const ControlLeaseToken& token) noexcept;
@ -60,6 +68,8 @@ private:
std::string owner_id;
std::uint64_t generation{0};
std::chrono::steady_clock::time_point deadline;
bool preemptible{true};
std::unordered_map<std::uint64_t, std::string> safety_holders;
};
bool expired_(const Entry& entry) const noexcept;

83
cmvr-es/manager/control_authority/src/control_authority_manager.cpp
View File

@ -42,7 +42,50 @@ ControlAcquireResult ControlAuthorityManager::tryAcquire(
Entry{
owner_id,
token.generation,
std::chrono::steady_clock::now() + ttl});
std::chrono::steady_clock::now() + ttl,
true,
{}});
return {true, std::move(token), {}};
}
ControlAcquireResult ControlAuthorityManager::preemptAcquire(
const std::string& resource_id,
const std::string& owner_id,
const Duration ttl)
{
if (resource_id.empty() || owner_id.empty() ||
ttl <= Duration::zero()) {
return {false, {}, "invalid control barrier request"};
}
std::lock_guard lock(mutex_);
const auto existing = entries_.find(resource_id);
if (existing != entries_.end()) {
if (!expired_(existing->second) &&
!existing->second.preemptible) {
ControlLeaseToken token;
token.resource_id = resource_id;
token.owner_id = owner_id;
token.generation = ++next_generation_;
existing->second.safety_holders.emplace(
token.generation, token.owner_id);
return {true, std::move(token), {}};
}
entries_.erase(existing);
}
ControlLeaseToken token;
token.resource_id = resource_id;
token.owner_id = owner_id;
token.generation = ++next_generation_;
entries_.emplace(
resource_id,
Entry{
owner_id,
token.generation,
std::chrono::steady_clock::time_point::max(),
false,
{{token.generation, owner_id}}});
return {true, std::move(token), {}};
}
@ -55,15 +98,22 @@ bool ControlAuthorityManager::renew(
}
std::lock_guard lock(mutex_);
const auto found = entries_.find(token.resource_id);
if (found == entries_.end() ||
expired_(found->second) ||
found->second.owner_id != token.owner_id ||
found->second.generation != token.generation) {
if (found == entries_.end() || expired_(found->second)) {
if (found != entries_.end() && expired_(found->second)) {
entries_.erase(found);
}
return false;
}
if (!found->second.preemptible) {
const auto holder =
found->second.safety_holders.find(token.generation);
return holder != found->second.safety_holders.end() &&
holder->second == token.owner_id;
}
if (found->second.owner_id != token.owner_id ||
found->second.generation != token.generation) {
return false;
}
found->second.deadline =
std::chrono::steady_clock::now() + ttl;
return true;
@ -84,6 +134,12 @@ bool ControlAuthorityManager::validate(
entries_.erase(found);
return false;
}
if (!found->second.preemptible) {
const auto holder =
found->second.safety_holders.find(token.generation);
return holder != found->second.safety_holders.end() &&
holder->second == token.owner_id;
}
return found->second.owner_id == token.owner_id &&
found->second.generation == token.generation;
}
@ -97,8 +153,21 @@ void ControlAuthorityManager::release(
try {
std::lock_guard lock(mutex_);
const auto found = entries_.find(token.resource_id);
if (found != entries_.end() &&
found->second.owner_id == token.owner_id &&
if (found == entries_.end()) {
return;
}
if (!found->second.preemptible) {
const auto holder =
found->second.safety_holders.find(token.generation);
if (holder == found->second.safety_holders.end() ||
holder->second != token.owner_id) {
return;
}
found->second.safety_holders.erase(holder);
if (found->second.safety_holders.empty()) {
entries_.erase(found);
}
} else if (found->second.owner_id == token.owner_id &&
found->second.generation == token.generation) {
entries_.erase(found);
}

32
cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp
View File

@ -61,6 +61,38 @@ TEST_F(ControlAuthorityManagerTest, StaleGenerationCannotReleaseNewLease)
EXPECT_TRUE(manager.validate(current.token));
}
TEST_F(ControlAuthorityManagerTest,
SafetyBarrierAtomicallyPreemptsControlAndRejectsOtherOwners)
{
auto& manager = ControlAuthorityManager::instance();
const auto control =
manager.tryAcquire("right_arm", "move-session", 100ms);
ASSERT_TRUE(control.acquired);
const auto barrier = manager.preemptAcquire(
"right_arm", "stop-operation", 100ms);
ASSERT_TRUE(barrier.acquired) << barrier.detail;
EXPECT_FALSE(manager.validate(control.token));
EXPECT_TRUE(manager.validate(barrier.token));
const auto move_during_stop =
manager.tryAcquire("right_arm", "new-move", 100ms);
EXPECT_FALSE(move_during_stop.acquired);
const auto second_stop = manager.preemptAcquire(
"right_arm", "second-stop", 100ms);
ASSERT_TRUE(second_stop.acquired) << second_stop.detail;
manager.release(control.token);
EXPECT_TRUE(manager.validate(barrier.token));
manager.release(barrier.token);
EXPECT_TRUE(manager.validate(second_stop.token));
EXPECT_FALSE(
manager.tryAcquire("right_arm", "new-move", 100ms)
.acquired);
manager.release(second_stop.token);
EXPECT_FALSE(manager.isLeased("right_arm"));
}
TEST_F(ControlAuthorityManagerTest, ExpiryAndRenewUseMonotonicLocalTime)
{
auto& manager = ControlAuthorityManager::instance();

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

@ -126,11 +126,16 @@ grpc::Status setDeviceNotFound(Response* response, const std::string& device_id)
grpc::Status setControlLeaseConflict(
api::CommandHeader_Feedback* response,
const std::string& device_id)
const std::string& device_id,
const std::string& detail)
{
const std::string message =
std::string message =
"RobotArm control is leased by another active control operation: " +
device_id;
if (!detail.empty()) {
CMVR_LOG(WARNING) << "[gRPCArmServiceImpl] control lease conflict, id="
<< device_id << ", detail=" << detail;
}
fillFeedback(response, false, message);
return grpc::Status(
grpc::StatusCode::FAILED_PRECONDITION, message);
@ -139,17 +144,19 @@ grpc::Status setControlLeaseConflict(
template <typename Response>
grpc::Status setControlLeaseConflict(
Response* response,
const std::string& device_id)
const std::string& device_id,
const std::string& detail)
{
return setControlLeaseConflict(
response->mutable_header(), device_id);
response->mutable_header(), device_id, detail);
}
class ScopedUnaryControlLease final {
public:
ScopedUnaryControlLease(
const std::string& device_id,
const char* operation)
const char* operation,
const bool preemptive = false)
: manager_(control::ControlAuthorityManager::instance())
{
static std::atomic<std::uint64_t> sequence{0};
@ -159,14 +166,15 @@ public:
sequence.fetch_add(
1U, std::memory_order_relaxed) +
1U);
auto acquired = manager_.tryAcquire(
device_id,
owner,
std::chrono::duration_cast<
const auto ttl = std::chrono::duration_cast<
control::ControlAuthorityManager::Duration>(
std::chrono::hours(24)));
std::chrono::hours(24));
auto acquired = preemptive
? manager_.preemptAcquire(device_id, owner, ttl)
: manager_.tryAcquire(device_id, owner, ttl);
acquired_ = acquired.acquired;
token_ = std::move(acquired.token);
detail_ = std::move(acquired.detail);
}
~ScopedUnaryControlLease()
@ -175,10 +183,12 @@ public:
}
bool acquired() const noexcept { return acquired_; }
const std::string& detail() const noexcept { return detail_; }
private:
control::ControlAuthorityManager& manager_;
control::ControlLeaseToken token_;
std::string detail_;
bool acquired_{false};
};
@ -199,10 +209,12 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*,
if (!arm) {
return setDeviceNotFound(response, device_id);
}
// A safety-disable command preempts any network teleoperation lease.
// The teleoperation executor must fail its next renew before it can
// dispatch another setpoint.
control::ControlAuthorityManager::instance().revoke(device_id);
ScopedUnaryControlLease control_barrier(
device_id, "torqueOff", true);
if (!control_barrier.acquired()) {
return setControlLeaseConflict(
response, device_id, control_barrier.detail());
}
const auto result = arm->torqueOff();
fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
if (result.ok()) {
@ -228,7 +240,8 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*,
ScopedUnaryControlLease control_lease(
device_id, "torqueOn");
if (!control_lease.acquired()) {
return setControlLeaseConflict(response, device_id);
return setControlLeaseConflict(
response, device_id, control_lease.detail());
}
const auto result = arm->torqueOn();
fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
@ -255,7 +268,8 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*,
ScopedUnaryControlLease control_lease(
device_id, "moveJ");
if (!control_lease.acquired()) {
return setControlLeaseConflict(response, device_id);
return setControlLeaseConflict(
response, device_id, control_lease.detail());
}
const auto result = arm->moveJ(toJointPositionCommand(request->target()),
toMotionOptions(request->options()));
@ -283,7 +297,8 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*,
ScopedUnaryControlLease control_lease(
device_id, "moveL");
if (!control_lease.acquired()) {
return setControlLeaseConflict(response, device_id);
return setControlLeaseConflict(
response, device_id, control_lease.detail());
}
const auto result = arm->moveL(toCartesianPose(request->target()),
toMotionOptions(request->options()),
@ -312,7 +327,8 @@ grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*,
ScopedUnaryControlLease control_lease(
device_id, "speedJ");
if (!control_lease.acquired()) {
return setControlLeaseConflict(response, device_id);
return setControlLeaseConflict(
response, device_id, control_lease.detail());
}
const auto result = arm->speedJ(toJointVelocityCommand(request->velocity()),
request->acceleration(),
@ -343,7 +359,8 @@ grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*,
ScopedUnaryControlLease control_lease(
device_id, "speedL");
if (!control_lease.acquired()) {
return setControlLeaseConflict(response, device_id);
return setControlLeaseConflict(
response, device_id, control_lease.detail());
}
const auto result = arm->speedL(toCartesianVelocity(request->velocity()),
request->acceleration(),
@ -375,7 +392,8 @@ grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext*,
ScopedUnaryControlLease control_lease(
device_id, "servoJ");
if (!control_lease.acquired()) {
return setControlLeaseConflict(response, device_id);
return setControlLeaseConflict(
response, device_id, control_lease.detail());
}
const auto result = arm->servoJ(toJointPositionCommand(request->target()));
if (result.ok()) {
@ -399,7 +417,12 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*,
if (!arm) {
return setDeviceNotFound(response, device_id);
}
control::ControlAuthorityManager::instance().revoke(device_id);
ScopedUnaryControlLease control_barrier(
device_id, "stopMotion", true);
if (!control_barrier.acquired()) {
return setControlLeaseConflict(
response, device_id, control_barrier.detail());
}
const auto result = arm->stopMotion();
fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
if (result.ok()) {
@ -480,7 +503,8 @@ grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*,
ScopedUnaryControlLease control_lease(
device_id, "calibrateZeroQ");
if (!control_lease.acquired()) {
return setControlLeaseConflict(response, device_id);
return setControlLeaseConflict(
response, device_id, control_lease.detail());
}
const auto result = arm->calibrateZeroQ(request->joint_name());
if (result.ok()) {
@ -557,7 +581,8 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context,
ScopedUnaryControlLease control_lease(
device_id, "clearFault");
if (!control_lease.acquired()) {
return setControlLeaseConflict(response, device_id);
return setControlLeaseConflict(
response, device_id, control_lease.detail());
}
const auto result = arm->clearFault();
fillFeedback(response, result.ok(), result.ok() ? "" : result.message);

65
cmvr-es/service/grpc/src/grpc_system_service.cpp
View File

@ -4,16 +4,65 @@
#include "../include/grpc_system_service.h"
#include <atomic>
#include <chrono>
#include <cstdint>
#include <stdexcept>
#include <string>
#include <vector>
#include "common/base/logging/logger.h"
#include "manager/control_authority/include/control_authority_manager.h"
using namespace cmvr::device;
using namespace cmvr::service;
namespace {
class ScopedControlBarrierSet final {
public:
~ScopedControlBarrierSet()
{
auto& authority =
cmvr::control::ControlAuthorityManager::instance();
for (const auto& token : tokens_) {
authority.release(token);
}
}
bool acquire(const std::string& device_id, std::string& detail)
{
static std::atomic<std::uint64_t> sequence{0};
const std::string owner =
"grpc-system:stop-all:" +
std::to_string(
sequence.fetch_add(1U, std::memory_order_relaxed) + 1U);
const auto result =
cmvr::control::ControlAuthorityManager::instance()
.preemptAcquire(
device_id,
owner,
std::chrono::duration_cast<
cmvr::control::ControlAuthorityManager::Duration>(
std::chrono::hours(24)));
if (!result.acquired) {
detail = result.detail;
return false;
}
try {
tokens_.push_back(result.token);
} catch (...) {
cmvr::control::ControlAuthorityManager::instance().release(
result.token);
throw;
}
return true;
}
private:
std::vector<cmvr::control::ControlLeaseToken> tokens_;
};
std::uint64_t unixTimeMs() noexcept
{
const auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
@ -239,6 +288,22 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context,
const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response)
{
try {
const auto snapshot = dmgr_.snapshot();
ScopedControlBarrierSet control_barriers;
for (const auto& device : snapshot.devices) {
if (device.kind == cmvr::device::DeviceKind::Arm) {
std::string detail;
if (!control_barriers.acquire(device.id, detail)) {
CMVR_LOG(WARNING)
<< "[gRPCSystemServiceImpl] (StopAll): failed to "
"acquire arm safety barrier, id="
<< device.id << ", detail=" << detail;
throw std::runtime_error(
"StopAll could not acquire the RobotArm safety "
"barrier: " + device.id);
}
}
}
dmgr_.stop();
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());

320
cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp
View File

@ -1,6 +1,10 @@
#include "service/grpc/include/grpc_arm_service.h"
#include <chrono>
#include <condition_variable>
#include <future>
#include <memory>
#include <mutex>
#include <optional>
#include <string>
#include <utility>
@ -11,6 +15,7 @@
#include <gtest/gtest.h>
#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
#include "manager/control_authority/include/control_authority_manager.h"
#include "manager/device_manager/include/device_manager.h"
namespace cmvr::service {
@ -57,7 +62,12 @@ public:
}
device::Result torqueOn() override { return device::Result::success(); }
device::Result torqueOff() override { return device::Result::success(); }
device::Result torqueOff() override
{
std::lock_guard lock(motion_mutex_);
++torque_off_calls_;
return device::Result::success();
}
device::Result calibrateZeroQ(const std::string&) override
{
return device::Result::success();
@ -82,7 +92,7 @@ public:
device::Result moveJ(const device::JointPositionCommand&,
const device::MotionOptions&) override
{
return device::Result::success();
return enterMotion("moveJ", move_j_calls_);
}
device::Result speedJ(const device::JointVelocityCommand&,
double,
@ -99,7 +109,7 @@ public:
const device::MotionOptions&,
device::FrameType = device::FrameType::Base) override
{
return device::Result::success();
return enterMotion("moveL", move_l_calls_);
}
device::Result speedL(
const device::CartesianVelocity&,
@ -115,9 +125,101 @@ public:
}
device::Result stopMotion() override
{
std::unique_lock lock(motion_mutex_);
++stop_motion_calls_;
if (block_next_stop_) {
block_next_stop_ = false;
blocking_stop_started_ = true;
stop_started_cv_.notify_all();
stop_release_cv_.wait(
lock,
[this]() { return release_blocking_stop_; });
}
return device::Result::success();
}
void blockNextMotion()
{
std::lock_guard lock(motion_mutex_);
block_next_motion_ = true;
blocking_motion_started_ = false;
release_blocking_motion_ = false;
blocking_motion_name_.clear();
}
bool waitForBlockingMotion(
const std::string& operation,
const std::chrono::milliseconds timeout)
{
std::unique_lock lock(motion_mutex_);
return motion_started_cv_.wait_for(
lock,
timeout,
[this, &operation]() {
return blocking_motion_started_ &&
blocking_motion_name_ == operation;
});
}
void releaseBlockingMotion()
{
{
std::lock_guard lock(motion_mutex_);
release_blocking_motion_ = true;
}
motion_release_cv_.notify_all();
}
void blockNextStopMotion()
{
std::lock_guard lock(motion_mutex_);
block_next_stop_ = true;
blocking_stop_started_ = false;
release_blocking_stop_ = false;
}
bool waitForBlockingStop(const std::chrono::milliseconds timeout)
{
std::unique_lock lock(motion_mutex_);
return stop_started_cv_.wait_for(
lock,
timeout,
[this]() { return blocking_stop_started_; });
}
void releaseBlockingStop()
{
{
std::lock_guard lock(motion_mutex_);
release_blocking_stop_ = true;
}
stop_release_cv_.notify_all();
}
int moveJCalls() const
{
std::lock_guard lock(motion_mutex_);
return move_j_calls_;
}
int moveLCalls() const
{
std::lock_guard lock(motion_mutex_);
return move_l_calls_;
}
int stopMotionCalls() const
{
std::lock_guard lock(motion_mutex_);
return stop_motion_calls_;
}
int torqueOffCalls() const
{
std::lock_guard lock(motion_mutex_);
return torque_off_calls_;
}
device::Result startServoMode(const device::ServoOptions&) override
{
return device::Result::success();
@ -217,6 +319,42 @@ public:
bool next_success{true};
std::string next_response_json;
std::string last_request_json;
private:
device::Result enterMotion(const char* operation, int& call_count)
{
std::unique_lock lock(motion_mutex_);
++call_count;
if (!block_next_motion_) {
return device::Result::success();
}
block_next_motion_ = false;
blocking_motion_started_ = true;
blocking_motion_name_ = operation;
motion_started_cv_.notify_all();
motion_release_cv_.wait(
lock,
[this]() { return release_blocking_motion_; });
return device::Result::success();
}
mutable std::mutex motion_mutex_;
std::condition_variable motion_started_cv_;
std::condition_variable motion_release_cv_;
std::condition_variable stop_started_cv_;
std::condition_variable stop_release_cv_;
bool block_next_motion_{false};
bool blocking_motion_started_{false};
bool release_blocking_motion_{false};
bool block_next_stop_{false};
bool blocking_stop_started_{false};
bool release_blocking_stop_{false};
std::string blocking_motion_name_;
int move_j_calls_{0};
int move_l_calls_{0};
int stop_motion_calls_{0};
int torque_off_calls_{0};
};
class JsonCommandNonArmDevice final : public device::AbstractDevice {
@ -243,6 +381,7 @@ class GrpcArmServiceTest : public ::testing::Test {
protected:
void SetUp() override
{
control::ControlAuthorityManager::instance().clear();
device::DeviceManager::destroyInstance();
config::DeviceManagerConfig config;
auto& manager = device::DeviceManager::getInstance(config);
@ -263,6 +402,7 @@ protected:
aubo_arm_.reset();
left_arm_.reset();
device::DeviceManager::destroyInstance();
control::ControlAuthorityManager::instance().clear();
}
grpc::Status execute(const std::string& device_id,
@ -276,6 +416,60 @@ protected:
return service_->ExecuteJsonCommand(&context, &request, &response);
}
struct MoveOutcome {
grpc::Status status;
bool response_success{false};
std::string response_error;
};
MoveOutcome moveJ(const std::string& device_id)
{
api::MoveJ_Request request;
request.mutable_header()->set_device_id(device_id);
request.mutable_target()->add_position(0.1);
api::MoveJ_Response response;
grpc::ServerContext context;
auto status = service_->moveJ(&context, &request, &response);
return {
std::move(status),
response.header().success(),
response.header().error_message()};
}
MoveOutcome moveL(const std::string& device_id)
{
api::MoveL_Request request;
request.mutable_header()->set_device_id(device_id);
request.mutable_target()->set_x(0.1);
api::MoveL_Response response;
grpc::ServerContext context;
auto status = service_->moveL(&context, &request, &response);
return {
std::move(status),
response.header().success(),
response.header().error_message()};
}
grpc::Status stopMotion(
const std::string& device_id,
api::CommandHeader_Feedback& response)
{
api::CommandHeader_Request request;
request.set_device_id(device_id);
grpc::ServerContext context;
return service_->stopMotion(&context, &request, &response);
}
grpc::Status torqueOff(
const std::string& device_id,
api::CommandHeader_Feedback& response)
{
api::CommandHeader_Request request;
request.set_device_id(device_id);
grpc::ServerContext context;
return service_->torqueOff(&context, &request, &response);
}
std::shared_ptr<JsonCommandRobotArm> left_arm_;
std::shared_ptr<JsonCommandRobotArm> aubo_arm_;
std::shared_ptr<JsonCommandNonArmDevice> non_arm_;
@ -374,5 +568,125 @@ TEST_F(GrpcArmServiceTest,
EXPECT_EQ(left_arm_->execute_calls, 0);
}
TEST_F(GrpcArmServiceTest,
StopMotionRevokesBlockedMoveJLeaseBeforeMoveLReturns)
{
aubo_arm_->blockNextMotion();
auto blocked_move = std::async(
std::launch::async,
[this]() { return moveJ("aubo_arm"); });
const bool move_started = aubo_arm_->waitForBlockingMotion(
"moveJ", std::chrono::seconds(2));
MoveOutcome conflict;
MoveOutcome during_stop;
api::CommandHeader_Feedback torque_off_response;
grpc::Status torque_off_status;
api::CommandHeader_Feedback stop_response;
grpc::Status stop_status;
MoveOutcome resumed_move;
bool stop_started = false;
std::future<grpc::Status> blocked_stop;
if (move_started) {
conflict = moveL("aubo_arm");
aubo_arm_->blockNextStopMotion();
blocked_stop = std::async(
std::launch::async,
[this, &stop_response]() {
return stopMotion("aubo_arm", stop_response);
});
stop_started = aubo_arm_->waitForBlockingStop(
std::chrono::seconds(2));
if (stop_started) {
torque_off_status = torqueOff(
"aubo_arm", torque_off_response);
during_stop = moveL("aubo_arm");
}
aubo_arm_->releaseBlockingStop();
stop_status = blocked_stop.get();
resumed_move = moveL("aubo_arm");
}
// Keep the original RPC active until after the replacement MoveL has
// attempted to acquire control. This models a driver whose stopped motion
// takes time to unwind and guards the lease hand-off itself.
aubo_arm_->releaseBlockingMotion();
const auto original_move = blocked_move.get();
ASSERT_TRUE(move_started);
ASSERT_TRUE(stop_started);
EXPECT_EQ(conflict.status.error_code(),
grpc::StatusCode::FAILED_PRECONDITION);
EXPECT_EQ(conflict.response_error, conflict.status.error_message());
EXPECT_EQ(during_stop.status.error_code(),
grpc::StatusCode::FAILED_PRECONDITION);
EXPECT_EQ(during_stop.response_error,
during_stop.status.error_message());
EXPECT_TRUE(torque_off_status.ok())
<< torque_off_status.error_message();
EXPECT_TRUE(torque_off_response.success())
<< torque_off_response.error_message();
EXPECT_TRUE(stop_status.ok()) << stop_status.error_message();
EXPECT_TRUE(stop_response.success())
<< stop_response.error_message();
EXPECT_TRUE(resumed_move.status.ok())
<< resumed_move.status.error_message();
EXPECT_TRUE(resumed_move.response_success)
<< resumed_move.response_error;
EXPECT_TRUE(original_move.status.ok())
<< original_move.status.error_message();
EXPECT_TRUE(original_move.response_success)
<< original_move.response_error;
EXPECT_EQ(aubo_arm_->moveJCalls(), 1);
EXPECT_EQ(aubo_arm_->moveLCalls(), 1);
EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1);
EXPECT_EQ(aubo_arm_->torqueOffCalls(), 1);
}
TEST_F(GrpcArmServiceTest,
StopMotionRevokesBlockedMoveLLeaseBeforeMoveJReturns)
{
aubo_arm_->blockNextMotion();
auto blocked_move = std::async(
std::launch::async,
[this]() { return moveL("aubo_arm"); });
const bool move_started = aubo_arm_->waitForBlockingMotion(
"moveL", std::chrono::seconds(2));
MoveOutcome conflict;
api::CommandHeader_Feedback stop_response;
grpc::Status stop_status;
MoveOutcome resumed_move;
if (move_started) {
conflict = moveJ("aubo_arm");
stop_status = stopMotion("aubo_arm", stop_response);
resumed_move = moveJ("aubo_arm");
}
aubo_arm_->releaseBlockingMotion();
const auto original_move = blocked_move.get();
ASSERT_TRUE(move_started);
EXPECT_EQ(conflict.status.error_code(),
grpc::StatusCode::FAILED_PRECONDITION);
EXPECT_EQ(conflict.response_error, conflict.status.error_message());
EXPECT_TRUE(stop_status.ok()) << stop_status.error_message();
EXPECT_TRUE(stop_response.success())
<< stop_response.error_message();
EXPECT_TRUE(resumed_move.status.ok())
<< resumed_move.status.error_message();
EXPECT_TRUE(resumed_move.response_success)
<< resumed_move.response_error;
EXPECT_TRUE(original_move.status.ok())
<< original_move.status.error_message();
EXPECT_TRUE(original_move.response_success)
<< original_move.response_error;
EXPECT_EQ(aubo_arm_->moveJCalls(), 1);
EXPECT_EQ(aubo_arm_->moveLCalls(), 1);
EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1);
}
} // namespace
} // namespace cmvr::service

122
cmvr-es/service/grpc/tests/grpc_system_service_test.cpp
View File

@ -2,8 +2,11 @@
#include <array>
#include <chrono>
#include <condition_variable>
#include <cstdint>
#include <future>
#include <memory>
#include <mutex>
#include <string>
#include <utility>
#include <vector>
@ -12,6 +15,7 @@
#include <gtest/gtest.h>
#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
#include "manager/control_authority/include/control_authority_manager.h"
#include "manager/device_manager/include/device_manager.h"
namespace cmvr::service {
@ -36,11 +40,58 @@ public:
{
return health_;
}
bool stop() override
{
std::unique_lock lock(stop_mutex_);
++stop_calls_;
if (block_next_stop_) {
block_next_stop_ = false;
stop_started_ = true;
stop_started_cv_.notify_all();
stop_release_cv_.wait(
lock,
[this]() { return release_stop_; });
}
return true;
}
int stopCalls() const
{
std::lock_guard lock(stop_mutex_);
return stop_calls_;
}
void blockNextStop()
{
std::lock_guard lock(stop_mutex_);
block_next_stop_ = true;
stop_started_ = false;
release_stop_ = false;
}
bool waitForStop(const std::chrono::milliseconds timeout)
{
std::unique_lock lock(stop_mutex_);
return stop_started_cv_.wait_for(
lock, timeout, [this]() { return stop_started_; });
}
void releaseStop()
{
{
std::lock_guard lock(stop_mutex_);
release_stop_ = true;
}
stop_release_cv_.notify_all();
}
private:
device::DeviceKind kind_;
std::string type_name_;
device::DeviceHealthSnapshot health_;
mutable std::mutex stop_mutex_;
std::condition_variable stop_started_cv_;
std::condition_variable stop_release_cv_;
int stop_calls_{0};
bool block_next_stop_{false};
bool stop_started_{false};
bool release_stop_{false};
};
std::uint64_t currentUnixTimeMs()
@ -66,6 +117,7 @@ class GrpcSystemServiceTest : public ::testing::Test {
protected:
void SetUp() override
{
control::ControlAuthorityManager::instance().clear();
device::DeviceManager::destroyInstance();
}
@ -74,6 +126,7 @@ protected:
service_.reset();
owned_devices_.clear();
device::DeviceManager::destroyInstance();
control::ControlAuthorityManager::instance().clear();
}
api::GetDeviceListCommand_Feedback getDeviceList()
@ -287,5 +340,74 @@ TEST_F(GrpcSystemServiceTest, MapsEveryKnownDeviceKind)
}
}
TEST_F(GrpcSystemServiceTest,
StopAllStopsRegisteredDevicesAndRevokesOnlyArmLease)
{
config::DeviceManagerConfig config;
auto& manager = device::DeviceManager::getInstance(config);
auto arm = std::make_shared<SnapshotDevice>(
"leased_arm", device::DeviceKind::Arm, "TestArm");
auto camera = std::make_shared<SnapshotDevice>(
"leased_camera", device::DeviceKind::Camera, "TestCamera");
auto already_stopping_arm = std::make_shared<SnapshotDevice>(
"already_stopping_arm", device::DeviceKind::Arm, "TestArm");
registerDevice(manager, arm);
registerDevice(manager, camera);
registerDevice(manager, already_stopping_arm);
auto& authority = control::ControlAuthorityManager::instance();
const auto arm_lease = authority.tryAcquire(
arm->id(), "arm-session", std::chrono::seconds(30));
const auto camera_lease = authority.tryAcquire(
camera->id(), "camera-session", std::chrono::seconds(30));
const auto existing_stop_barrier = authority.preemptAcquire(
already_stopping_arm->id(),
"existing-stop",
std::chrono::seconds(30));
ASSERT_TRUE(arm_lease.acquired) << arm_lease.detail;
ASSERT_TRUE(camera_lease.acquired) << camera_lease.detail;
ASSERT_TRUE(existing_stop_barrier.acquired)
<< existing_stop_barrier.detail;
ASSERT_TRUE(authority.validate(arm_lease.token));
ASSERT_TRUE(authority.validate(camera_lease.token));
service_ = std::make_unique<gRPCSystemServiceImpl>();
arm->blockNextStop();
api::StopAllCommand_Request request;
api::StopAllCommand_Feedback response;
auto stop_all = std::async(
std::launch::async,
[this, &request, &response]() {
grpc::ServerContext context;
return service_->StopAll(&context, &request, &response);
});
const bool stop_started = arm->waitForStop(
std::chrono::seconds(2));
const auto move_during_stop = authority.tryAcquire(
arm->id(), "move-during-stop", std::chrono::seconds(30));
arm->releaseStop();
const auto status = stop_all.get();
ASSERT_TRUE(stop_started);
EXPECT_FALSE(move_during_stop.acquired);
ASSERT_TRUE(status.ok()) << status.error_message();
ASSERT_TRUE(response.header().success())
<< response.header().error_message();
EXPECT_EQ(arm->stopCalls(), 1);
EXPECT_EQ(camera->stopCalls(), 1);
EXPECT_EQ(already_stopping_arm->stopCalls(), 1);
EXPECT_FALSE(authority.validate(arm_lease.token));
EXPECT_FALSE(authority.isLeased(arm->id()));
EXPECT_TRUE(authority.validate(camera_lease.token));
EXPECT_TRUE(authority.isLeased(camera->id()));
EXPECT_TRUE(authority.validate(existing_stop_barrier.token));
EXPECT_TRUE(authority.isLeased(already_stopping_arm->id()));
const auto move_after_stop = authority.tryAcquire(
arm->id(), "move-after-stop", std::chrono::seconds(30));
EXPECT_TRUE(move_after_stop.acquired) << move_after_stop.detail;
authority.release(move_after_stop.token);
authority.release(existing_stop_barrier.token);
}
} // namespace
} // namespace cmvr::service