fix(stop-all): cancel blocked arm startup safely
This commit is contained in:
parent
de764de607
commit
55912abad0
@ -3,6 +3,7 @@
|
||||
#include "devices/arm/aubo_arm/aubo_motion_result.h"
|
||||
#include "devices/arm/aubo_arm/aubo_motion_state.h"
|
||||
#include "devices/arm/aubo_arm/aubo_safety_state.h"
|
||||
#include "devices/arm/aubo_arm/aubo_torque_on_result.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cctype>
|
||||
@ -547,9 +548,30 @@ bool enforceControllerTermination(
|
||||
{
|
||||
std::unique_lock termination_lock(monitor->termination_mutex);
|
||||
monitor->motion_state->cancelActiveForSafety();
|
||||
const auto stop_request = monitor->motion_state->beginStop();
|
||||
auto stop_request = monitor->motion_state->beginStop();
|
||||
if (!stop_request.started()) {
|
||||
return monitor->cancellation_confirmed.load();
|
||||
constexpr auto kExistingStopTimeout = std::chrono::seconds(6);
|
||||
const auto existing_result =
|
||||
monitor->motion_state->waitForStopCompletion(
|
||||
stop_request,
|
||||
std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
kExistingStopTimeout));
|
||||
if (existing_result == aubo_internal::StopWaitStatus::Timeout) {
|
||||
monitor->cancellation_confirmed.store(false);
|
||||
return false;
|
||||
}
|
||||
|
||||
// A regular stopMotion confirms direct-motion idle, while safety
|
||||
// termination additionally verifies runtime, servo and controller
|
||||
// queues. Re-enter the stop state and perform that stronger check. A
|
||||
// failed existing stop is retried here as well, while MotionState
|
||||
// remains fail-closed between the two attempts.
|
||||
monitor->motion_state->cancelActiveForSafety();
|
||||
stop_request = monitor->motion_state->beginStop();
|
||||
if (!stop_request.started()) {
|
||||
monitor->cancellation_confirmed.store(false);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
const auto fail = [&monitor]() {
|
||||
@ -1010,18 +1032,38 @@ RobotInterfacePtr getPrimaryRobotInterface(const std::shared_ptr<arcs::aubo_sdk:
|
||||
return robot_interface;
|
||||
}
|
||||
|
||||
bool waitForRobotMode(const RobotInterfacePtr& robot_interface,
|
||||
const RobotModeType& target_mode)
|
||||
enum class RobotModeWaitResult {
|
||||
Reached,
|
||||
Cancelled,
|
||||
Timeout,
|
||||
};
|
||||
|
||||
RobotModeWaitResult waitForRobotMode(
|
||||
const RobotInterfacePtr& robot_interface,
|
||||
const RobotModeType& target_mode,
|
||||
const std::function<bool()>& cancellation_requested)
|
||||
{
|
||||
const auto start_time = std::chrono::steady_clock::now();
|
||||
while (std::chrono::steady_clock::now() - start_time < std::chrono::seconds(20)) {
|
||||
if (cancellationRequested(cancellation_requested)) {
|
||||
return RobotModeWaitResult::Cancelled;
|
||||
}
|
||||
const auto current_mode = robot_interface->getRobotState()->getRobotModeType();
|
||||
if (current_mode == target_mode) {
|
||||
return true;
|
||||
return RobotModeWaitResult::Reached;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
}
|
||||
return false;
|
||||
return cancellationRequested(cancellation_requested)
|
||||
? RobotModeWaitResult::Cancelled
|
||||
: RobotModeWaitResult::Timeout;
|
||||
}
|
||||
|
||||
bool waitForRobotMode(const RobotInterfacePtr& robot_interface,
|
||||
const RobotModeType& target_mode)
|
||||
{
|
||||
return waitForRobotMode(robot_interface, target_mode, {}) ==
|
||||
RobotModeWaitResult::Reached;
|
||||
}
|
||||
|
||||
template <typename IsCancelled>
|
||||
@ -1487,6 +1529,12 @@ bool AuboArm::busy() const
|
||||
}
|
||||
|
||||
Result AuboArm::torqueOn()
|
||||
{
|
||||
return torqueOn({});
|
||||
}
|
||||
|
||||
Result AuboArm::torqueOn(
|
||||
const std::function<bool()>& cancellation_requested)
|
||||
{
|
||||
std::shared_ptr<arcs::aubo_sdk::RpcClient> rpc_client;
|
||||
std::shared_ptr<AuboSafetyMonitor> monitor;
|
||||
@ -1500,22 +1548,82 @@ Result AuboArm::torqueOn()
|
||||
monitor = sdk_->safety_monitor;
|
||||
}
|
||||
|
||||
bool cancellation_latched = false;
|
||||
bool controller_mutated = false;
|
||||
const std::function<bool()> cancellation_check =
|
||||
[&cancellation_requested, &cancellation_latched]() {
|
||||
if (!cancellation_latched) {
|
||||
cancellation_latched = cancellationRequested(
|
||||
cancellation_requested);
|
||||
}
|
||||
return cancellation_latched;
|
||||
};
|
||||
const auto cancelled_before_startup = []() {
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandRejected,
|
||||
"[AuboArm] torqueOn cancelled before controller startup");
|
||||
};
|
||||
const auto cancellation_result = [&]() -> std::optional<Result> {
|
||||
if (!cancellation_check()) {
|
||||
return std::nullopt;
|
||||
}
|
||||
if (!controller_mutated) {
|
||||
return cancelled_before_startup();
|
||||
}
|
||||
|
||||
try {
|
||||
std::unique_lock<std::recursive_mutex> cleanup_rpc_lock(
|
||||
monitor->command_rpc_mutex);
|
||||
cancelForSafetyTransition(monitor);
|
||||
if (!enforceControllerTermination(rpc_client, monitor)) {
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandFailed,
|
||||
"[AuboArm] torqueOn cancelled: control ownership was revoked, but controller safety termination could not be confirmed");
|
||||
}
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandRejected,
|
||||
"[AuboArm] torqueOn cancelled: control ownership was revoked; controller safety termination confirmed");
|
||||
} catch (const std::exception& e) {
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandFailed,
|
||||
"[AuboArm] torqueOn cancelled: controller safety termination raised an exception: " +
|
||||
std::string(e.what()));
|
||||
} catch (...) {
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandFailed,
|
||||
"[AuboArm] torqueOn cancelled: controller safety termination raised an unknown exception");
|
||||
}
|
||||
};
|
||||
|
||||
try {
|
||||
if (!monitor) {
|
||||
return Result::failure(
|
||||
ArmErrorCode::RobotNotReady,
|
||||
"[AuboArm] torqueOn failed: hardware safety monitor is unavailable");
|
||||
}
|
||||
|
||||
if (cancellation_check()) {
|
||||
return cancelled_before_startup();
|
||||
}
|
||||
std::unique_lock<std::recursive_mutex> command_rpc_lock(
|
||||
monitor->command_rpc_mutex);
|
||||
if (cancellation_check()) {
|
||||
return cancelled_before_startup();
|
||||
}
|
||||
Result interface_result;
|
||||
auto robot_interface = getPrimaryRobotInterface(
|
||||
rpc_client, "torqueOn", interface_result);
|
||||
if (!interface_result.ok()) {
|
||||
return interface_result;
|
||||
}
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
|
||||
refreshSafetySample(rpc_client, monitor, robot_interface);
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
auto safety_snapshot = monitor->safety_state->snapshot();
|
||||
const std::uint64_t entry_safety_epoch = safety_snapshot.epoch;
|
||||
if (monitor->emergency_stop_source.load() != 0) {
|
||||
@ -1554,21 +1662,37 @@ Result AuboArm::torqueOn()
|
||||
std::string(safetyConditionName(condition)));
|
||||
}
|
||||
|
||||
const int restart_ret =
|
||||
robot_interface->getRobotManage()->restartInterfaceBoard();
|
||||
if (restart_ret != arcs::common_interface::AUBO_OK) {
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandFailed,
|
||||
"[AuboArm] torqueOn recovery failed: restartInterfaceBoard ret=" +
|
||||
std::to_string(restart_ret));
|
||||
controller_mutated = true;
|
||||
if (const auto mutation_result =
|
||||
aubo_internal::runTorqueOnControllerMutation(
|
||||
arcs::common_interface::AUBO_OK,
|
||||
[&robot_interface] {
|
||||
return robot_interface->getRobotManage()
|
||||
->restartInterfaceBoard();
|
||||
},
|
||||
[](const int return_code) {
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandFailed,
|
||||
"[AuboArm] torqueOn recovery failed: "
|
||||
"restartInterfaceBoard ret=" +
|
||||
std::to_string(return_code));
|
||||
},
|
||||
cancellation_result)) {
|
||||
return *mutation_result;
|
||||
}
|
||||
|
||||
const auto safety_deadline =
|
||||
std::chrono::steady_clock::now() +
|
||||
std::chrono::seconds(10);
|
||||
do {
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::milliseconds(100));
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
refreshSafetySample(
|
||||
rpc_client, monitor, robot_interface);
|
||||
safety_snapshot = monitor->safety_state->snapshot();
|
||||
@ -1580,6 +1704,10 @@ Result AuboArm::torqueOn()
|
||||
safety_deadline);
|
||||
}
|
||||
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
|
||||
safety_snapshot = monitor->safety_state->snapshot();
|
||||
if (safety_snapshot.epoch != entry_safety_epoch) {
|
||||
return Result::failure(
|
||||
@ -1595,6 +1723,9 @@ Result AuboArm::torqueOn()
|
||||
}
|
||||
|
||||
if (recovering) {
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
const auto token = monitor->safety_state->beginRecovery(
|
||||
entry_safety_epoch);
|
||||
if (!token.has_value()) {
|
||||
@ -1607,30 +1738,71 @@ Result AuboArm::torqueOn()
|
||||
monitor->safety_state, recovery_token);
|
||||
}
|
||||
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
|
||||
double mass = 0.0;
|
||||
std::vector<double> cog(3, 0.0);
|
||||
std::vector<double> aom(3, 0.0);
|
||||
std::vector<double> inertia(6, 0.0);
|
||||
robot_interface->getRobotConfig()->setPayload(mass, cog, aom, inertia);
|
||||
controller_mutated = true;
|
||||
if (const auto mutation_result =
|
||||
aubo_internal::runTorqueOnControllerMutation(
|
||||
arcs::common_interface::AUBO_OK,
|
||||
[&robot_interface, mass, &cog, &aom, &inertia] {
|
||||
return robot_interface->getRobotConfig()->setPayload(
|
||||
mass, cog, aom, inertia);
|
||||
},
|
||||
[](const int return_code) {
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandFailed,
|
||||
"[AuboArm] torqueOn failed: setPayload ret=" +
|
||||
std::to_string(return_code));
|
||||
},
|
||||
cancellation_result)) {
|
||||
return *mutation_result;
|
||||
}
|
||||
|
||||
auto current_mode =
|
||||
robot_interface->getRobotState()->getRobotModeType();
|
||||
if (current_mode != RobotModeType::Running &&
|
||||
current_mode != RobotModeType::Idle) {
|
||||
const int poweron_ret =
|
||||
robot_interface->getRobotManage()->poweron();
|
||||
if (poweron_ret != arcs::common_interface::AUBO_OK) {
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandFailed,
|
||||
"[AuboArm] torqueOn failed: poweron ret=" +
|
||||
std::to_string(poweron_ret));
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Idle)) {
|
||||
controller_mutated = true;
|
||||
if (const auto mutation_result =
|
||||
aubo_internal::runTorqueOnControllerMutation(
|
||||
arcs::common_interface::AUBO_OK,
|
||||
[&robot_interface] {
|
||||
return robot_interface->getRobotManage()->poweron();
|
||||
},
|
||||
[](const int return_code) {
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandFailed,
|
||||
"[AuboArm] torqueOn failed: poweron ret=" +
|
||||
std::to_string(return_code));
|
||||
},
|
||||
cancellation_result)) {
|
||||
return *mutation_result;
|
||||
}
|
||||
const auto idle_wait = waitForRobotMode(
|
||||
robot_interface,
|
||||
arcs::common_interface::RobotModeType::Idle,
|
||||
cancellation_check);
|
||||
if (idle_wait != RobotModeWaitResult::Reached) {
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Idle");
|
||||
}
|
||||
current_mode = RobotModeType::Idle;
|
||||
}
|
||||
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
refreshSafetySample(rpc_client, monitor, robot_interface);
|
||||
auto before_brake_release =
|
||||
monitor->safety_state->snapshot();
|
||||
@ -1651,6 +1823,10 @@ Result AuboArm::torqueOn()
|
||||
}
|
||||
|
||||
if (recovering) {
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
controller_mutated = true;
|
||||
cancelForSafetyTransition(monitor);
|
||||
const bool cleanup_ok = current_mode == RobotModeType::Running
|
||||
? enforceControllerTermination(rpc_client, monitor)
|
||||
@ -1660,23 +1836,50 @@ Result AuboArm::torqueOn()
|
||||
ArmErrorCode::CommandFailed,
|
||||
"[AuboArm] torqueOn recovery failed: old controller queue could not be acknowledged and cleared before brake release");
|
||||
}
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
}
|
||||
|
||||
if (current_mode != RobotModeType::Running) {
|
||||
const int startup_ret =
|
||||
robot_interface->getRobotManage()->startup();
|
||||
if (startup_ret != arcs::common_interface::AUBO_OK) {
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandFailed,
|
||||
"[AuboArm] torqueOn failed: startup ret=" +
|
||||
std::to_string(startup_ret));
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Running)) {
|
||||
controller_mutated = true;
|
||||
if (const auto mutation_result =
|
||||
aubo_internal::runTorqueOnControllerMutation(
|
||||
arcs::common_interface::AUBO_OK,
|
||||
[&robot_interface] {
|
||||
return robot_interface->getRobotManage()->startup();
|
||||
},
|
||||
[](const int return_code) {
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandFailed,
|
||||
"[AuboArm] torqueOn failed: startup ret=" +
|
||||
std::to_string(return_code));
|
||||
},
|
||||
cancellation_result)) {
|
||||
return *mutation_result;
|
||||
}
|
||||
const auto running_wait = waitForRobotMode(
|
||||
robot_interface,
|
||||
arcs::common_interface::RobotModeType::Running,
|
||||
cancellation_check);
|
||||
if (running_wait != RobotModeWaitResult::Reached) {
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Running");
|
||||
}
|
||||
}
|
||||
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
refreshSafetySample(rpc_client, monitor, robot_interface);
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
const auto after_startup = monitor->safety_state->snapshot();
|
||||
const bool post_recovery_token_current = !recovering ||
|
||||
(after_startup.recovery_in_progress &&
|
||||
@ -1692,12 +1895,18 @@ Result AuboArm::torqueOn()
|
||||
"[AuboArm] torqueOn rejected: safety state changed during startup; the new event remains latched");
|
||||
}
|
||||
if (recovering) {
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
cancelForSafetyTransition(monitor);
|
||||
if (!enforceControllerTermination(rpc_client, monitor)) {
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandFailed,
|
||||
"[AuboArm] torqueOn recovery failed: controller did not reach an empty, steady state after startup");
|
||||
}
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
refreshSafetySample(rpc_client, monitor, robot_interface);
|
||||
const bool robot_running =
|
||||
monitor->robot_mode.load() ==
|
||||
@ -1711,11 +1920,31 @@ Result AuboArm::torqueOn()
|
||||
"[AuboArm] torqueOn recovery failed: safety state changed during recovery");
|
||||
}
|
||||
}
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
emergency_stopped_.store(false);
|
||||
servo_mode_.store(false);
|
||||
if (const auto cancelled = cancellation_result()) {
|
||||
return *cancelled;
|
||||
}
|
||||
return Result::success();
|
||||
} catch (const std::exception& e) {
|
||||
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOn failed: ") + e.what());
|
||||
auto primary_failure = Result::failure(
|
||||
ArmErrorCode::CommandFailed,
|
||||
std::string("[AuboArm] torqueOn failed: ") + e.what());
|
||||
return controller_mutated
|
||||
? aubo_internal::preservePrimaryTorqueOnFailure(
|
||||
std::move(primary_failure), cancellation_result())
|
||||
: primary_failure;
|
||||
} catch (...) {
|
||||
auto primary_failure = Result::failure(
|
||||
ArmErrorCode::CommandFailed,
|
||||
"[AuboArm] torqueOn failed: unknown exception");
|
||||
return controller_mutated
|
||||
? aubo_internal::preservePrimaryTorqueOnFailure(
|
||||
std::move(primary_failure), cancellation_result())
|
||||
: primary_failure;
|
||||
}
|
||||
}
|
||||
|
||||
@ -2293,9 +2522,28 @@ Result AuboArm::stopMotion_(
|
||||
const auto stop_request =
|
||||
motion_state->beginStop(forced_kind);
|
||||
if (!stop_request.started()) {
|
||||
busy_.store(true);
|
||||
constexpr auto kExistingStopTimeout =
|
||||
std::chrono::seconds(6);
|
||||
const auto existing_result =
|
||||
motion_state->waitForStopCompletion(
|
||||
stop_request,
|
||||
std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
kExistingStopTimeout));
|
||||
busy_.store(motion_state->busy());
|
||||
if (existing_result ==
|
||||
aubo_internal::StopWaitStatus::Completed) {
|
||||
return Result::success();
|
||||
}
|
||||
if (existing_result ==
|
||||
aubo_internal::StopWaitStatus::Timeout) {
|
||||
return Result::failure(
|
||||
ArmErrorCode::Timeout,
|
||||
"[AuboArm] stopMotion failed: timeout waiting for the existing stop operation to complete");
|
||||
}
|
||||
return Result::failure(
|
||||
ArmErrorCode::RobotNotReady,
|
||||
"[AuboArm] stopMotion rejected: another stop operation is in progress");
|
||||
ArmErrorCode::CommandFailed,
|
||||
"[AuboArm] stopMotion failed: the existing stop operation could not confirm controller idle");
|
||||
}
|
||||
busy_.store(true);
|
||||
StopStateGuard stop_state_guard{
|
||||
|
||||
@ -3,6 +3,7 @@
|
||||
|
||||
#include <atomic>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
@ -36,6 +37,8 @@ public:
|
||||
bool supportsActionQueueMotion() const noexcept override { return true; }
|
||||
|
||||
Result torqueOn() override;
|
||||
Result torqueOn(
|
||||
const std::function<bool()>& cancellation_requested) override;
|
||||
Result torqueOff() override;
|
||||
Result calibrateZeroQ(const std::string& joint_name) override;
|
||||
Result emergencyStop() override;
|
||||
|
||||
@ -5,6 +5,7 @@
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
|
||||
namespace cmvr::device::aubo_internal {
|
||||
@ -54,11 +55,50 @@ enum class StopStartStatus {
|
||||
AlreadyStopping,
|
||||
};
|
||||
|
||||
enum class StopWaitStatus {
|
||||
Completed,
|
||||
Failed,
|
||||
Timeout,
|
||||
};
|
||||
|
||||
class StopCompletion final {
|
||||
public:
|
||||
StopWaitStatus waitFor(const std::chrono::milliseconds timeout)
|
||||
{
|
||||
std::unique_lock lock(mutex_);
|
||||
if (!cv_.wait_for(lock, timeout, [this]() { return completed_; })) {
|
||||
return StopWaitStatus::Timeout;
|
||||
}
|
||||
return succeeded_
|
||||
? StopWaitStatus::Completed
|
||||
: StopWaitStatus::Failed;
|
||||
}
|
||||
|
||||
private:
|
||||
friend class MotionState;
|
||||
|
||||
void finish(const bool succeeded)
|
||||
{
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
succeeded_ = succeeded;
|
||||
completed_ = true;
|
||||
}
|
||||
cv_.notify_all();
|
||||
}
|
||||
|
||||
std::mutex mutex_;
|
||||
std::condition_variable cv_;
|
||||
bool completed_{false};
|
||||
bool succeeded_{false};
|
||||
};
|
||||
|
||||
struct StopRequest {
|
||||
StopStartStatus status{StopStartStatus::AlreadyStopping};
|
||||
MotionKind kind{MotionKind::None};
|
||||
MotionToken active_token;
|
||||
bool tracked_motion{false};
|
||||
std::shared_ptr<StopCompletion> completion;
|
||||
|
||||
bool started() const noexcept
|
||||
{
|
||||
@ -152,10 +192,17 @@ public:
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
if (stop_in_progress_) {
|
||||
return {};
|
||||
return {
|
||||
StopStartStatus::AlreadyStopping,
|
||||
MotionKind::None,
|
||||
{},
|
||||
false,
|
||||
active_stop_completion_};
|
||||
}
|
||||
|
||||
auto completion = std::make_shared<StopCompletion>();
|
||||
stop_in_progress_ = true;
|
||||
active_stop_completion_ = completion;
|
||||
const MotionToken active = owner_active_
|
||||
? active_token_
|
||||
: MotionToken{};
|
||||
@ -186,7 +233,8 @@ public:
|
||||
StopStartStatus::Started,
|
||||
kind,
|
||||
active,
|
||||
tracked_motion};
|
||||
tracked_motion,
|
||||
completion};
|
||||
}
|
||||
|
||||
SafetyCancelResult cancelActiveForSafety()
|
||||
@ -248,27 +296,51 @@ public:
|
||||
active_token_.generation == token.generation;
|
||||
}
|
||||
|
||||
StopWaitStatus waitForStopCompletion(
|
||||
const StopRequest& request,
|
||||
const std::chrono::milliseconds timeout) const
|
||||
{
|
||||
if (!request.completion) {
|
||||
return StopWaitStatus::Failed;
|
||||
}
|
||||
return request.completion->waitFor(timeout);
|
||||
}
|
||||
|
||||
bool completeStop()
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
if (owner_active_) {
|
||||
return false;
|
||||
std::shared_ptr<StopCompletion> completion;
|
||||
{
|
||||
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;
|
||||
completion = std::move(active_stop_completion_);
|
||||
owner_finished_cv_.notify_all();
|
||||
}
|
||||
if (completion) {
|
||||
completion->finish(true);
|
||||
}
|
||||
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();
|
||||
std::shared_ptr<StopCompletion> completion;
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
stop_in_progress_ = false;
|
||||
blocked_ = true;
|
||||
completion = std::move(active_stop_completion_);
|
||||
owner_finished_cv_.notify_all();
|
||||
}
|
||||
if (completion) {
|
||||
completion->finish(false);
|
||||
}
|
||||
}
|
||||
|
||||
bool busy() const
|
||||
@ -286,6 +358,7 @@ private:
|
||||
MotionToken active_token_;
|
||||
MotionKind last_kind_{MotionKind::None};
|
||||
MotionKind previous_kind_{MotionKind::None};
|
||||
std::shared_ptr<StopCompletion> active_stop_completion_;
|
||||
bool owner_active_{false};
|
||||
bool stop_in_progress_{false};
|
||||
bool blocked_{false};
|
||||
|
||||
50
cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h
Normal file
50
cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h
Normal file
@ -0,0 +1,50 @@
|
||||
#ifndef CMVR_ES_AUBO_TORQUE_ON_RESULT_H
|
||||
#define CMVR_ES_AUBO_TORQUE_ON_RESULT_H
|
||||
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#include "common/types/arm/arm_types.h"
|
||||
|
||||
namespace cmvr::device::aubo_internal {
|
||||
|
||||
inline Result preservePrimaryTorqueOnFailure(
|
||||
Result primary_failure,
|
||||
const std::optional<Result>& cancellation_outcome)
|
||||
{
|
||||
if (!cancellation_outcome.has_value() ||
|
||||
cancellation_outcome->message.empty()) {
|
||||
return primary_failure;
|
||||
}
|
||||
|
||||
if (!primary_failure.message.empty()) {
|
||||
primary_failure.message += "; ";
|
||||
}
|
||||
primary_failure.message +=
|
||||
"cancellation handling: " + cancellation_outcome->message;
|
||||
return primary_failure;
|
||||
}
|
||||
|
||||
template <typename Mutation, typename FailureResult,
|
||||
typename CancellationOutcome>
|
||||
std::optional<Result> runTorqueOnControllerMutation(
|
||||
const int success_code,
|
||||
Mutation&& mutation,
|
||||
FailureResult&& failure_result,
|
||||
CancellationOutcome&& cancellation_outcome)
|
||||
{
|
||||
const int return_code = std::forward<Mutation>(mutation)();
|
||||
if (return_code != success_code) {
|
||||
auto primary_failure =
|
||||
std::forward<FailureResult>(failure_result)(return_code);
|
||||
return preservePrimaryTorqueOnFailure(
|
||||
std::move(primary_failure),
|
||||
std::forward<CancellationOutcome>(cancellation_outcome)());
|
||||
}
|
||||
return std::forward<CancellationOutcome>(cancellation_outcome)();
|
||||
}
|
||||
|
||||
} // namespace cmvr::device::aubo_internal
|
||||
|
||||
#endif // CMVR_ES_AUBO_TORQUE_ON_RESULT_H
|
||||
@ -1,6 +1,10 @@
|
||||
#include "devices/arm/aubo_arm/aubo_motion_result.h"
|
||||
#include "devices/arm/aubo_arm/aubo_torque_on_result.h"
|
||||
|
||||
#include <iostream>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace {
|
||||
|
||||
@ -19,7 +23,9 @@ int main()
|
||||
{
|
||||
using cmvr::device::aubo_internal::MotionCommandOutcome;
|
||||
using cmvr::device::aubo_internal::MotionWaitResult;
|
||||
using cmvr::device::aubo_internal::preservePrimaryTorqueOnFailure;
|
||||
using cmvr::device::aubo_internal::resolveMotionCommand;
|
||||
using cmvr::device::aubo_internal::runTorqueOnControllerMutation;
|
||||
|
||||
constexpr int success_code = 0;
|
||||
constexpr int request_ignore_code = 13;
|
||||
@ -79,5 +85,74 @@ int main()
|
||||
wait_cancelled) == MotionCommandOutcome::Cancelled);
|
||||
CHECK_TRUE(wait_calls == 1);
|
||||
|
||||
using cmvr::device::ArmErrorCode;
|
||||
using cmvr::device::Result;
|
||||
std::vector<std::string> mutation_trace;
|
||||
const auto failed_mutation = runTorqueOnControllerMutation(
|
||||
success_code,
|
||||
[&mutation_trace] {
|
||||
mutation_trace.push_back("mutation");
|
||||
return 42;
|
||||
},
|
||||
[&mutation_trace](const int return_code) {
|
||||
mutation_trace.push_back("primary-failure");
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandFailed,
|
||||
"vendor failure ret=" + std::to_string(return_code));
|
||||
},
|
||||
[&mutation_trace]() -> std::optional<Result> {
|
||||
mutation_trace.push_back("cancellation-cleanup");
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandRejected,
|
||||
"controller safety termination confirmed");
|
||||
});
|
||||
CHECK_TRUE(failed_mutation.has_value());
|
||||
CHECK_TRUE(failed_mutation->code == ArmErrorCode::CommandFailed);
|
||||
CHECK_TRUE(
|
||||
failed_mutation->message.find("vendor failure ret=42") !=
|
||||
std::string::npos);
|
||||
CHECK_TRUE(
|
||||
failed_mutation->message.find(
|
||||
"controller safety termination confirmed") !=
|
||||
std::string::npos);
|
||||
CHECK_TRUE(mutation_trace.size() == 3);
|
||||
CHECK_TRUE(mutation_trace[0] == "mutation");
|
||||
CHECK_TRUE(mutation_trace[1] == "primary-failure");
|
||||
CHECK_TRUE(mutation_trace[2] == "cancellation-cleanup");
|
||||
|
||||
const auto cleanup_failed = preservePrimaryTorqueOnFailure(
|
||||
Result::failure(
|
||||
ArmErrorCode::CommandFailed,
|
||||
"vendor exception"),
|
||||
std::optional<Result>{Result::failure(
|
||||
ArmErrorCode::CommandFailed,
|
||||
"safety termination failed")});
|
||||
CHECK_TRUE(cleanup_failed.code == ArmErrorCode::CommandFailed);
|
||||
CHECK_TRUE(
|
||||
cleanup_failed.message.find("vendor exception") !=
|
||||
std::string::npos);
|
||||
CHECK_TRUE(
|
||||
cleanup_failed.message.find("safety termination failed") !=
|
||||
std::string::npos);
|
||||
|
||||
int successful_cancellation_checks = 0;
|
||||
const auto cancelled_after_success = runTorqueOnControllerMutation(
|
||||
success_code,
|
||||
[] { return 0; },
|
||||
[](const int) {
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandFailed, "must not be used");
|
||||
},
|
||||
[&successful_cancellation_checks]() -> std::optional<Result> {
|
||||
++successful_cancellation_checks;
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandRejected,
|
||||
"cancelled after successful mutation");
|
||||
});
|
||||
CHECK_TRUE(cancelled_after_success.has_value());
|
||||
CHECK_TRUE(
|
||||
cancelled_after_success->code == ArmErrorCode::CommandRejected);
|
||||
CHECK_TRUE(successful_cancellation_checks == 1);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -1,7 +1,9 @@
|
||||
#include "devices/arm/aubo_arm/aubo_motion_state.h"
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
|
||||
namespace {
|
||||
|
||||
@ -36,8 +38,13 @@ int main()
|
||||
joint.token.generation);
|
||||
CHECK_TRUE(stop_joint.tracked_motion);
|
||||
CHECK_TRUE(state.cancelled(joint.token));
|
||||
CHECK_TRUE(state.beginStop().status ==
|
||||
const auto joined_stop_joint = state.beginStop();
|
||||
CHECK_TRUE(joined_stop_joint.status ==
|
||||
StopStartStatus::AlreadyStopping);
|
||||
CHECK_TRUE(
|
||||
state.waitForStopCompletion(
|
||||
joined_stop_joint, std::chrono::milliseconds(1)) ==
|
||||
StopWaitStatus::Timeout);
|
||||
CHECK_TRUE(state.begin(MotionKind::Linear).status ==
|
||||
MotionStartStatus::Stopping);
|
||||
CHECK_TRUE(!state.waitForOwnerExit(
|
||||
@ -48,6 +55,10 @@ int main()
|
||||
CHECK_TRUE(state.waitForOwnerExit(
|
||||
joint.token, std::chrono::milliseconds(1)));
|
||||
CHECK_TRUE(state.completeStop());
|
||||
CHECK_TRUE(
|
||||
state.waitForStopCompletion(
|
||||
joined_stop_joint, std::chrono::milliseconds(1)) ==
|
||||
StopWaitStatus::Completed);
|
||||
CHECK_TRUE(!state.busy());
|
||||
|
||||
const auto linear = state.begin(MotionKind::Linear);
|
||||
@ -81,7 +92,14 @@ int main()
|
||||
const auto idle_stop = state.beginStop();
|
||||
CHECK_TRUE(idle_stop.kind == MotionKind::None);
|
||||
CHECK_TRUE(!idle_stop.tracked_motion);
|
||||
const auto joined_idle_stop = state.beginStop();
|
||||
CHECK_TRUE(joined_idle_stop.status ==
|
||||
StopStartStatus::AlreadyStopping);
|
||||
state.failStop();
|
||||
CHECK_TRUE(
|
||||
state.waitForStopCompletion(
|
||||
joined_idle_stop, std::chrono::milliseconds(1)) ==
|
||||
StopWaitStatus::Failed);
|
||||
const auto retry_idle_stop = state.beginStop();
|
||||
CHECK_TRUE(retry_idle_stop.kind == MotionKind::None);
|
||||
CHECK_TRUE(!retry_idle_stop.tracked_motion);
|
||||
@ -152,5 +170,44 @@ int main()
|
||||
CHECK_TRUE(retained_stop.tracked_motion);
|
||||
CHECK_TRUE(state.completeStop());
|
||||
|
||||
// A waiter keeps the completion for the stop it joined even when another
|
||||
// stop starts and finishes before the waiter is scheduled again.
|
||||
const auto concurrent_first_stop = state.beginStop();
|
||||
CHECK_TRUE(concurrent_first_stop.started());
|
||||
const auto concurrent_first_join = state.beginStop();
|
||||
CHECK_TRUE(concurrent_first_join.status ==
|
||||
StopStartStatus::AlreadyStopping);
|
||||
std::atomic<bool> waiter_entered{false};
|
||||
std::atomic<StopWaitStatus> first_wait_result{
|
||||
StopWaitStatus::Timeout};
|
||||
std::thread first_waiter([&]() {
|
||||
waiter_entered.store(true, std::memory_order_release);
|
||||
first_wait_result.store(
|
||||
state.waitForStopCompletion(
|
||||
concurrent_first_join,
|
||||
std::chrono::milliseconds(500)),
|
||||
std::memory_order_release);
|
||||
});
|
||||
while (!waiter_entered.load(std::memory_order_acquire)) {
|
||||
std::this_thread::yield();
|
||||
}
|
||||
|
||||
const bool concurrent_first_completed = state.completeStop();
|
||||
const auto concurrent_second_stop = state.beginStop();
|
||||
const auto concurrent_second_join = state.beginStop();
|
||||
state.failStop();
|
||||
first_waiter.join();
|
||||
|
||||
CHECK_TRUE(concurrent_first_completed);
|
||||
CHECK_TRUE(concurrent_second_stop.started());
|
||||
CHECK_TRUE(concurrent_second_join.status ==
|
||||
StopStartStatus::AlreadyStopping);
|
||||
CHECK_TRUE(first_wait_result.load(std::memory_order_acquire) ==
|
||||
StopWaitStatus::Completed);
|
||||
CHECK_TRUE(
|
||||
state.waitForStopCompletion(
|
||||
concurrent_second_join, std::chrono::milliseconds(1)) ==
|
||||
StopWaitStatus::Failed);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -2,6 +2,7 @@
|
||||
#define CMVR_ES_ROBOT_ARM_H
|
||||
|
||||
#include <cstddef>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
@ -44,6 +45,28 @@ public:
|
||||
}
|
||||
|
||||
virtual Result torqueOn() = 0;
|
||||
// Long-running startup implementations may cooperatively observe loss of
|
||||
// their control lease. Backends which have not adopted cancellation retain
|
||||
// the legacy behavior, while still rejecting an already-cancelled request
|
||||
// before entering the vendor API.
|
||||
virtual Result torqueOn(
|
||||
const std::function<bool()>& cancellation_requested)
|
||||
{
|
||||
if (cancellation_requested) {
|
||||
try {
|
||||
if (cancellation_requested()) {
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandRejected,
|
||||
"torqueOn cancelled before execution");
|
||||
}
|
||||
} catch (...) {
|
||||
return Result::failure(
|
||||
ArmErrorCode::CommandRejected,
|
||||
"torqueOn cancellation check failed");
|
||||
}
|
||||
}
|
||||
return torqueOn();
|
||||
}
|
||||
virtual Result torqueOff() = 0;
|
||||
virtual Result calibrateZeroQ(const std::string& joint_name) = 0;
|
||||
|
||||
|
||||
@ -5,6 +5,7 @@
|
||||
#ifndef GRPC_SYSTEM_SERVICE_H
|
||||
#define GRPC_SYSTEM_SERVICE_H
|
||||
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
|
||||
#include "cmvr/api/system_service.grpc.pb.h"
|
||||
@ -19,7 +20,12 @@ namespace cmvr::service
|
||||
class gRPCSystemServiceImpl: public api::SystemService::Service {
|
||||
public:
|
||||
gRPCSystemServiceImpl();
|
||||
explicit gRPCSystemServiceImpl(
|
||||
std::chrono::milliseconds stop_timeout);
|
||||
~gRPCSystemServiceImpl() override;
|
||||
// Exposed only to synchronize lifecycle concurrency tests.
|
||||
static bool waitForStopDispatcherDestructionForTesting(
|
||||
std::chrono::milliseconds timeout);
|
||||
// Called by GrpcServerTask before grpc::Server::Shutdown so accepted
|
||||
// ActionQueue handlers can reach a terminal result and do not hold the
|
||||
// synchronous server shutdown open indefinitely.
|
||||
@ -32,10 +38,10 @@ namespace cmvr::service
|
||||
grpc::Status ExecuteActionQueue(grpc::ServerContext* context, const cmvr::api::ActionQueueCommand_Request* request, cmvr::api::ActionQueueCommand_Feedback* response) override;
|
||||
private:
|
||||
device::DeviceManager& dmgr_;
|
||||
// Outlives ActionQueueExecutor and every StopAll RPC stack. A stop
|
||||
// backend which ignores the shared deadline can therefore finish in
|
||||
// its owned worker without accessing destroyed RPC-local state.
|
||||
std::unique_ptr<StopOperationDispatcher> stop_dispatcher_;
|
||||
const std::chrono::milliseconds stop_timeout_;
|
||||
// Process instances share running jobs through a lifecycle registry.
|
||||
// The last service owner joins every worker before replacement.
|
||||
std::shared_ptr<StopOperationDispatcher> stop_dispatcher_;
|
||||
std::unique_ptr<ActionQueueExecutor> action_queue_;
|
||||
};
|
||||
}
|
||||
|
||||
@ -173,6 +173,19 @@ grpc::Status setControlLeaseConflict(
|
||||
response->mutable_header(), device_id, detail);
|
||||
}
|
||||
|
||||
grpc::Status setControlCancelled(
|
||||
api::CommandHeader_Feedback* response,
|
||||
const std::string& device_id,
|
||||
const std::string& reason)
|
||||
{
|
||||
std::string message = "RobotArm control was cancelled: " + device_id;
|
||||
if (!reason.empty()) {
|
||||
message += ", " + reason;
|
||||
}
|
||||
fillFeedback(response, false, message);
|
||||
return grpc::Status(grpc::StatusCode::CANCELLED, message);
|
||||
}
|
||||
|
||||
class ScopedUnaryControlLease final {
|
||||
public:
|
||||
ScopedUnaryControlLease(
|
||||
@ -383,7 +396,7 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*,
|
||||
}
|
||||
}
|
||||
|
||||
grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*,
|
||||
grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext* context,
|
||||
const api::CommandHeader_Request* request,
|
||||
api::CommandHeader_Feedback* response)
|
||||
{
|
||||
@ -404,7 +417,27 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*,
|
||||
return setControlDispatchFailure(
|
||||
response, device_id, control_lease, "torqueOn");
|
||||
}
|
||||
const auto result = arm->torqueOn();
|
||||
const auto cancellation_requested =
|
||||
control_lease.cancellationRequested(context);
|
||||
const auto result = arm->torqueOn(cancellation_requested);
|
||||
const bool control_current = control_lease.current();
|
||||
const bool cancellation_result =
|
||||
result.ok() ||
|
||||
result.code == device::ArmErrorCode::CommandRejected;
|
||||
if (cancellation_result &&
|
||||
!control_lease.admissionCurrent()) {
|
||||
return setStopAllRejected(response, device_id);
|
||||
}
|
||||
const bool rpc_cancelled = context && context->IsCancelled();
|
||||
if (cancellation_result &&
|
||||
(rpc_cancelled || !control_current)) {
|
||||
return setControlCancelled(
|
||||
response,
|
||||
device_id,
|
||||
rpc_cancelled
|
||||
? "the RPC was cancelled"
|
||||
: "control ownership was revoked");
|
||||
}
|
||||
fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
|
||||
if (result.ok()) {
|
||||
logRpcSuccess("torqueOn", device_id);
|
||||
|
||||
@ -7,6 +7,7 @@
|
||||
#include <algorithm>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
@ -407,6 +408,92 @@ using StopDispatcher = cmvr::service::StopOperationDispatcher;
|
||||
using StopHandle = StopDispatcher::Handle;
|
||||
using StopOutcome = StopDispatcher::OperationResult;
|
||||
|
||||
struct ProcessStopDispatcherRegistry final {
|
||||
std::mutex mutex;
|
||||
std::condition_variable available;
|
||||
std::weak_ptr<StopDispatcher> dispatcher;
|
||||
bool destroying{false};
|
||||
};
|
||||
|
||||
ProcessStopDispatcherRegistry& processStopDispatcherRegistry()
|
||||
{
|
||||
static ProcessStopDispatcherRegistry registry;
|
||||
return registry;
|
||||
}
|
||||
|
||||
void acquireProcessStopDispatcher(
|
||||
std::shared_ptr<StopDispatcher>& owner)
|
||||
{
|
||||
auto& registry = processStopDispatcherRegistry();
|
||||
std::unique_lock lock(registry.mutex);
|
||||
registry.available.wait(
|
||||
lock, [®istry] { return !registry.destroying; });
|
||||
if (auto existing = registry.dispatcher.lock()) {
|
||||
owner = std::move(existing);
|
||||
return;
|
||||
}
|
||||
|
||||
owner = std::make_shared<StopDispatcher>();
|
||||
registry.dispatcher = owner;
|
||||
}
|
||||
|
||||
void releaseProcessStopDispatcher(
|
||||
std::shared_ptr<StopDispatcher>& owner) noexcept
|
||||
{
|
||||
if (!owner) {
|
||||
return;
|
||||
}
|
||||
|
||||
auto& registry = processStopDispatcherRegistry();
|
||||
std::unique_lock lock(registry.mutex);
|
||||
if (owner.use_count() > 1) {
|
||||
owner.reset();
|
||||
return;
|
||||
}
|
||||
|
||||
// Do not admit a replacement while the last dispatcher is joining a
|
||||
// worker which may still be inside a device driver.
|
||||
registry.destroying = true;
|
||||
registry.dispatcher.reset();
|
||||
registry.available.notify_all();
|
||||
auto last_owner = std::move(owner);
|
||||
lock.unlock();
|
||||
last_owner.reset();
|
||||
lock.lock();
|
||||
registry.destroying = false;
|
||||
lock.unlock();
|
||||
registry.available.notify_all();
|
||||
}
|
||||
|
||||
class StopDeadlineDetail final {
|
||||
public:
|
||||
void publishInitial(std::string detail)
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
detail_ = std::move(detail);
|
||||
}
|
||||
|
||||
void publishUntil(
|
||||
const StopDispatcher::Deadline deadline,
|
||||
std::string detail)
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
if (StopDispatcher::Clock::now() < deadline) {
|
||||
detail_ = std::move(detail);
|
||||
}
|
||||
}
|
||||
|
||||
std::string snapshot() const
|
||||
{
|
||||
std::lock_guard lock(mutex_);
|
||||
return detail_;
|
||||
}
|
||||
|
||||
private:
|
||||
mutable std::mutex mutex_;
|
||||
std::string detail_;
|
||||
};
|
||||
|
||||
struct StopHandleEntry final {
|
||||
std::string device_id;
|
||||
StopHandle handle;
|
||||
@ -565,8 +652,13 @@ StopOutcome stopControlWithFence(
|
||||
const StopDispatcher::Deadline deadline,
|
||||
InitialStop&& initial_stop,
|
||||
FinalStop&& final_stop,
|
||||
const std::string& description)
|
||||
const std::string& description,
|
||||
const std::shared_ptr<StopDeadlineDetail>& deadline_detail)
|
||||
{
|
||||
deadline_detail->publishUntil(
|
||||
deadline,
|
||||
"initial " + description +
|
||||
" stop did not complete before the StopAll deadline");
|
||||
const auto initial = invokeStopOperation(
|
||||
std::forward<InitialStop>(initial_stop),
|
||||
"initial " + description + " stop");
|
||||
@ -582,18 +674,37 @@ StopOutcome stopControlWithFence(
|
||||
" safety barrier was unavailable after the initial stop"};
|
||||
}
|
||||
|
||||
const std::string handler_timeout_detail =
|
||||
"timed out waiting for the preempted " + description +
|
||||
" control handler to exit";
|
||||
deadline_detail->publishUntil(deadline, handler_timeout_detail);
|
||||
const bool handler_drained =
|
||||
cmvr::control::ControlAuthorityManager::instance()
|
||||
.waitForPreemptedRelease(
|
||||
barrier, remainingStopBudget(deadline));
|
||||
if (handler_drained) {
|
||||
deadline_detail->publishUntil(
|
||||
deadline,
|
||||
"final " + description +
|
||||
" stop did not complete before the StopAll deadline");
|
||||
}
|
||||
// This final typed stop remains mandatory even when the handler wait used
|
||||
// the entire RPC budget. The dispatcher owns this worker past the RPC
|
||||
// deadline, closing the race where an old handler resumes after the first
|
||||
// stop while the safety barrier remains fail-closed.
|
||||
const auto final = invokeStopOperation(
|
||||
std::forward<FinalStop>(final_stop),
|
||||
"final " + description + " stop");
|
||||
if (!handler_drained) {
|
||||
return {
|
||||
false,
|
||||
"timed out waiting for the preempted " + description +
|
||||
" control handler to exit"};
|
||||
std::string detail = handler_timeout_detail;
|
||||
if (!final.success) {
|
||||
detail += "; " +
|
||||
(final.detail.empty()
|
||||
? "final " + description +
|
||||
" stop was not confirmed"
|
||||
: final.detail);
|
||||
}
|
||||
return {false, std::move(detail)};
|
||||
}
|
||||
return final;
|
||||
}
|
||||
@ -927,13 +1038,42 @@ cmvr::api::SystemDeviceHealth toApiDeviceHealth(
|
||||
} // namespace
|
||||
|
||||
gRPCSystemServiceImpl::gRPCSystemServiceImpl()
|
||||
: dmgr_(DeviceManager::getInstance()),
|
||||
stop_dispatcher_(std::make_unique<StopOperationDispatcher>()),
|
||||
action_queue_(std::make_unique<ActionQueueExecutor>(dmgr_))
|
||||
: gRPCSystemServiceImpl(std::chrono::seconds(15))
|
||||
{
|
||||
}
|
||||
|
||||
gRPCSystemServiceImpl::~gRPCSystemServiceImpl() = default;
|
||||
gRPCSystemServiceImpl::gRPCSystemServiceImpl(
|
||||
const std::chrono::milliseconds stop_timeout)
|
||||
: dmgr_(DeviceManager::getInstance()),
|
||||
stop_timeout_(
|
||||
stop_timeout > std::chrono::milliseconds::zero()
|
||||
? stop_timeout
|
||||
: std::chrono::seconds(15)),
|
||||
action_queue_(std::make_unique<ActionQueueExecutor>(dmgr_))
|
||||
{
|
||||
// Acquire only after ActionQueue construction succeeds. This ensures an
|
||||
// exception cannot release the last dispatcher outside the registry.
|
||||
acquireProcessStopDispatcher(stop_dispatcher_);
|
||||
}
|
||||
|
||||
gRPCSystemServiceImpl::~gRPCSystemServiceImpl()
|
||||
{
|
||||
// The dispatcher intentionally outlives ActionQueue, then joins any
|
||||
// deadline-overrunning stop workers before the last service disappears.
|
||||
action_queue_.reset();
|
||||
releaseProcessStopDispatcher(stop_dispatcher_);
|
||||
}
|
||||
|
||||
bool gRPCSystemServiceImpl::waitForStopDispatcherDestructionForTesting(
|
||||
const std::chrono::milliseconds timeout)
|
||||
{
|
||||
auto& registry = processStopDispatcherRegistry();
|
||||
std::unique_lock lock(registry.mutex);
|
||||
return registry.available.wait_for(
|
||||
lock,
|
||||
timeout,
|
||||
[®istry] { return registry.destroying; });
|
||||
}
|
||||
|
||||
void gRPCSystemServiceImpl::prepareForShutdown()
|
||||
{
|
||||
@ -1079,9 +1219,8 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context,
|
||||
const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response)
|
||||
{
|
||||
(void)request;
|
||||
constexpr auto stop_timeout = std::chrono::seconds(15);
|
||||
const auto stop_deadline =
|
||||
std::chrono::steady_clock::now() + stop_timeout;
|
||||
std::chrono::steady_clock::now() + stop_timeout_;
|
||||
|
||||
std::unique_lock<std::timed_mutex> stop_all_lock(
|
||||
processStopAllMutex(), std::defer_lock);
|
||||
@ -1266,41 +1405,60 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context,
|
||||
control_stops.reserve(control_targets.size());
|
||||
for (const auto& target : control_targets) {
|
||||
StopHandle handle;
|
||||
auto deadline_detail = std::make_shared<StopDeadlineDetail>();
|
||||
if (target.arm) {
|
||||
const auto arm = target.arm;
|
||||
const auto barrier = target.barrier;
|
||||
deadline_detail->publishInitial(
|
||||
"initial RobotArm stop did not complete before the "
|
||||
"StopAll deadline");
|
||||
handle = stop_dispatcher_->submit(
|
||||
"control:" + stopResourceKey(target.id),
|
||||
[arm, barrier, stop_deadline] {
|
||||
[arm, barrier, stop_deadline, deadline_detail] {
|
||||
return stopControlWithFence(
|
||||
barrier, stop_deadline,
|
||||
[arm] { return stopArm(arm, false); },
|
||||
[arm] { return stopArm(arm, true); },
|
||||
"RobotArm");
|
||||
"RobotArm", deadline_detail);
|
||||
},
|
||||
[deadline_detail] {
|
||||
return deadline_detail->snapshot();
|
||||
});
|
||||
} else if (target.agv) {
|
||||
const auto agv = target.agv;
|
||||
const auto barrier = target.barrier;
|
||||
deadline_detail->publishInitial(
|
||||
"initial AGV stop did not complete before the StopAll "
|
||||
"deadline");
|
||||
handle = stop_dispatcher_->submit(
|
||||
"control:" + stopResourceKey(target.id),
|
||||
[agv, barrier, stop_deadline] {
|
||||
[agv, barrier, stop_deadline, deadline_detail] {
|
||||
return stopControlWithFence(
|
||||
barrier, stop_deadline,
|
||||
[agv] { return stopAgv(agv, false); },
|
||||
[agv] { return stopAgv(agv, true); },
|
||||
"AGV");
|
||||
"AGV", deadline_detail);
|
||||
},
|
||||
[deadline_detail] {
|
||||
return deadline_detail->snapshot();
|
||||
});
|
||||
} else {
|
||||
const auto hand = target.hand;
|
||||
const auto barrier = target.barrier;
|
||||
deadline_detail->publishInitial(
|
||||
"initial DexHand stop did not complete before the "
|
||||
"StopAll deadline");
|
||||
handle = stop_dispatcher_->submit(
|
||||
"control:" + stopResourceKey(target.id),
|
||||
[hand, barrier, stop_deadline] {
|
||||
[hand, barrier, stop_deadline, deadline_detail] {
|
||||
return stopControlWithFence(
|
||||
barrier, stop_deadline,
|
||||
[hand] { return stopDexHand(hand, false); },
|
||||
[hand] { return stopDexHand(hand, true); },
|
||||
"DexHand");
|
||||
"DexHand", deadline_detail);
|
||||
},
|
||||
[deadline_detail] {
|
||||
return deadline_detail->snapshot();
|
||||
});
|
||||
}
|
||||
control_stops.push_back(
|
||||
|
||||
@ -1,23 +1,28 @@
|
||||
#include "service/grpc/include/grpc_arm_service.h"
|
||||
|
||||
#include <chrono>
|
||||
#include <cstdio>
|
||||
#include <condition_variable>
|
||||
#include <functional>
|
||||
#include <future>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <google/protobuf/descriptor.h>
|
||||
#include <grpcpp/grpcpp.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <unistd.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"
|
||||
#include "service/grpc/include/grpc_error_logging_interceptor.h"
|
||||
#include "service/stop_all/include/stop_all_admission_gate.h"
|
||||
|
||||
namespace cmvr::service {
|
||||
@ -63,7 +68,43 @@ public:
|
||||
return device::ControlMode::None;
|
||||
}
|
||||
|
||||
device::Result torqueOn() override { return device::Result::success(); }
|
||||
device::Result torqueOn() override { return torqueOn({}); }
|
||||
device::Result torqueOn(
|
||||
const std::function<bool()>& cancellation_requested) override
|
||||
{
|
||||
std::unique_lock lock(motion_mutex_);
|
||||
++torque_on_calls_;
|
||||
last_torque_on_had_cancellation_ =
|
||||
static_cast<bool>(cancellation_requested);
|
||||
if (!block_next_torque_on_) {
|
||||
return device::Result::success();
|
||||
}
|
||||
|
||||
block_next_torque_on_ = false;
|
||||
blocking_torque_on_started_ = true;
|
||||
torque_on_started_cv_.notify_all();
|
||||
while (!release_blocking_torque_on_) {
|
||||
lock.unlock();
|
||||
const bool cancelled =
|
||||
cancellation_requested && cancellation_requested();
|
||||
lock.lock();
|
||||
if (cancelled) {
|
||||
last_torque_on_cancellation_requested_ = true;
|
||||
if (fail_next_torque_on_cancellation_) {
|
||||
fail_next_torque_on_cancellation_ = false;
|
||||
return device::Result::failure(
|
||||
device::ArmErrorCode::CommandFailed,
|
||||
"simulated torqueOn safety termination failure");
|
||||
}
|
||||
return device::Result::failure(
|
||||
device::ArmErrorCode::CommandRejected,
|
||||
"simulated torqueOn cancellation");
|
||||
}
|
||||
torque_on_release_cv_.wait_for(
|
||||
lock, std::chrono::milliseconds(5));
|
||||
}
|
||||
return device::Result::success();
|
||||
}
|
||||
device::Result torqueOff() override
|
||||
{
|
||||
std::lock_guard lock(motion_mutex_);
|
||||
@ -159,6 +200,40 @@ public:
|
||||
blocking_motion_name_.clear();
|
||||
}
|
||||
|
||||
void blockNextTorqueOn()
|
||||
{
|
||||
std::lock_guard lock(motion_mutex_);
|
||||
block_next_torque_on_ = true;
|
||||
blocking_torque_on_started_ = false;
|
||||
release_blocking_torque_on_ = false;
|
||||
last_torque_on_cancellation_requested_ = false;
|
||||
}
|
||||
|
||||
void failNextTorqueOnCancellation()
|
||||
{
|
||||
std::lock_guard lock(motion_mutex_);
|
||||
fail_next_torque_on_cancellation_ = true;
|
||||
}
|
||||
|
||||
bool waitForBlockingTorqueOn(
|
||||
const std::chrono::milliseconds timeout)
|
||||
{
|
||||
std::unique_lock lock(motion_mutex_);
|
||||
return torque_on_started_cv_.wait_for(
|
||||
lock,
|
||||
timeout,
|
||||
[this]() { return blocking_torque_on_started_; });
|
||||
}
|
||||
|
||||
void releaseBlockingTorqueOn()
|
||||
{
|
||||
{
|
||||
std::lock_guard lock(motion_mutex_);
|
||||
release_blocking_torque_on_ = true;
|
||||
}
|
||||
torque_on_release_cv_.notify_all();
|
||||
}
|
||||
|
||||
bool waitForBlockingMotion(
|
||||
const std::string& operation,
|
||||
const std::chrono::milliseconds timeout)
|
||||
@ -244,6 +319,24 @@ public:
|
||||
return torque_off_calls_;
|
||||
}
|
||||
|
||||
int torqueOnCalls() const
|
||||
{
|
||||
std::lock_guard lock(motion_mutex_);
|
||||
return torque_on_calls_;
|
||||
}
|
||||
|
||||
bool lastTorqueOnHadCancellation() const
|
||||
{
|
||||
std::lock_guard lock(motion_mutex_);
|
||||
return last_torque_on_had_cancellation_;
|
||||
}
|
||||
|
||||
bool lastTorqueOnCancellationRequested() const
|
||||
{
|
||||
std::lock_guard lock(motion_mutex_);
|
||||
return last_torque_on_cancellation_requested_;
|
||||
}
|
||||
|
||||
bool lastMotionHadCancellation() const
|
||||
{
|
||||
std::lock_guard lock(motion_mutex_);
|
||||
@ -388,6 +481,8 @@ private:
|
||||
std::condition_variable motion_release_cv_;
|
||||
std::condition_variable stop_started_cv_;
|
||||
std::condition_variable stop_release_cv_;
|
||||
std::condition_variable torque_on_started_cv_;
|
||||
std::condition_variable torque_on_release_cv_;
|
||||
bool block_next_motion_{false};
|
||||
bool blocking_motion_started_{false};
|
||||
bool release_blocking_motion_{false};
|
||||
@ -396,11 +491,18 @@ private:
|
||||
bool release_blocking_stop_{false};
|
||||
bool fail_next_stop_{false};
|
||||
bool throw_next_stop_{false};
|
||||
bool block_next_torque_on_{false};
|
||||
bool blocking_torque_on_started_{false};
|
||||
bool release_blocking_torque_on_{false};
|
||||
bool fail_next_torque_on_cancellation_{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};
|
||||
int torque_on_calls_{0};
|
||||
bool last_torque_on_had_cancellation_{false};
|
||||
bool last_torque_on_cancellation_requested_{false};
|
||||
bool last_motion_had_cancellation_{false};
|
||||
bool last_motion_cancellation_requested_{false};
|
||||
};
|
||||
@ -520,6 +622,19 @@ protected:
|
||||
return service_->torqueOff(&context, &request, &response);
|
||||
}
|
||||
|
||||
MoveOutcome torqueOn(const std::string& device_id)
|
||||
{
|
||||
api::CommandHeader_Request request;
|
||||
request.set_device_id(device_id);
|
||||
api::CommandHeader_Feedback response;
|
||||
grpc::ServerContext context;
|
||||
auto status = service_->torqueOn(&context, &request, &response);
|
||||
return {
|
||||
std::move(status),
|
||||
response.success(),
|
||||
response.error_message()};
|
||||
}
|
||||
|
||||
std::shared_ptr<JsonCommandRobotArm> left_arm_;
|
||||
std::shared_ptr<JsonCommandRobotArm> aubo_arm_;
|
||||
std::shared_ptr<JsonCommandNonArmDevice> non_arm_;
|
||||
@ -718,6 +833,166 @@ TEST_F(GrpcArmServiceTest, MoveBindsLeaseRevocationCancellation)
|
||||
EXPECT_FALSE(aubo_arm_->lastMotionCancellationRequested());
|
||||
}
|
||||
|
||||
TEST_F(GrpcArmServiceTest,
|
||||
StopAllCancelsInFlightTorqueOnWithoutReportingSuccess)
|
||||
{
|
||||
aubo_arm_->blockNextTorqueOn();
|
||||
auto blocked_torque_on = std::async(
|
||||
std::launch::async,
|
||||
[this]() { return torqueOn("aubo_arm"); });
|
||||
|
||||
const bool torque_on_started = aubo_arm_->waitForBlockingTorqueOn(
|
||||
std::chrono::seconds(2));
|
||||
auto& admission = globalStopAllAdmissionGate();
|
||||
StopAllAdmissionGate::StopAllTicket stop_all_ticket;
|
||||
if (torque_on_started) {
|
||||
stop_all_ticket = admission.beginStopAll();
|
||||
}
|
||||
|
||||
const bool cancelled_promptly =
|
||||
blocked_torque_on.wait_for(std::chrono::seconds(2)) ==
|
||||
std::future_status::ready;
|
||||
if (!cancelled_promptly) {
|
||||
aubo_arm_->releaseBlockingTorqueOn();
|
||||
}
|
||||
const auto outcome = blocked_torque_on.get();
|
||||
|
||||
ASSERT_TRUE(torque_on_started);
|
||||
ASSERT_TRUE(stop_all_ticket.valid());
|
||||
EXPECT_TRUE(cancelled_promptly);
|
||||
EXPECT_EQ(outcome.status.error_code(), grpc::StatusCode::UNAVAILABLE);
|
||||
EXPECT_FALSE(outcome.response_success);
|
||||
EXPECT_EQ(outcome.response_error, outcome.status.error_message());
|
||||
EXPECT_NE(outcome.response_error.find("StopAll"), std::string::npos);
|
||||
EXPECT_EQ(aubo_arm_->torqueOnCalls(), 1);
|
||||
EXPECT_TRUE(aubo_arm_->lastTorqueOnHadCancellation());
|
||||
EXPECT_TRUE(aubo_arm_->lastTorqueOnCancellationRequested());
|
||||
|
||||
EXPECT_TRUE(admission.finishStopAll(stop_all_ticket, true));
|
||||
}
|
||||
|
||||
TEST_F(GrpcArmServiceTest,
|
||||
StopAllPreservesTorqueOnSafetyTerminationFailure)
|
||||
{
|
||||
aubo_arm_->blockNextTorqueOn();
|
||||
aubo_arm_->failNextTorqueOnCancellation();
|
||||
auto blocked_torque_on = std::async(
|
||||
std::launch::async,
|
||||
[this]() { return torqueOn("aubo_arm"); });
|
||||
|
||||
const bool torque_on_started = aubo_arm_->waitForBlockingTorqueOn(
|
||||
std::chrono::seconds(2));
|
||||
auto& admission = globalStopAllAdmissionGate();
|
||||
StopAllAdmissionGate::StopAllTicket stop_all_ticket;
|
||||
if (torque_on_started) {
|
||||
stop_all_ticket = admission.beginStopAll();
|
||||
}
|
||||
|
||||
const bool completed_promptly =
|
||||
blocked_torque_on.wait_for(std::chrono::seconds(2)) ==
|
||||
std::future_status::ready;
|
||||
if (!completed_promptly) {
|
||||
aubo_arm_->releaseBlockingTorqueOn();
|
||||
}
|
||||
const auto outcome = blocked_torque_on.get();
|
||||
|
||||
ASSERT_TRUE(torque_on_started);
|
||||
ASSERT_TRUE(stop_all_ticket.valid());
|
||||
EXPECT_TRUE(completed_promptly);
|
||||
EXPECT_EQ(outcome.status.error_code(), grpc::StatusCode::INTERNAL);
|
||||
EXPECT_FALSE(outcome.response_success);
|
||||
EXPECT_EQ(
|
||||
outcome.response_error,
|
||||
"simulated torqueOn safety termination failure");
|
||||
EXPECT_EQ(outcome.status.error_message(), outcome.response_error);
|
||||
EXPECT_TRUE(aubo_arm_->lastTorqueOnCancellationRequested());
|
||||
|
||||
EXPECT_FALSE(admission.finishStopAll(stop_all_ticket, false));
|
||||
}
|
||||
|
||||
TEST_F(GrpcArmServiceTest,
|
||||
ClientCancellationPreservesTorqueOnSafetyTerminationFailure)
|
||||
{
|
||||
std::mutex records_mutex;
|
||||
std::condition_variable records_changed;
|
||||
std::vector<GrpcFailureRecord> records;
|
||||
|
||||
grpc::ServerBuilder builder;
|
||||
const std::string socket_path =
|
||||
"/tmp/cmvr_grpc_arm_service_test_" +
|
||||
std::to_string(static_cast<long long>(::getpid())) + ".sock";
|
||||
std::remove(socket_path.c_str());
|
||||
const std::string address = "unix:" + socket_path;
|
||||
builder.AddListeningPort(
|
||||
address,
|
||||
grpc::InsecureServerCredentials());
|
||||
builder.RegisterService(service_.get());
|
||||
std::vector<std::unique_ptr<
|
||||
grpc::experimental::ServerInterceptorFactoryInterface>> factories;
|
||||
factories.emplace_back(makeGrpcErrorLoggingInterceptorFactory(
|
||||
[&records, &records_mutex, &records_changed](
|
||||
const GrpcFailureRecord& record) {
|
||||
{
|
||||
std::lock_guard lock(records_mutex);
|
||||
records.push_back(record);
|
||||
}
|
||||
records_changed.notify_all();
|
||||
}));
|
||||
builder.experimental().SetInterceptorCreators(std::move(factories));
|
||||
auto server = builder.BuildAndStart();
|
||||
ASSERT_NE(server, nullptr);
|
||||
|
||||
const auto channel = grpc::CreateChannel(
|
||||
address,
|
||||
grpc::InsecureChannelCredentials());
|
||||
auto stub = api::ArmService::NewStub(channel);
|
||||
grpc::ClientContext client_context;
|
||||
client_context.set_deadline(
|
||||
std::chrono::system_clock::now() + std::chrono::seconds(3));
|
||||
api::CommandHeader_Request request;
|
||||
request.set_device_id("aubo_arm");
|
||||
api::CommandHeader_Feedback response;
|
||||
grpc::Status client_status;
|
||||
|
||||
aubo_arm_->blockNextTorqueOn();
|
||||
aubo_arm_->failNextTorqueOnCancellation();
|
||||
std::thread client_call([&]() {
|
||||
client_status = stub->torqueOn(
|
||||
&client_context, request, &response);
|
||||
});
|
||||
const bool torque_on_started = aubo_arm_->waitForBlockingTorqueOn(
|
||||
std::chrono::seconds(2));
|
||||
client_context.TryCancel();
|
||||
client_call.join();
|
||||
|
||||
bool failure_recorded = false;
|
||||
{
|
||||
std::unique_lock lock(records_mutex);
|
||||
failure_recorded = records_changed.wait_for(
|
||||
lock,
|
||||
std::chrono::seconds(2),
|
||||
[&records]() { return !records.empty(); });
|
||||
}
|
||||
if (!failure_recorded) {
|
||||
aubo_arm_->releaseBlockingTorqueOn();
|
||||
}
|
||||
server->Shutdown();
|
||||
server->Wait();
|
||||
std::remove(socket_path.c_str());
|
||||
|
||||
ASSERT_TRUE(torque_on_started);
|
||||
EXPECT_EQ(client_status.error_code(), grpc::StatusCode::CANCELLED);
|
||||
ASSERT_TRUE(failure_recorded);
|
||||
ASSERT_EQ(records.size(), 1U);
|
||||
EXPECT_EQ(records.front().kind, GrpcFailureKind::GRPC_STATUS);
|
||||
EXPECT_EQ(records.front().status_code, grpc::StatusCode::INTERNAL);
|
||||
EXPECT_EQ(records.front().severity, GrpcFailureSeverity::ERROR);
|
||||
EXPECT_EQ(
|
||||
records.front().detail,
|
||||
"simulated torqueOn safety termination failure");
|
||||
EXPECT_TRUE(aubo_arm_->lastTorqueOnCancellationRequested());
|
||||
}
|
||||
|
||||
TEST_F(GrpcArmServiceTest,
|
||||
StopAllGateRejectsMutatingCommandsButAllowsReadsAndStops)
|
||||
{
|
||||
|
||||
@ -3024,6 +3024,190 @@ TEST_F(GrpcSystemServiceTest,
|
||||
EXPECT_EQ(active_arm->motionCalls(), 1);
|
||||
}
|
||||
|
||||
TEST_F(GrpcSystemServiceTest,
|
||||
StopAllSharesTimedOutArmStopAndDetailAcrossServiceInstances)
|
||||
{
|
||||
config::DeviceManagerConfig config;
|
||||
auto& manager = device::DeviceManager::getInstance(config);
|
||||
auto trace = std::make_shared<ActionTrace>();
|
||||
auto arm = std::make_shared<ActionTestArm>(
|
||||
"preempted-handler-arm", trace);
|
||||
manager.registerDevice(arm);
|
||||
|
||||
auto& authority = control::ControlAuthorityManager::instance();
|
||||
const auto old_handler = authority.tryAcquire(
|
||||
arm->id(), "blocked-control-handler", std::chrono::seconds(30));
|
||||
ASSERT_TRUE(old_handler.acquired) << old_handler.detail;
|
||||
service_ = std::make_unique<gRPCSystemServiceImpl>(
|
||||
std::chrono::milliseconds(300));
|
||||
|
||||
api::StopAllCommand_Request request;
|
||||
auto stop_all = std::async(std::launch::async, [this, &request] {
|
||||
api::StopAllCommand_Feedback response;
|
||||
grpc::ServerContext context;
|
||||
const auto status = service_->StopAll(
|
||||
&context, &request, &response);
|
||||
return std::make_pair(status, response);
|
||||
});
|
||||
|
||||
const bool initial_stop_completed = arm->waitForStopMotionCalls(
|
||||
1, std::chrono::milliseconds(500));
|
||||
if (initial_stop_completed) {
|
||||
arm->blockNextStopMotion();
|
||||
}
|
||||
const auto completion = stop_all.wait_for(std::chrono::seconds(1));
|
||||
const bool final_stop_started = initial_stop_completed &&
|
||||
arm->waitForBlockedStopMotion(std::chrono::milliseconds(500));
|
||||
|
||||
std::optional<std::pair<grpc::Status, api::StopAllCommand_Feedback>>
|
||||
result;
|
||||
std::optional<std::pair<grpc::Status, api::StopAllCommand_Feedback>>
|
||||
second_result;
|
||||
std::unique_ptr<gRPCSystemServiceImpl> second_service;
|
||||
std::promise<void> last_destroy_started;
|
||||
auto last_destroy_started_signal = last_destroy_started.get_future();
|
||||
std::promise<void> replacement_construct_started;
|
||||
auto replacement_construct_started_signal =
|
||||
replacement_construct_started.get_future();
|
||||
std::future<void> first_service_destroy;
|
||||
std::future<void> last_service_destroy;
|
||||
std::future<std::unique_ptr<gRPCSystemServiceImpl>>
|
||||
replacement_service_construct;
|
||||
std::optional<std::future_status> first_destroy_before_release;
|
||||
std::optional<std::future_status> last_destroy_before_release;
|
||||
std::optional<std::future_status> last_destroy_after_release;
|
||||
std::optional<std::future_status> replacement_before_release;
|
||||
std::optional<std::future_status> replacement_after_release;
|
||||
std::unique_ptr<gRPCSystemServiceImpl> replacement_service;
|
||||
bool dispatcher_destruction_observed{false};
|
||||
int stop_calls_before_second{-1};
|
||||
int stop_calls_after_second{-1};
|
||||
control::ControlAcquireResult control_while_failed_closed;
|
||||
if (completion == std::future_status::ready) {
|
||||
result = stop_all.get();
|
||||
stop_calls_before_second = arm->stopMotionCalls();
|
||||
second_service = std::make_unique<gRPCSystemServiceImpl>(
|
||||
std::chrono::milliseconds(300));
|
||||
api::StopAllCommand_Feedback second_response;
|
||||
grpc::ServerContext second_context;
|
||||
const auto second_status = second_service->StopAll(
|
||||
&second_context, &request, &second_response);
|
||||
second_result.emplace(second_status, std::move(second_response));
|
||||
stop_calls_after_second = arm->stopMotionCalls();
|
||||
control_while_failed_closed = authority.tryAcquire(
|
||||
arm->id(), "control-after-timed-out-stop-all",
|
||||
std::chrono::seconds(30));
|
||||
first_service_destroy = std::async(
|
||||
std::launch::async,
|
||||
[this] { service_.reset(); });
|
||||
first_destroy_before_release =
|
||||
first_service_destroy.wait_for(std::chrono::seconds(1));
|
||||
if (*first_destroy_before_release == std::future_status::ready) {
|
||||
first_service_destroy.get();
|
||||
last_service_destroy = std::async(
|
||||
std::launch::async,
|
||||
[&second_service, &last_destroy_started] {
|
||||
last_destroy_started.set_value();
|
||||
second_service.reset();
|
||||
});
|
||||
last_destroy_started_signal.wait();
|
||||
dispatcher_destruction_observed =
|
||||
gRPCSystemServiceImpl::
|
||||
waitForStopDispatcherDestructionForTesting(
|
||||
std::chrono::seconds(1));
|
||||
if (dispatcher_destruction_observed) {
|
||||
last_destroy_before_release =
|
||||
last_service_destroy.wait_for(
|
||||
std::chrono::milliseconds::zero());
|
||||
replacement_service_construct = std::async(
|
||||
std::launch::async,
|
||||
[&replacement_construct_started] {
|
||||
replacement_construct_started.set_value();
|
||||
return std::make_unique<gRPCSystemServiceImpl>(
|
||||
std::chrono::milliseconds(300));
|
||||
});
|
||||
replacement_construct_started_signal.wait();
|
||||
replacement_before_release =
|
||||
replacement_service_construct.wait_for(
|
||||
std::chrono::milliseconds(100));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Always release both test blocks before an assertion can abort the test;
|
||||
// the dispatcher owns the final stop worker past the RPC deadline.
|
||||
arm->releaseBlockedStopMotion();
|
||||
authority.release(old_handler.token);
|
||||
if (first_service_destroy.valid()) {
|
||||
if (first_service_destroy.wait_for(std::chrono::seconds(1)) ==
|
||||
std::future_status::ready) {
|
||||
first_service_destroy.get();
|
||||
}
|
||||
}
|
||||
if (last_service_destroy.valid()) {
|
||||
last_destroy_after_release =
|
||||
last_service_destroy.wait_for(std::chrono::seconds(1));
|
||||
if (*last_destroy_after_release == std::future_status::ready) {
|
||||
last_service_destroy.get();
|
||||
}
|
||||
}
|
||||
if (replacement_service_construct.valid()) {
|
||||
replacement_after_release =
|
||||
replacement_service_construct.wait_for(std::chrono::seconds(1));
|
||||
if (*replacement_after_release == std::future_status::ready) {
|
||||
replacement_service = replacement_service_construct.get();
|
||||
}
|
||||
}
|
||||
replacement_service.reset();
|
||||
|
||||
EXPECT_TRUE(initial_stop_completed);
|
||||
EXPECT_TRUE(final_stop_started);
|
||||
ASSERT_EQ(completion, std::future_status::ready);
|
||||
ASSERT_TRUE(result.has_value());
|
||||
ASSERT_TRUE(second_result.has_value());
|
||||
const auto& [status, response] = *result;
|
||||
const auto& [second_status, second_response] = *second_result;
|
||||
ASSERT_TRUE(status.ok()) << status.error_message();
|
||||
EXPECT_FALSE(response.header().success());
|
||||
EXPECT_NE(
|
||||
response.header().error_message().find(
|
||||
"timed out waiting for the preempted RobotArm control handler "
|
||||
"to exit"),
|
||||
std::string::npos);
|
||||
EXPECT_EQ(
|
||||
response.header().error_message().find(
|
||||
"stop operation did not complete before the deadline"),
|
||||
std::string::npos);
|
||||
ASSERT_TRUE(second_status.ok()) << second_status.error_message();
|
||||
EXPECT_FALSE(second_response.header().success());
|
||||
EXPECT_NE(
|
||||
second_response.header().error_message().find(
|
||||
"timed out waiting for the preempted RobotArm control handler "
|
||||
"to exit"),
|
||||
std::string::npos);
|
||||
EXPECT_EQ(
|
||||
second_response.header().error_message().find(
|
||||
"stop operation did not complete before the deadline"),
|
||||
std::string::npos);
|
||||
EXPECT_EQ(stop_calls_before_second, 2);
|
||||
EXPECT_EQ(stop_calls_after_second, stop_calls_before_second);
|
||||
EXPECT_FALSE(control_while_failed_closed.acquired);
|
||||
ASSERT_TRUE(first_destroy_before_release.has_value());
|
||||
EXPECT_EQ(
|
||||
*first_destroy_before_release, std::future_status::ready);
|
||||
EXPECT_TRUE(dispatcher_destruction_observed);
|
||||
ASSERT_TRUE(last_destroy_before_release.has_value());
|
||||
ASSERT_TRUE(last_destroy_after_release.has_value());
|
||||
ASSERT_TRUE(replacement_before_release.has_value());
|
||||
ASSERT_TRUE(replacement_after_release.has_value());
|
||||
EXPECT_EQ(
|
||||
*last_destroy_before_release, std::future_status::timeout);
|
||||
EXPECT_EQ(*last_destroy_after_release, std::future_status::ready);
|
||||
EXPECT_EQ(
|
||||
*replacement_before_release, std::future_status::timeout);
|
||||
EXPECT_EQ(*replacement_after_release, std::future_status::ready);
|
||||
}
|
||||
|
||||
TEST_F(GrpcSystemServiceTest,
|
||||
StopAllDoesNotStopDeviceLifecyclesAndRevokesOnlyArmLease)
|
||||
{
|
||||
|
||||
@ -36,6 +36,7 @@ public:
|
||||
};
|
||||
|
||||
using Operation = std::function<OperationResult()>;
|
||||
using TimeoutDetailProvider = std::function<std::string()>;
|
||||
|
||||
class Handle final {
|
||||
public:
|
||||
@ -62,8 +63,15 @@ public:
|
||||
|
||||
// A running job is shared by every submission for the same resource key.
|
||||
// Once it has completed, the next submission joins/reaps the old worker
|
||||
// and starts a new job. Empty keys or operations return an invalid handle.
|
||||
// and starts a new job. A timeout provider belongs to the job that is
|
||||
// actually created; submissions joining a running job retain that job's
|
||||
// provider. Multiple handles may call the provider concurrently, so it
|
||||
// must be thread-safe. Empty keys or operations return an invalid handle.
|
||||
Handle submit(std::string resource_key, Operation operation);
|
||||
Handle submit(
|
||||
std::string resource_key,
|
||||
Operation operation,
|
||||
TimeoutDetailProvider timeout_detail_provider);
|
||||
|
||||
// Exposed only to verify dispatcher lifecycle behavior in tests.
|
||||
std::size_t jobCountForTesting() const;
|
||||
|
||||
@ -12,10 +12,16 @@
|
||||
namespace cmvr::service {
|
||||
|
||||
struct StopOperationDispatcher::JobState final {
|
||||
explicit JobState(TimeoutDetailProvider provider)
|
||||
: timeout_detail_provider(std::move(provider))
|
||||
{
|
||||
}
|
||||
|
||||
std::mutex mutex;
|
||||
std::condition_variable condition;
|
||||
bool completed{false};
|
||||
OperationResult outcome;
|
||||
const TimeoutDetailProvider timeout_detail_provider;
|
||||
};
|
||||
|
||||
struct StopOperationDispatcher::Impl final {
|
||||
@ -50,9 +56,18 @@ StopOperationDispatcher::Handle::waitUntil(const Deadline deadline) const
|
||||
if (!state_->condition.wait_until(lock, deadline, [this] {
|
||||
return state_->completed;
|
||||
})) {
|
||||
return {
|
||||
false,
|
||||
false,
|
||||
lock.unlock();
|
||||
if (state_->timeout_detail_provider) {
|
||||
try {
|
||||
auto detail = state_->timeout_detail_provider();
|
||||
if (!detail.empty()) {
|
||||
return {false, false, std::move(detail)};
|
||||
}
|
||||
} catch (...) {
|
||||
// Diagnostics must not change timeout behavior.
|
||||
}
|
||||
}
|
||||
return {false, false,
|
||||
"stop operation did not complete before the deadline"};
|
||||
}
|
||||
|
||||
@ -85,6 +100,14 @@ StopOperationDispatcher::~StopOperationDispatcher()
|
||||
StopOperationDispatcher::Handle StopOperationDispatcher::submit(
|
||||
std::string resource_key,
|
||||
Operation operation)
|
||||
{
|
||||
return submit(std::move(resource_key), std::move(operation), {});
|
||||
}
|
||||
|
||||
StopOperationDispatcher::Handle StopOperationDispatcher::submit(
|
||||
std::string resource_key,
|
||||
Operation operation,
|
||||
TimeoutDetailProvider timeout_detail_provider)
|
||||
{
|
||||
if (resource_key.empty() || !operation) {
|
||||
return {};
|
||||
@ -141,7 +164,8 @@ StopOperationDispatcher::Handle StopOperationDispatcher::submit(
|
||||
continue;
|
||||
}
|
||||
|
||||
auto state = std::make_shared<JobState>();
|
||||
auto state = std::make_shared<JobState>(
|
||||
std::move(timeout_detail_provider));
|
||||
const auto inserted = impl_->jobs.emplace(
|
||||
std::piecewise_construct,
|
||||
std::forward_as_tuple(std::move(resource_key)),
|
||||
|
||||
@ -78,6 +78,88 @@ TEST(StopOperationDispatcherTest, TimeoutDoesNotCancelTheJob)
|
||||
EXPECT_EQ(completed.detail, "stopped");
|
||||
}
|
||||
|
||||
TEST(StopOperationDispatcherTest,
|
||||
RunningJobKeepsItsOriginalTimeoutDetailProvider)
|
||||
{
|
||||
StopOperationDispatcher dispatcher;
|
||||
std::promise<void> release;
|
||||
auto released = release.get_future().share();
|
||||
std::atomic<int> first_operation_calls{0};
|
||||
std::atomic<int> duplicate_operation_calls{0};
|
||||
std::atomic<int> first_provider_calls{0};
|
||||
std::atomic<int> duplicate_provider_calls{0};
|
||||
|
||||
const auto first = dispatcher.submit(
|
||||
"arm:one",
|
||||
[&] {
|
||||
++first_operation_calls;
|
||||
released.wait();
|
||||
return StopOperationDispatcher::OperationResult{true, {}};
|
||||
},
|
||||
[&] {
|
||||
++first_provider_calls;
|
||||
return std::string("first job is waiting for its handler");
|
||||
});
|
||||
const auto duplicate = dispatcher.submit(
|
||||
"arm:one",
|
||||
[&] {
|
||||
++duplicate_operation_calls;
|
||||
return StopOperationDispatcher::OperationResult{true, {}};
|
||||
},
|
||||
[&] {
|
||||
++duplicate_provider_calls;
|
||||
return std::string("duplicate submission detail");
|
||||
});
|
||||
|
||||
const auto first_timeout = first.waitUntil(
|
||||
StopOperationDispatcher::Clock::now() + 20ms);
|
||||
const auto duplicate_timeout = duplicate.waitUntil(
|
||||
StopOperationDispatcher::Clock::now() + 20ms);
|
||||
release.set_value();
|
||||
const auto completed = first.waitUntil(
|
||||
StopOperationDispatcher::Clock::now() + 1s);
|
||||
|
||||
EXPECT_FALSE(first_timeout.completed);
|
||||
EXPECT_EQ(
|
||||
first_timeout.detail, "first job is waiting for its handler");
|
||||
EXPECT_FALSE(duplicate_timeout.completed);
|
||||
EXPECT_EQ(
|
||||
duplicate_timeout.detail, "first job is waiting for its handler");
|
||||
EXPECT_TRUE(completed.completed);
|
||||
EXPECT_TRUE(completed.result);
|
||||
EXPECT_EQ(first_operation_calls.load(), 1);
|
||||
EXPECT_EQ(duplicate_operation_calls.load(), 0);
|
||||
EXPECT_EQ(first_provider_calls.load(), 2);
|
||||
EXPECT_EQ(duplicate_provider_calls.load(), 0);
|
||||
}
|
||||
|
||||
TEST(StopOperationDispatcherTest,
|
||||
ThrowingTimeoutDetailProviderFallsBackToGenericDetail)
|
||||
{
|
||||
StopOperationDispatcher dispatcher;
|
||||
std::promise<void> release;
|
||||
auto released = release.get_future().share();
|
||||
const auto handle = dispatcher.submit(
|
||||
"arm:one",
|
||||
[released] {
|
||||
released.wait();
|
||||
return StopOperationDispatcher::OperationResult{true, {}};
|
||||
},
|
||||
[]() -> std::string {
|
||||
throw std::runtime_error("diagnostic provider failed");
|
||||
});
|
||||
|
||||
const auto timed_out = handle.waitUntil(
|
||||
StopOperationDispatcher::Clock::now() + 20ms);
|
||||
release.set_value();
|
||||
|
||||
EXPECT_FALSE(timed_out.completed);
|
||||
EXPECT_FALSE(timed_out.result);
|
||||
EXPECT_EQ(
|
||||
timed_out.detail,
|
||||
"stop operation did not complete before the deadline");
|
||||
}
|
||||
|
||||
TEST(StopOperationDispatcherTest, RunningSubmissionsForAKeyShareOneJob)
|
||||
{
|
||||
StopOperationDispatcher dispatcher;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user