From 3b87f681cf1e909a5c839ce3c0e8ae0e7101cd68 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 7 Aug 2026 13:35:30 +0800 Subject: [PATCH] fix(aubo): prevent resume after hardware e-stop --- cmvr-es/devices/arm/aubo_arm/CMakeLists.txt | 16 + cmvr-es/devices/arm/aubo_arm/README.md | 20 +- cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp | 1743 ++++++++++++++++- cmvr-es/devices/arm/aubo_arm/aubo_arm.h | 25 +- .../devices/arm/aubo_arm/aubo_motion_state.h | 31 + .../devices/arm/aubo_arm/aubo_safety_state.h | 184 ++ .../aubo_arm/tests/aubo_motion_state_test.cpp | 29 + .../aubo_arm/tests/aubo_safety_state_test.cpp | 88 + 8 files changed, 2075 insertions(+), 61 deletions(-) create mode 100644 cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h create mode 100644 cmvr-es/devices/arm/aubo_arm/tests/aubo_safety_state_test.cpp diff --git a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt index aee5d72a..d52da874 100644 --- a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt @@ -2,6 +2,8 @@ add_library(aubo_arm SHARED aubo_arm.cpp ) +find_package(Threads REQUIRED) + target_include_directories(aubo_arm PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) set(AUBO_SDK_ROOT ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/aubo_sdk/v0.27.1) @@ -47,6 +49,7 @@ target_link_libraries(aubo_arm PRIVATE glog jsoncpp + Threads::Threads ) add_library(cmvr_es::device::aubo_arm ALIAS aubo_arm) @@ -80,6 +83,19 @@ if(BUILD_TESTING) ) set_tests_properties(aubo_motion_state_test PROPERTIES TIMEOUT 10) + add_executable(aubo_safety_state_test + tests/aubo_safety_state_test.cpp + ) + target_include_directories(aubo_safety_state_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + add_test( + NAME aubo_safety_state_test + COMMAND aubo_safety_state_test + ) + set_tests_properties(aubo_safety_state_test PROPERTIES TIMEOUT 10) + add_executable(aubo_arm_json_command_test tests/aubo_arm_json_command_test.cpp ) diff --git a/cmvr-es/devices/arm/aubo_arm/README.md b/cmvr-es/devices/arm/aubo_arm/README.md index 0045108b..05e6d903 100644 --- a/cmvr-es/devices/arm/aubo_arm/README.md +++ b/cmvr-es/devices/arm/aubo_arm/README.md @@ -103,6 +103,19 @@ cmake --install build ## 安全与语义边界 +- 后端使用独立 SDK RPC 会话持续读取控制器的 `SafetyModeType`、 + `RobotModeType` 和硬件急停来源；首次有效样本前、监控断线或样本过期时， + 所有 Move、Speed、Servo 和程序启动请求均按不安全状态拒绝； +- 硬件急停、防护停机、Safety Fault/Violation 会锁存安全事件，并使当前运动 + generation 失效。控制器重新报告 `Normal`/`ReducedMode` 不会自动解除锁存； +- 锁存后会终止直接运动与程序、关闭 servo 模式并清理控制器轨迹。只有确认 + `ExecId == -1`、普通队列和轨迹队列均为空、运行时已停止且机械臂稳定后， + 显式 `torqueOn`/`clearFault`/`unlockProtectiveStop` 才可能恢复运动权限； +- 恢复流程不会调用 `resume`、`arbitraryResume`、`startMove`，也不会重新提交 + 急停前的目标、速度、servo 指令或程序； +- AUBO SDK 未在本地文档中保证急停期间 `clearPath` 的可用性，也未说明释放 + 急停开关后的控制器恢复时序。因此本实现保持 fail-closed 并在释放后再次清队列， + 但“释放开关后零位移”的最终保证仍需真机验证及控制器侧安全配置配合； - 只访问控制柜 Standard 数字 IO，不访问工具端 IO、可配置 IO 或安全 IO； - `set_do` 不修改输出 runstate； - 只有 `StandardOutputRunState::None` 的通道允许写入，否则返回 @@ -116,9 +129,12 @@ cmake --install build ## 测试 ```bash -cmake --build build --target aubo_arm_json_command_test -j4 +cmake --build build --target \ + aubo_safety_state_test \ + aubo_motion_state_test \ + aubo_arm_json_command_test -j4 ctest --test-dir build \ - -R '^aubo_arm_json_command_test$' \ + -R 'aubo_(safety_state|motion_state|arm_json_command)_test' \ --output-on-failure ``` diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index e2936689..ed446d4f 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -2,14 +2,18 @@ #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 #include #include +#include #include #include +#include #include #include +#include #include "common/base/logging/logger.h" #include "json/json.h" @@ -115,6 +119,51 @@ private: bool completed_{false}; }; +class SafetyRecoveryGuard final { +public: + SafetyRecoveryGuard( + std::shared_ptr state, + const aubo_internal::RecoveryToken token) + : state_(std::move(state)), token_(token) + { + } + + ~SafetyRecoveryGuard() noexcept + { + if (!completed_ && state_) { + try { + state_->failRecovery(token_); + } catch (...) { + // The safety latch itself remains set on every failure path. + } + } + } + + SafetyRecoveryGuard(const SafetyRecoveryGuard&) = delete; + SafetyRecoveryGuard& operator=(const SafetyRecoveryGuard&) = delete; + + bool complete( + const bool robot_running, + const bool controller_idle, + const bool cancellation_confirmed) + { + if (!state_->completeRecovery( + token_, + robot_running, + controller_idle, + cancellation_confirmed)) { + return false; + } + completed_ = true; + return true; + } + +private: + std::shared_ptr state_; + aubo_internal::RecoveryToken token_; + bool completed_{false}; +}; + const char* motionStartFailure( const aubo_internal::MotionStartStatus status) noexcept { @@ -168,9 +217,664 @@ std::string vendorBrandName(const config::VendorRobotArmBrand brand) } using arcs::common_interface::RobotModeType; +using arcs::common_interface::RuntimeState; +using arcs::common_interface::SafetyModeType; using arcs::aubo_sdk::RobotInterfacePtr; +RobotInterfacePtr getPrimaryRobotInterface( + const std::shared_ptr& rpc_client, + const std::string& context, + Result& result); + constexpr int kAuboServoMode = 3; +constexpr auto kSafetyPollInterval = std::chrono::milliseconds(50); +constexpr auto kSafetyReconnectInterval = std::chrono::milliseconds(250); +constexpr auto kSafetySampleMaxAge = std::chrono::milliseconds(500); + +std::int64_t monotonicNowNs() noexcept +{ + return std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + +aubo_internal::SafetyCondition safetyConditionFromSdk( + const SafetyModeType mode) noexcept +{ + using Condition = aubo_internal::SafetyCondition; + switch (mode) { + case SafetyModeType::Normal: + return Condition::Normal; + case SafetyModeType::ReducedMode: + return Condition::Reduced; + case SafetyModeType::Recovery: + return Condition::Recovery; + case SafetyModeType::Violation: + return Condition::Violation; + case SafetyModeType::ProtectiveStop: + return Condition::ProtectiveStop; + case SafetyModeType::SafeguardStop: + return Condition::SafeguardStop; + case SafetyModeType::SystemEmergencyStop: + return Condition::SystemEmergencyStop; + case SafetyModeType::RobotEmergencyStop: + return Condition::RobotEmergencyStop; + case SafetyModeType::Fault: + return Condition::Fault; + case SafetyModeType::Undefined: + break; + } + return Condition::Unknown; +} + +const char* safetyConditionName( + const aubo_internal::SafetyCondition condition) noexcept +{ + using Condition = aubo_internal::SafetyCondition; + switch (condition) { + case Condition::Normal: + return "Normal"; + case Condition::Reduced: + return "Reduced"; + case Condition::Recovery: + return "Recovery"; + case Condition::Violation: + return "Violation"; + case Condition::ProtectiveStop: + return "ProtectiveStop"; + case Condition::SafeguardStop: + return "SafeguardStop"; + case Condition::SystemEmergencyStop: + return "SystemEmergencyStop"; + case Condition::RobotEmergencyStop: + return "RobotEmergencyStop"; + case Condition::Fault: + return "Fault"; + case Condition::Unknown: + break; + } + return "Unknown"; +} + +SafetyMode publicSafetyMode( + const aubo_internal::SafetyCondition condition) noexcept +{ + using Condition = aubo_internal::SafetyCondition; + switch (condition) { + case Condition::Normal: + return SafetyMode::Normal; + case Condition::Reduced: + return SafetyMode::Reduced; + case Condition::ProtectiveStop: + return SafetyMode::ProtectiveStop; + case Condition::SafeguardStop: + return SafetyMode::SafeguardStop; + case Condition::SystemEmergencyStop: + return SafetyMode::SystemEmergencyStop; + case Condition::RobotEmergencyStop: + return SafetyMode::EmergencyStop; + case Condition::Violation: + case Condition::Fault: + return SafetyMode::Fault; + case Condition::Recovery: + case Condition::Unknown: + break; + } + return SafetyMode::Unknown; +} + +RobotMode publicRobotMode(const RobotModeType mode) noexcept +{ + switch (mode) { + case RobotModeType::NoController: + case RobotModeType::Disconnected: + return RobotMode::Disconnected; + case RobotModeType::PowerOff: + case RobotModeType::PowerOffing: + return RobotMode::PowerOff; + case RobotModeType::Running: + return RobotMode::Running; + case RobotModeType::Error: + return RobotMode::Fault; + case RobotModeType::PowerOn: + case RobotModeType::Idle: + case RobotModeType::BrakeReleasing: + case RobotModeType::BackDrive: + return RobotMode::Idle; + case RobotModeType::ConfirmSafety: + case RobotModeType::Booting: + case RobotModeType::Maintaince: + break; + } + return RobotMode::Unknown; +} + +struct AuboSafetyMonitor final { + std::shared_ptr safety_state{ + std::make_shared()}; + std::shared_ptr motion_state; + std::atomic safety_mode{ + static_cast(SafetyModeType::Undefined)}; + std::atomic robot_mode{ + static_cast(RobotModeType::Disconnected)}; + std::atomic runtime_state{ + static_cast(RuntimeState::Stopped)}; + std::atomic emergency_stop_source{-1}; + std::atomic servo_mode_select{0}; + std::atomic last_sample_ns{0}; + std::atomic cancellation_confirmed{true}; + std::atomic runtime_abort_required{false}; + std::atomic servo_disable_required{false}; + std::atomic path_clear_required{false}; + std::atomic stop_requested{false}; + std::mutex wait_mutex; + std::condition_variable wait_cv; + std::mutex termination_mutex; + std::recursive_mutex command_rpc_mutex; + std::string arm_id; +}; + +std::shared_ptr makeRpcClient() +{ + return std::shared_ptr( + ::createRpcClient(), + [](arcs::aubo_sdk::RpcClient* client) { + if (client) { + ::destroyRpcClient(client); + } + }); +} + +bool monitorWait( + const std::shared_ptr& monitor, + const std::chrono::milliseconds duration) +{ + std::unique_lock lock(monitor->wait_mutex); + return monitor->wait_cv.wait_for( + lock, + duration, + [&monitor]() { return monitor->stop_requested.load(); }); +} + +void cancelForSafetyTransition( + const std::shared_ptr& monitor) +{ + monitor->cancellation_confirmed.store(false); + if (monitor->runtime_state.load() != + static_cast(RuntimeState::Stopped)) { + monitor->runtime_abort_required.store(true); + } + if (monitor->servo_mode_select.load() != 0) { + monitor->servo_disable_required.store(true); + } + monitor->motion_state->cancelActiveForSafety(); +} + +void publishSafetySample( + const std::shared_ptr& monitor, + const SafetyModeType safety_mode, + const RobotModeType robot_mode, + const RuntimeState runtime_state, + const int emergency_stop_source, + const int servo_mode_select) +{ + const auto previous = monitor->safety_state->snapshot(); + const int previous_runtime_state = monitor->runtime_state.load(); + const int previous_servo_mode = monitor->servo_mode_select.load(); + const auto condition = aubo_internal::effectiveSafetyCondition( + safetyConditionFromSdk(safety_mode), emergency_stop_source); + monitor->safety_state->observe(condition); + const auto current = monitor->safety_state->snapshot(); + + monitor->safety_mode.store(static_cast(safety_mode)); + monitor->robot_mode.store(static_cast(robot_mode)); + monitor->runtime_state.store(static_cast(runtime_state)); + monitor->emergency_stop_source.store(emergency_stop_source); + monitor->servo_mode_select.store(servo_mode_select); + monitor->last_sample_ns.store(monotonicNowNs()); + + if (previous.observed != condition || + (!previous.latched && current.latched)) { + if (current.latched) { + if (previous_runtime_state != + static_cast(RuntimeState::Stopped) || + runtime_state != RuntimeState::Stopped) { + monitor->runtime_abort_required.store(true); + } + if (previous_servo_mode != 0 || servo_mode_select != 0) { + monitor->servo_disable_required.store(true); + } + cancelForSafetyTransition(monitor); + } + if (current.latched) { + CMVR_LOG(WARNING) + << "[AuboArm] safety state changed, id=" << monitor->arm_id + << ", state=" << safetyConditionName(condition) + << ", latched=true"; + } else { + CMVR_LOG(INFO) + << "[AuboArm] safety state changed, id=" << monitor->arm_id + << ", state=" << safetyConditionName(condition) + << ", latched=false"; + } + } +} + +void refreshSafetySample( + const std::shared_ptr& rpc_client, + const std::shared_ptr& monitor, + const RobotInterfacePtr& robot_interface) +{ + auto robot_state = robot_interface->getRobotState(); + publishSafetySample( + monitor, + robot_state->getSafetyModeType(), + robot_state->getRobotModeType(), + rpc_client->getRuntimeMachine()->getRuntimeState(), + robot_interface->getRobotConfig() + ->getRobotEmergencyStopSource(), + robot_interface->getMotionControl()->getServoModeSelect()); +} + +void publishSafetyUnavailable( + const std::shared_ptr& monitor, + const std::string& reason) +{ + const auto previous = monitor->safety_state->snapshot(); + monitor->safety_state->observe( + aubo_internal::SafetyCondition::Unknown); + monitor->safety_mode.store( + static_cast(SafetyModeType::Undefined)); + monitor->robot_mode.store( + static_cast(RobotModeType::Disconnected)); + monitor->emergency_stop_source.store(-1); + monitor->last_sample_ns.store(0); + if (previous.observed != aubo_internal::SafetyCondition::Unknown || + !previous.latched) { + cancelForSafetyTransition(monitor); + CMVR_LOG(WARNING) + << "[AuboArm] safety monitor unavailable, id=" + << monitor->arm_id << ", reason=" << reason; + } +} + +bool safetySampleFresh( + const std::shared_ptr& monitor) noexcept +{ + const auto sample_ns = monitor->last_sample_ns.load(); + if (sample_ns <= 0) { + return false; + } + const auto age_ns = monotonicNowNs() - sample_ns; + return age_ns >= 0 && + age_ns <= std::chrono::duration_cast( + kSafetySampleMaxAge) + .count(); +} + +bool validateSafetyPermit( + const std::shared_ptr& monitor, + const aubo_internal::SafetyPermit permit) +{ + if (!safetySampleFresh(monitor)) { + publishSafetyUnavailable(monitor, "sample is stale"); + return false; + } + return monitor->emergency_stop_source.load() == 0 && + monitor->robot_mode.load() == + static_cast(RobotModeType::Running) && + monitor->safety_state->validate(permit); +} + +bool enforceControllerTermination( + const std::shared_ptr& rpc_client, + const std::shared_ptr& monitor) +{ + std::unique_lock termination_lock(monitor->termination_mutex); + monitor->motion_state->cancelActiveForSafety(); + const auto stop_request = monitor->motion_state->beginStop(); + if (!stop_request.started()) { + return monitor->cancellation_confirmed.load(); + } + + const auto fail = [&monitor]() { + monitor->motion_state->failStop(); + monitor->cancellation_confirmed.store(false); + return false; + }; + + try { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface( + rpc_client, "safety termination", interface_result); + if (!interface_result.ok()) { + return fail(); + } + auto motion_control = robot_interface->getMotionControl(); + auto robot_state = robot_interface->getRobotState(); + auto runtime = rpc_client->getRuntimeMachine(); + + constexpr auto kTerminationTimeout = std::chrono::seconds(2); + constexpr int kStableSamples = 3; + const auto deadline = + std::chrono::steady_clock::now() + kTerminationTimeout; + int stable_samples = 0; + int iteration = 0; + bool typed_stop_acknowledged = + !stop_request.tracked_motion; + while (!monitor->stop_requested.load() && + std::chrono::steady_clock::now() < deadline) { + const int exec_id = motion_control->getExecId(); + const bool steady = robot_state->isSteady(); + int queue_size = motion_control->getQueueSize(); + int trajectory_queue_size = + motion_control->getTrajectoryQueueSize(); + int servo_mode = motion_control->getServoModeSelect(); + auto runtime_state = runtime->getRuntimeState(); + + if (runtime_state != RuntimeState::Stopped) { + monitor->runtime_abort_required.store(true); + } + if (servo_mode != 0) { + monitor->servo_disable_required.store(true); + } + if (queue_size != 0 || trajectory_queue_size != 0) { + monitor->path_clear_required.store(true); + } + + if (monitor->runtime_abort_required.load() && + iteration % 4 == 0 && + runtime->abort() == arcs::common_interface::AUBO_OK) { + monitor->runtime_abort_required.store(false); + } + if (monitor->servo_disable_required.load() && + iteration % 4 == 0 && + motion_control->setServoModeSelect(0) == + arcs::common_interface::AUBO_OK) { + monitor->servo_disable_required.store(false); + } + + const bool controller_moving = exec_id != -1 || !steady; + if ((stop_request.tracked_motion || controller_moving) && + iteration % 4 == 0) { + int stop_ret = arcs::common_interface::AUBO_OK; + if (stop_request.kind == aubo_internal::MotionKind::Joint) { + stop_ret = motion_control->stopJoint(31.0); + } else if (stop_request.kind == + aubo_internal::MotionKind::Linear) { + stop_ret = motion_control->stopLine(10.0, 10.0); + } else { + // RuntimeMachine::abort() is the only typed-independent + // SDK primitive documented to stop arbitrary operation. + monitor->runtime_abort_required.store(true); + stop_ret = runtime->abort(); + if (stop_ret == arcs::common_interface::AUBO_OK) { + monitor->runtime_abort_required.store(false); + } + } + if (stop_request.kind != + aubo_internal::MotionKind::None && + stop_ret == arcs::common_interface::AUBO_OK) { + typed_stop_acknowledged = true; + } + } + + if (monitor->path_clear_required.load() && + iteration % 4 == 0 && + motion_control->clearPath() == + arcs::common_interface::AUBO_OK) { + monitor->path_clear_required.store(false); + } + + queue_size = motion_control->getQueueSize(); + trajectory_queue_size = + motion_control->getTrajectoryQueueSize(); + servo_mode = motion_control->getServoModeSelect(); + runtime_state = runtime->getRuntimeState(); + const bool owner_active = monitor->motion_state->ownerActive( + stop_request.active_token); + const bool idle = + motion_control->getExecId() == -1 && + robot_state->isSteady() && + queue_size == 0 && + trajectory_queue_size == 0 && servo_mode == 0 && + runtime_state == RuntimeState::Stopped && !owner_active && + typed_stop_acknowledged && + !monitor->runtime_abort_required.load() && + !monitor->servo_disable_required.load() && + !monitor->path_clear_required.load() && + aubo_internal::isMotionSafe( + monitor->safety_state->snapshot().observed) && + monitor->emergency_stop_source.load() == 0; + + if (idle) { + if (++stable_samples >= kStableSamples) { + if (!monitor->motion_state->completeStop()) { + return fail(); + } + monitor->servo_mode_select.store(0); + monitor->runtime_state.store( + static_cast(RuntimeState::Stopped)); + monitor->cancellation_confirmed.store(true); + CMVR_LOG(INFO) + << "[AuboArm] safety termination confirmed, id=" + << monitor->arm_id; + return true; + } + } else { + stable_samples = 0; + } + + ++iteration; + if (monitorWait(monitor, kSafetyPollInterval)) { + break; + } + } + } catch (const std::exception& e) { + CMVR_LOG(WARNING) + << "[AuboArm] safety termination attempt failed, id=" + << monitor->arm_id << ", error=" << e.what(); + } + return fail(); +} + +// Powering on to Idle keeps the brakes engaged. This pre-startup phase clears +// the controller queues without completing MotionState, so the retained +// Joint/Linear kind survives until a typed stop is acknowledged in Running. +bool prepareControllerForStartup( + const std::shared_ptr& rpc_client, + const std::shared_ptr& monitor) +{ + std::unique_lock termination_lock(monitor->termination_mutex); + const auto cancellation = + monitor->motion_state->cancelActiveForSafety(); + + try { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface( + rpc_client, "pre-startup safety cleanup", interface_result); + if (!interface_result.ok()) { + return false; + } + auto motion_control = robot_interface->getMotionControl(); + auto runtime = rpc_client->getRuntimeMachine(); + + constexpr auto kCleanupTimeout = std::chrono::seconds(2); + constexpr int kStableSamples = 3; + const auto deadline = + std::chrono::steady_clock::now() + kCleanupTimeout; + int stable_samples = 0; + int iteration = 0; + while (!monitor->stop_requested.load() && + std::chrono::steady_clock::now() < deadline) { + int queue_size = motion_control->getQueueSize(); + int trajectory_queue_size = + motion_control->getTrajectoryQueueSize(); + int servo_mode = motion_control->getServoModeSelect(); + auto runtime_state = runtime->getRuntimeState(); + + if (runtime_state != RuntimeState::Stopped) { + monitor->runtime_abort_required.store(true); + } + if (servo_mode != 0) { + monitor->servo_disable_required.store(true); + } + if (queue_size != 0 || trajectory_queue_size != 0) { + monitor->path_clear_required.store(true); + } + + if (iteration % 4 == 0) { + if (monitor->runtime_abort_required.load() && + runtime->abort() == arcs::common_interface::AUBO_OK) { + monitor->runtime_abort_required.store(false); + } + if (monitor->servo_disable_required.load() && + motion_control->setServoModeSelect(0) == + arcs::common_interface::AUBO_OK) { + monitor->servo_disable_required.store(false); + } + if (monitor->path_clear_required.load() && + motion_control->clearPath() == + arcs::common_interface::AUBO_OK) { + monitor->path_clear_required.store(false); + } + } + + queue_size = motion_control->getQueueSize(); + trajectory_queue_size = + motion_control->getTrajectoryQueueSize(); + servo_mode = motion_control->getServoModeSelect(); + runtime_state = runtime->getRuntimeState(); + const bool owner_active = + monitor->motion_state->ownerActive( + cancellation.active_token); + const bool queues_cleared = + motion_control->getExecId() == -1 && + queue_size == 0 && trajectory_queue_size == 0 && + servo_mode == 0 && + runtime_state == RuntimeState::Stopped && + !owner_active && + !monitor->runtime_abort_required.load() && + !monitor->servo_disable_required.load() && + !monitor->path_clear_required.load(); + if (queues_cleared) { + if (++stable_samples >= kStableSamples) { + CMVR_LOG(INFO) + << "[AuboArm] pre-startup safety cleanup confirmed, id=" + << monitor->arm_id; + return true; + } + } else { + stable_samples = 0; + } + + ++iteration; + if (monitorWait(monitor, kSafetyPollInterval)) { + break; + } + } + } catch (const std::exception& e) { + CMVR_LOG(WARNING) + << "[AuboArm] pre-startup safety cleanup failed, id=" + << monitor->arm_id << ", error=" << e.what(); + } + return false; +} + +bool controllerStillQuiescent( + const std::shared_ptr& rpc_client, + const RobotInterfacePtr& robot_interface) +{ + auto motion_control = robot_interface->getMotionControl(); + return motion_control->getExecId() == -1 && + motion_control->getQueueSize() == 0 && + motion_control->getTrajectoryQueueSize() == 0 && + robot_interface->getRobotState()->isSteady() && + motion_control->getServoModeSelect() == 0 && + rpc_client->getRuntimeMachine()->getRuntimeState() == + RuntimeState::Stopped; +} + +void runSafetyMonitor( + const std::shared_ptr& monitor, + const std::string& ip, + const int port, + const std::string& username, + const std::string& password) +{ + while (!monitor->stop_requested.load()) { + auto rpc_client = makeRpcClient(); + try { + if (!rpc_client) { + publishSafetyUnavailable(monitor, "create RPC client failed"); + } else { + rpc_client->setRequestTimeout(250); + const int connect_ret = rpc_client->connect(ip, port); + const int login_ret = connect_ret == 0 + ? rpc_client->login(username, password) + : connect_ret; + if (connect_ret != 0 || login_ret != 0) { + publishSafetyUnavailable( + monitor, + "monitor RPC connect/login failed"); + } else { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface( + rpc_client, "safety monitor", interface_result); + if (!interface_result.ok()) { + publishSafetyUnavailable( + monitor, interface_result.message); + } else { + while (!monitor->stop_requested.load()) { + refreshSafetySample( + rpc_client, monitor, robot_interface); + + if (monitor->safety_state->snapshot().latched) { + if (monitor->cancellation_confirmed.load() && + !controllerStillQuiescent( + rpc_client, robot_interface)) { + CMVR_LOG(WARNING) + << "[AuboArm] controller activity reappeared while safety was latched, id=" + << monitor->arm_id; + cancelForSafetyTransition(monitor); + } + if (!monitor->cancellation_confirmed.load()) { + (void)enforceControllerTermination( + rpc_client, monitor); + } + } + if (monitorWait( + monitor, kSafetyPollInterval)) { + break; + } + } + } + } + } + } catch (const std::exception& e) { + publishSafetyUnavailable(monitor, e.what()); + } + + if (rpc_client) { + try { + if (rpc_client->hasLogined()) { + rpc_client->logout(); + } + if (rpc_client->hasConnected()) { + rpc_client->disconnect(); + } + } catch (const std::exception& e) { + CMVR_LOG(WARNING) + << "[AuboArm] safety monitor cleanup failed, id=" + << monitor->arm_id << ", error=" << e.what(); + } + } + if (!monitor->stop_requested.load()) { + publishSafetyUnavailable(monitor, "monitor RPC disconnected"); + (void)monitorWait(monitor, kSafetyReconnectInterval); + } + } +} enum class CabinetIoOperation { GetDigitalInput, @@ -372,6 +1076,19 @@ struct AuboArm::SdkState { std::shared_ptr rpc_client; std::shared_ptr motion_state{ std::make_shared()}; + std::shared_ptr safety_monitor; + std::thread safety_monitor_thread; + + ~SdkState() + { + if (safety_monitor) { + safety_monitor->stop_requested.store(true); + safety_monitor->wait_cv.notify_all(); + } + if (safety_monitor_thread.joinable()) { + safety_monitor_thread.join(); + } + } }; AuboArm::AuboArm(const config::RobotArmConfig& cfg) @@ -565,13 +1282,31 @@ ArmState AuboArm::getRobotState() const { ArmState state; state.connected = connected_.load(); - state.powered_on = state.connected; - state.brake_released = state.connected; state.moving = busy(); state.robot_mode = getRobotMode(); state.safety_mode = getSafetyMode(); state.control_mode = getControlMode(); - state.emergency_stopped = emergency_stopped_; + state.powered_on = state.connected && + state.robot_mode != RobotMode::Disconnected && + state.robot_mode != RobotMode::PowerOff && + state.robot_mode != RobotMode::Unknown; + state.brake_released = + state.robot_mode == RobotMode::Running && + state.safety_mode != SafetyMode::EmergencyStop && + state.safety_mode != SafetyMode::SystemEmergencyStop && + state.safety_mode != SafetyMode::SafeguardStop; + state.program_running = false; + { + std::lock_guard lock(mutex_); + if (sdk_ && sdk_->safety_monitor) { + state.program_running = + sdk_->safety_monitor->runtime_state.load() == + static_cast(RuntimeState::Running); + } + } + state.protective_stopped = isProtectiveStopped(); + state.emergency_stopped = isEmergencyStopped(); + state.fault = isFault(); state.speed_scaling = speed_scaling_; state.actual_joint_state = getJointState(); state.target_joint_state = state.actual_joint_state; @@ -656,10 +1391,75 @@ RobotMode AuboArm::getRobotMode() const if (!connected_.load()) { return RobotMode::Disconnected; } - if (emergency_stopped_) { + const auto safety_mode = getSafetyMode(); + if (safety_mode == SafetyMode::Fault) { + return RobotMode::Fault; + } + if (safety_mode == SafetyMode::ProtectiveStop || + safety_mode == SafetyMode::SafeguardStop || + safety_mode == SafetyMode::EmergencyStop || + safety_mode == SafetyMode::SystemEmergencyStop) { return RobotMode::Stopped; } - return busy() ? RobotMode::Running : RobotMode::Idle; + + std::lock_guard lock(mutex_); + if (!sdk_ || !sdk_->safety_monitor) { + return RobotMode::Unknown; + } + return publicRobotMode(static_cast( + sdk_->safety_monitor->robot_mode.load())); +} + +SafetyMode AuboArm::getSafetyMode() const +{ + if (!connected_.load()) { + return SafetyMode::Unknown; + } + std::lock_guard lock(mutex_); + if (!sdk_ || !sdk_->safety_monitor) { + return SafetyMode::Unknown; + } + const auto monitor = sdk_->safety_monitor; + if (!safetySampleFresh(monitor)) { + publishSafetyUnavailable(monitor, "sample is stale"); + } + const auto snapshot = monitor->safety_state->snapshot(); + return publicSafetyMode( + snapshot.latched ? snapshot.latched_reason : snapshot.observed); +} + +ControlMode AuboArm::getControlMode() const +{ + if (!connected_.load()) { + return ControlMode::None; + } + std::lock_guard lock(mutex_); + if (sdk_ && sdk_->safety_monitor && + sdk_->safety_monitor->servo_mode_select.load() != 0) { + return ControlMode::Servo; + } + return ControlMode::Position; +} + +bool AuboArm::isProtectiveStopped() const +{ + const auto mode = getSafetyMode(); + return mode == SafetyMode::ProtectiveStop || + mode == SafetyMode::SafeguardStop; +} + +bool AuboArm::isEmergencyStopped() const +{ + const auto mode = getSafetyMode(); + return emergency_stopped_.load() || + mode == SafetyMode::EmergencyStop || + mode == SafetyMode::SystemEmergencyStop; +} + +bool AuboArm::isFault() const +{ + return getSafetyMode() == SafetyMode::Fault || + getRobotMode() == RobotMode::Fault; } bool AuboArm::busy() const @@ -673,19 +1473,123 @@ bool AuboArm::busy() const Result AuboArm::torqueOn() { - const auto ready = ensureConnected_("torqueOn"); - if (!ready.ok()) { - return ready; + std::shared_ptr rpc_client; + std::shared_ptr monitor; + { + std::lock_guard lock(mutex_); + const auto ready = ensureConnected_("torqueOn"); + if (!ready.ok()) { + return ready; + } + rpc_client = sdk_->rpc_client; + monitor = sdk_->safety_monitor; } try { - const auto robot_names = sdk_->rpc_client->getRobotNames(); - if (robot_names.empty()) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); + if (!monitor) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] torqueOn failed: hardware safety monitor is unavailable"); } - auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); - if (!robot_interface) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); + std::unique_lock command_rpc_lock( + monitor->command_rpc_mutex); + Result interface_result; + auto robot_interface = getPrimaryRobotInterface( + rpc_client, "torqueOn", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + + refreshSafetySample(rpc_client, monitor, robot_interface); + auto safety_snapshot = monitor->safety_state->snapshot(); + const std::uint64_t entry_safety_epoch = safety_snapshot.epoch; + if (monitor->emergency_stop_source.load() != 0) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "[AuboArm] torqueOn rejected: hardware emergency-stop input is still active"); + } + + aubo_internal::RecoveryToken recovery_token; + std::unique_ptr recovery_guard; + bool recovering = safety_snapshot.latched; + if (recovering && + !aubo_internal::isMotionSafe(safety_snapshot.observed)) { + const auto condition = safety_snapshot.observed; + if (aubo_internal::needsProtectiveUnlock(condition)) { + return Result::failure( + ArmErrorCode::RobotInProtectiveStop, + "[AuboArm] torqueOn rejected: ProtectiveStop/Violation must be cleared with unlockProtectiveStop first"); + } + if (condition == + aubo_internal::SafetyCondition::SafeguardStop) { + return Result::failure( + ArmErrorCode::RobotInProtectiveStop, + "[AuboArm] torqueOn rejected: SafeguardStop requires the external safety IO to be cleared"); + } + if (condition == + aubo_internal::SafetyCondition::Recovery) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] torqueOn rejected: Recovery mode requires manually moving the arm inside its safety limits"); + } + if (!aubo_internal::needsInterfaceBoardRestart(condition)) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] torqueOn rejected: safety state is " + + 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)); + } + + const auto safety_deadline = + std::chrono::steady_clock::now() + + std::chrono::seconds(10); + do { + std::this_thread::sleep_for( + std::chrono::milliseconds(100)); + refreshSafetySample( + rpc_client, monitor, robot_interface); + safety_snapshot = monitor->safety_state->snapshot(); + if (aubo_internal::isMotionSafe( + safety_snapshot.observed)) { + break; + } + } while (std::chrono::steady_clock::now() < + safety_deadline); + } + + safety_snapshot = monitor->safety_state->snapshot(); + if (safety_snapshot.epoch != entry_safety_epoch) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] torqueOn recovery rejected: a new safety event occurred while resetting the controller; retry recovery explicitly"); + } + if (!aubo_internal::isMotionSafe(safety_snapshot.observed)) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] torqueOn rejected: safety state is " + + std::string(safetyConditionName( + safety_snapshot.observed))); + } + + if (recovering) { + const auto token = monitor->safety_state->beginRecovery( + entry_safety_epoch); + if (!token.has_value()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] torqueOn recovery rejected: safety latch changed"); + } + recovery_token = *token; + recovery_guard = std::make_unique( + monitor->safety_state, recovery_token); } double mass = 0.0; @@ -694,18 +1598,106 @@ Result AuboArm::torqueOn() std::vector inertia(6, 0.0); robot_interface->getRobotConfig()->setPayload(mass, cog, aom, inertia); - const auto current_mode = robot_interface->getRobotState()->getRobotModeType(); - if (current_mode != arcs::common_interface::RobotModeType::Running) { - robot_interface->getRobotManage()->poweron(); + 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 (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Idle)) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Idle"); } - robot_interface->getRobotManage()->startup(); + current_mode = RobotModeType::Idle; + } + + refreshSafetySample(rpc_client, monitor, robot_interface); + auto before_brake_release = + monitor->safety_state->snapshot(); + const std::uint64_t expected_epoch = recovering + ? recovery_token.epoch + : entry_safety_epoch; + const bool recovery_token_current = !recovering || + (before_brake_release.recovery_in_progress && + before_brake_release.epoch == recovery_token.epoch); + if (before_brake_release.epoch != expected_epoch || + !recovery_token_current || before_brake_release.latched != recovering || + !aubo_internal::isMotionSafe( + before_brake_release.observed) || + monitor->emergency_stop_source.load() != 0) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] torqueOn rejected: safety state changed before brake release; the new event remains latched"); + } + + if (recovering) { + cancelForSafetyTransition(monitor); + const bool cleanup_ok = current_mode == RobotModeType::Running + ? enforceControllerTermination(rpc_client, monitor) + : prepareControllerForStartup(rpc_client, monitor); + if (!cleanup_ok) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn recovery failed: old controller queue could not be acknowledged and cleared before brake release"); + } + } + + 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 (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Running)) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Running"); } } - emergency_stopped_ = false; + + refreshSafetySample(rpc_client, monitor, robot_interface); + const auto after_startup = monitor->safety_state->snapshot(); + const bool post_recovery_token_current = !recovering || + (after_startup.recovery_in_progress && + after_startup.epoch == recovery_token.epoch); + if (after_startup.epoch != expected_epoch || + !post_recovery_token_current || after_startup.latched != recovering || + !aubo_internal::isMotionSafe(after_startup.observed) || + monitor->emergency_stop_source.load() != 0 || + monitor->robot_mode.load() != + static_cast(RobotModeType::Running)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] torqueOn rejected: safety state changed during startup; the new event remains latched"); + } + if (recovering) { + 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"); + } + refreshSafetySample(rpc_client, monitor, robot_interface); + const bool robot_running = + monitor->robot_mode.load() == + static_cast(RobotModeType::Running); + if (!recovery_guard->complete( + robot_running, + true, + monitor->cancellation_confirmed.load())) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] torqueOn recovery failed: safety state changed during recovery"); + } + } + emergency_stopped_.store(false); + servo_mode_.store(false); return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOn failed: ") + e.what()); @@ -746,7 +1738,28 @@ Result AuboArm::calibrateZeroQ(const std::string& joint_name) Result AuboArm::emergencyStop() { - emergency_stopped_ = true; + emergency_stopped_.store(true); + { + std::lock_guard lock(mutex_); + if (sdk_ && sdk_->safety_monitor) { + sdk_->safety_monitor->safety_state->observe( + aubo_internal::SafetyCondition::RobotEmergencyStop); + cancelForSafetyTransition(sdk_->safety_monitor); + } + } + return stopMotion(); +} + +Result AuboArm::protectiveStop() +{ + { + std::lock_guard lock(mutex_); + if (sdk_ && sdk_->safety_monitor) { + sdk_->safety_monitor->safety_state->observe( + aubo_internal::SafetyCondition::ProtectiveStop); + cancelForSafetyTransition(sdk_->safety_monitor); + } + } return stopMotion(); } @@ -771,8 +1784,16 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o if (!locked_ready.ok()) { return locked_ready; } + std::uint64_t safety_epoch = 0; + const auto safety_ready = ensureMotionReady_( + "moveJ", safety_epoch); + if (!safety_ready.ok()) { + return safety_ready; + } const auto rpc_client = sdk_->rpc_client; const auto motion_state = sdk_->motion_state; + const auto safety_monitor = sdk_->safety_monitor; + const aubo_internal::SafetyPermit safety_permit{safety_epoch}; const auto motion = motion_state->begin( aubo_internal::MotionKind::Joint); if (!motion.started()) { @@ -797,6 +1818,12 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o auto motion_control = robot_interface->getMotionControl(); motion_control->setSpeedFraction(speed_scaling_); motion_owner.requireExplicitSettlement(); + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveJ cancelled by hardware safety before submission"); + } const int ret = motion_control->moveJoint( target.position, options.acceleration > 0.0 ? options.acceleration : 0.5, @@ -812,13 +1839,28 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o ret, arcs::common_interface::AUBO_OK, arcs::common_interface::AUBO_REQUEST_IGNORE, - [&robot_interface, motion_state, token = motion.token]() { + [&robot_interface, + motion_state, + safety_monitor, + safety_permit, + token = motion.token]() { return waitArrival( robot_interface, - [motion_state, token]() { - return motion_state->cancelled(token); + [motion_state, + safety_monitor, + safety_permit, + token]() { + return motion_state->cancelled(token) || + !validateSafetyPermit( + safety_monitor, safety_permit); }); }); + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveJ cancelled by hardware safety event"); + } switch (outcome) { case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion: CMVR_LOG(DEBUG) << "[AuboArm] moveJ completed without motion: sdk ret=" @@ -833,7 +1875,7 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o motion_owner.settle(); return Result::failure( ArmErrorCode::CommandRejected, - "[AuboArm] moveJ stopped by stopMotion"); + "[AuboArm] moveJ cancelled by stopMotion or hardware safety event"); case aubo_internal::MotionCommandOutcome::SubmitFailed: return Result::failure( ArmErrorCode::CommandFailed, @@ -860,8 +1902,16 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration if (!locked_ready.ok()) { return locked_ready; } + std::uint64_t safety_epoch = 0; + const auto safety_ready = ensureMotionReady_( + "speedJ", safety_epoch); + if (!safety_ready.ok()) { + return safety_ready; + } const auto rpc_client = sdk_->rpc_client; const auto motion_state = sdk_->motion_state; + const auto safety_monitor = sdk_->safety_monitor; + const aubo_internal::SafetyPermit safety_permit{safety_epoch}; const auto motion = motion_state->begin( aubo_internal::MotionKind::Joint, true); if (!motion.started()) { @@ -887,21 +1937,23 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration const double resolved_duration = duration > 0.0 ? duration : 100.0; motion_owner.requireExplicitSettlement(); submit_lock.unlock(); - if (motion_state->cancelled(motion.token)) { + if (motion_state->cancelled(motion.token) || + !validateSafetyPermit(safety_monitor, safety_permit)) { motion_owner.settle(); return Result::failure( ArmErrorCode::CommandRejected, - "[AuboArm] speedJ stopped by stopMotion before submission"); + "[AuboArm] speedJ cancelled before submission"); } const int ret = robot_interface->getMotionControl()->speedJoint( velocity.velocity, resolved_acceleration, resolved_duration); - if (motion_state->cancelled(motion.token)) { + if (motion_state->cancelled(motion.token) || + !validateSafetyPermit(safety_monitor, safety_permit)) { motion_owner.settle(); return Result::failure( ArmErrorCode::CommandRejected, - "[AuboArm] speedJ stopped by stopMotion"); + "[AuboArm] speedJ cancelled by stopMotion or hardware safety event"); } if (ret != 0) { motion_owner.settle(); @@ -930,8 +1982,16 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, if (!locked_ready.ok()) { return locked_ready; } + std::uint64_t safety_epoch = 0; + const auto safety_ready = ensureMotionReady_( + "moveL", safety_epoch); + if (!safety_ready.ok()) { + return safety_ready; + } const auto rpc_client = sdk_->rpc_client; const auto motion_state = sdk_->motion_state; + const auto safety_monitor = sdk_->safety_monitor; + const aubo_internal::SafetyPermit safety_permit{safety_epoch}; const auto motion = motion_state->begin( aubo_internal::MotionKind::Linear); if (!motion.started()) { @@ -959,6 +2019,12 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); std::vector pose{target.x, target.y, target.z, target.rx, target.ry, target.rz}; motion_owner.requireExplicitSettlement(); + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveL cancelled by hardware safety before submission"); + } const int ret = motion_control->moveLine( pose, options.acceleration > 0.0 ? options.acceleration : 0.5, @@ -974,13 +2040,28 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, ret, arcs::common_interface::AUBO_OK, arcs::common_interface::AUBO_REQUEST_IGNORE, - [&robot_interface, motion_state, token = motion.token]() { + [&robot_interface, + motion_state, + safety_monitor, + safety_permit, + token = motion.token]() { return waitArrival( robot_interface, - [motion_state, token]() { - return motion_state->cancelled(token); + [motion_state, + safety_monitor, + safety_permit, + token]() { + return motion_state->cancelled(token) || + !validateSafetyPermit( + safety_monitor, safety_permit); }); }); + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveL cancelled by hardware safety event"); + } switch (outcome) { case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion: CMVR_LOG(DEBUG) << "[AuboArm] moveL completed without motion: sdk ret=" @@ -995,7 +2076,7 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, motion_owner.settle(); return Result::failure( ArmErrorCode::CommandRejected, - "[AuboArm] moveL stopped by stopMotion"); + "[AuboArm] moveL cancelled by stopMotion or hardware safety event"); case aubo_internal::MotionCommandOutcome::SubmitFailed: return Result::failure( ArmErrorCode::CommandFailed, @@ -1018,8 +2099,16 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d if (!locked_ready.ok()) { return locked_ready; } + std::uint64_t safety_epoch = 0; + const auto safety_ready = ensureMotionReady_( + "speedL", safety_epoch); + if (!safety_ready.ok()) { + return safety_ready; + } const auto rpc_client = sdk_->rpc_client; const auto motion_state = sdk_->motion_state; + const auto safety_monitor = sdk_->safety_monitor; + const aubo_internal::SafetyPermit safety_permit{safety_epoch}; const auto motion = motion_state->begin( aubo_internal::MotionKind::Linear, true); if (!motion.started()) { @@ -1077,21 +2166,23 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d const double resolved_duration = duration > 0.0 ? duration : 100.0; motion_owner.requireExplicitSettlement(); submit_lock.unlock(); - if (motion_state->cancelled(motion.token)) { + if (motion_state->cancelled(motion.token) || + !validateSafetyPermit(safety_monitor, safety_permit)) { motion_owner.settle(); return Result::failure( ArmErrorCode::CommandRejected, - "[AuboArm] speedL stopped by stopMotion before submission"); + "[AuboArm] speedL cancelled before submission"); } const int ret = robot_interface->getMotionControl()->speedLine( speed, resolved_acceleration, resolved_duration); - if (motion_state->cancelled(motion.token)) { + if (motion_state->cancelled(motion.token) || + !validateSafetyPermit(safety_monitor, safety_permit)) { motion_owner.settle(); return Result::failure( ArmErrorCode::CommandRejected, - "[AuboArm] speedL stopped by stopMotion"); + "[AuboArm] speedL cancelled by stopMotion or hardware safety event"); } if (ret != 0) { motion_owner.settle(); @@ -1279,6 +2370,14 @@ Result AuboArm::startServoMode(const ServoOptions& options) if (!ready.ok()) { return ready; } + std::uint64_t safety_epoch = 0; + const auto safety_ready = ensureMotionReady_( + "startServoMode", safety_epoch); + if (!safety_ready.ok()) { + return safety_ready; + } + const auto safety_monitor = sdk_->safety_monitor; + const aubo_internal::SafetyPermit safety_permit{safety_epoch}; try { Result interface_result; @@ -1286,6 +2385,11 @@ Result AuboArm::startServoMode(const ServoOptions& options) if (!interface_result.ok()) { return interface_result; } + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] startServoMode cancelled by hardware safety"); + } const int ret = robot_interface->getMotionControl()->setServoModeSelect(kAuboServoMode); if (ret != 0) { return Result::failure(ArmErrorCode::CommandFailed, @@ -1295,8 +2399,15 @@ Result AuboArm::startServoMode(const ServoOptions& options) return Result::failure(ArmErrorCode::Timeout, "[AuboArm] startServoMode failed: timeout waiting for servo mode"); } + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + (void)robot_interface->getMotionControl()->setServoModeSelect(0); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] startServoMode cancelled by hardware safety event"); + } servo_options_ = options; servo_mode_.store(true); + safety_monitor->servo_mode_select.store(kAuboServoMode); return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, @@ -1314,6 +2425,13 @@ Result AuboArm::servoJ(const JointPositionCommand& target) if (!ready.ok()) { return ready; } + std::uint64_t safety_epoch = 0; + const auto safety_ready = ensureMotionReady_("servoJ", safety_epoch); + if (!safety_ready.ok()) { + return safety_ready; + } + const auto safety_monitor = sdk_->safety_monitor; + const aubo_internal::SafetyPermit safety_permit{safety_epoch}; try { Result interface_result; @@ -1328,6 +2446,11 @@ Result AuboArm::servoJ(const JointPositionCommand& target) } } const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008; + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] servoJ cancelled by hardware safety before submission"); + } const int ret = robot_interface->getMotionControl()->servoJoint( target.position, 0.0, @@ -1339,6 +2462,11 @@ Result AuboArm::servoJ(const JointPositionCommand& target) return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] servoJ failed: ret=" + std::to_string(ret)); } + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] servoJ cancelled by hardware safety event"); + } return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] servoJ failed: ") + e.what()); @@ -1351,6 +2479,13 @@ Result AuboArm::servoL(const CartesianPose& target, FrameType frame) if (!ready.ok()) { return ready; } + std::uint64_t safety_epoch = 0; + const auto safety_ready = ensureMotionReady_("servoL", safety_epoch); + if (!safety_ready.ok()) { + return safety_ready; + } + const auto safety_monitor = sdk_->safety_monitor; + const aubo_internal::SafetyPermit safety_permit{safety_epoch}; try { Result interface_result; @@ -1379,6 +2514,11 @@ Result AuboArm::servoL(const CartesianPose& target, FrameType frame) } const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008; + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] servoL cancelled by hardware safety before submission"); + } const int ret = robot_interface->getMotionControl()->servoCartesian( pose, 0.0, @@ -1390,6 +2530,11 @@ Result AuboArm::servoL(const CartesianPose& target, FrameType frame) return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] servoL failed: ret=" + std::to_string(ret)); } + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] servoL cancelled by hardware safety event"); + } return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] servoL failed: ") + e.what()); @@ -1422,6 +2567,9 @@ Result AuboArm::stopServoMode() } const int ret = robot_interface->getMotionControl()->setServoModeSelect(0); servo_mode_.store(false); + if (sdk_->safety_monitor) { + sdk_->safety_monitor->servo_mode_select.store(0); + } if (ret != 0) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] stopServoMode failed: ret=" + std::to_string(ret)); @@ -1450,13 +2598,7 @@ Result AuboArm::connect(const std::string& ip, const int port) try { const int resolved_port = port > 0 ? port : 30004; auto sdk_state = std::make_unique(); - sdk_state->rpc_client = std::shared_ptr( - ::createRpcClient(), - [](arcs::aubo_sdk::RpcClient* client) { - if (client) { - ::destroyRpcClient(client); - } - }); + sdk_state->rpc_client = makeRpcClient(); if (!sdk_state->rpc_client) { return Result::failure(ArmErrorCode::ConnectionFailed, "[AuboArm] connect failed: create RPC client failed"); @@ -1492,8 +2634,38 @@ Result AuboArm::connect(const std::string& ip, const int port) "[AuboArm] connect failed: robot name list is empty"); } + auto robot_interface = sdk_state->rpc_client->getRobotInterface( + robot_names.front()); + if (!robot_interface) { + sdk_state->rpc_client->logout(); + sdk_state->rpc_client->disconnect(); + return Result::failure(ArmErrorCode::ConnectionFailed, + "[AuboArm] connect failed: robot interface is null"); + } + + sdk_state->safety_monitor = + std::make_shared(); + sdk_state->safety_monitor->motion_state = + sdk_state->motion_state; + sdk_state->safety_monitor->arm_id = id_; + publishSafetySample( + sdk_state->safety_monitor, + robot_interface->getRobotState()->getSafetyModeType(), + robot_interface->getRobotState()->getRobotModeType(), + sdk_state->rpc_client->getRuntimeMachine()->getRuntimeState(), + robot_interface->getRobotConfig() + ->getRobotEmergencyStopSource(), + robot_interface->getMotionControl()->getServoModeSelect()); + ip_ = ip; port_ = resolved_port; + sdk_state->safety_monitor_thread = std::thread( + runSafetyMonitor, + sdk_state->safety_monitor, + ip_, + port_, + username_, + password_); sdk_ = std::move(sdk_state); connected_.store(true); return Result::success(); @@ -1509,21 +2681,38 @@ Result AuboArm::disconnect() { std::lock_guard lock(mutex_); try { - if (sdk_ && sdk_->rpc_client) { - if (sdk_->rpc_client->hasLogined()) { - sdk_->rpc_client->logout(); - } - if (sdk_->rpc_client->hasConnected()) { - sdk_->rpc_client->disconnect(); + connected_.store(false); + if (sdk_ && sdk_->safety_monitor) { + sdk_->safety_monitor->stop_requested.store(true); + sdk_->safety_monitor->wait_cv.notify_all(); + } + if (sdk_ && sdk_->safety_monitor_thread.joinable()) { + sdk_->safety_monitor_thread.join(); + } + const auto close_command_rpc = [this]() { + if (sdk_ && sdk_->rpc_client) { + if (sdk_->rpc_client->hasLogined()) { + sdk_->rpc_client->logout(); + } + if (sdk_->rpc_client->hasConnected()) { + sdk_->rpc_client->disconnect(); + } } + }; + if (sdk_ && sdk_->safety_monitor) { + std::unique_lock command_rpc_lock( + sdk_->safety_monitor->command_rpc_mutex); + close_command_rpc(); + } else { + close_command_rpc(); } } catch (const std::exception& e) { CMVR_LOG(ERROR) << "[AuboArm] disconnect failed: " << e.what(); } sdk_.reset(); - connected_.store(false); busy_.store(false); servo_mode_.store(false); + emergency_stopped_.store(false); return Result::success(); } @@ -1533,6 +2722,234 @@ Result AuboArm::shutdown() return disconnect(); } +Result AuboArm::clearFault() +{ + std::shared_ptr rpc_client; + std::shared_ptr monitor; + { + std::lock_guard lock(mutex_); + const auto ready = ensureConnected_("clearFault"); + if (!ready.ok()) { + return ready; + } + rpc_client = sdk_->rpc_client; + monitor = sdk_->safety_monitor; + } + + try { + if (!monitor) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] clearFault failed: hardware safety monitor is unavailable"); + } + std::unique_lock command_rpc_lock( + monitor->command_rpc_mutex); + Result interface_result; + auto robot_interface = getPrimaryRobotInterface( + rpc_client, "clearFault", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + refreshSafetySample(rpc_client, monitor, robot_interface); + auto snapshot = monitor->safety_state->snapshot(); + const std::uint64_t expected_safety_epoch = snapshot.epoch; + if (!snapshot.latched && + aubo_internal::isMotionSafe(snapshot.observed) && + monitor->robot_mode.load() != + static_cast(RobotModeType::Error)) { + return Result::success(); + } + if (!snapshot.latched && + monitor->robot_mode.load() == + static_cast(RobotModeType::Error)) { + return Result::failure( + ArmErrorCode::RobotInFault, + "[AuboArm] clearFault rejected: RobotMode is Error even though the safety mode is Normal/Reduced"); + } + if (monitor->emergency_stop_source.load() != 0) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "[AuboArm] clearFault rejected: hardware emergency-stop input is still active"); + } + + if (aubo_internal::needsProtectiveUnlock(snapshot.observed) || + aubo_internal::needsProtectiveUnlock( + snapshot.latched_reason)) { + command_rpc_lock.unlock(); + return unlockProtectiveStop_(expected_safety_epoch); + } + + if (aubo_internal::isMotionSafe(snapshot.observed)) { + if (monitor->robot_mode.load() == + static_cast(RobotModeType::Running)) { + command_rpc_lock.unlock(); + return completeSafetyRecovery_( + "clearFault", expected_safety_epoch); + } + return Result::failure( + ArmErrorCode::RobotNotPowered, + "[AuboArm] safety condition is clear, but torqueOn is required to verify the old queue and complete recovery"); + } + + if (!aubo_internal::needsInterfaceBoardRestart( + snapshot.observed)) { + const std::string guidance = + snapshot.observed == + aubo_internal::SafetyCondition::SafeguardStop + ? "clear the external safety IO" + : (snapshot.observed == + aubo_internal::SafetyCondition::Recovery + ? "manually move the arm inside its safety limits" + : "restore a valid controller safety state"); + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] clearFault rejected for " + + std::string(safetyConditionName(snapshot.observed)) + + ": " + guidance); + } + + const int ret = + robot_interface->getRobotManage()->restartInterfaceBoard(); + if (ret != arcs::common_interface::AUBO_OK) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] clearFault failed: restartInterfaceBoard ret=" + + std::to_string(ret)); + } + + const auto deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(10); + while (std::chrono::steady_clock::now() < deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + refreshSafetySample(rpc_client, monitor, robot_interface); + snapshot = monitor->safety_state->snapshot(); + if (aubo_internal::isMotionSafe(snapshot.observed)) { + break; + } + } + if (!aubo_internal::isMotionSafe(snapshot.observed)) { + return Result::failure( + ArmErrorCode::Timeout, + "[AuboArm] clearFault failed: timeout waiting for a safe controller state"); + } + if (monitor->robot_mode.load() == + static_cast(RobotModeType::Running)) { + command_rpc_lock.unlock(); + return completeSafetyRecovery_( + "clearFault", expected_safety_epoch); + } + return Result::failure( + ArmErrorCode::RobotNotPowered, + "[AuboArm] controller fault was reset, but the safety latch remains until torqueOn verifies an empty queue in Running mode"); + } catch (const std::exception& e) { + return Result::failure( + ArmErrorCode::CommandFailed, + std::string("[AuboArm] clearFault failed: ") + e.what()); + } +} + +Result AuboArm::unlockProtectiveStop() +{ + return unlockProtectiveStop_(std::nullopt); +} + +Result AuboArm::unlockProtectiveStop_( + const std::optional expected_safety_epoch) +{ + std::shared_ptr rpc_client; + std::shared_ptr monitor; + { + std::lock_guard lock(mutex_); + const auto ready = ensureConnected_("unlockProtectiveStop"); + if (!ready.ok()) { + return ready; + } + rpc_client = sdk_->rpc_client; + monitor = sdk_->safety_monitor; + } + + try { + if (!monitor) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] unlockProtectiveStop failed: hardware safety monitor is unavailable"); + } + std::unique_lock command_rpc_lock( + monitor->command_rpc_mutex); + Result interface_result; + auto robot_interface = getPrimaryRobotInterface( + rpc_client, "unlockProtectiveStop", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + refreshSafetySample(rpc_client, monitor, robot_interface); + auto snapshot = monitor->safety_state->snapshot(); + const std::uint64_t recovery_epoch = + expected_safety_epoch.value_or(snapshot.epoch); + if (snapshot.epoch != recovery_epoch) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] unlockProtectiveStop rejected: a newer safety event superseded this recovery request"); + } + if (!snapshot.latched && + aubo_internal::isMotionSafe(snapshot.observed)) { + return Result::success(); + } + if (monitor->emergency_stop_source.load() != 0) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "[AuboArm] unlockProtectiveStop rejected: hardware emergency-stop input is active"); + } + + if (aubo_internal::needsProtectiveUnlock( + snapshot.observed)) { + const int ret = robot_interface->getRobotManage() + ->setUnlockProtectiveStop(); + if (ret != arcs::common_interface::AUBO_OK) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] unlockProtectiveStop failed: sdk ret=" + + std::to_string(ret)); + } + } else if (!aubo_internal::isMotionSafe(snapshot.observed)) { + return Result::failure( + ArmErrorCode::RobotInProtectiveStop, + "[AuboArm] unlockProtectiveStop rejected: current safety state is " + + std::string(safetyConditionName(snapshot.observed))); + } + + const auto deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(5); + while (std::chrono::steady_clock::now() < deadline) { + refreshSafetySample(rpc_client, monitor, robot_interface); + snapshot = monitor->safety_state->snapshot(); + if (aubo_internal::isMotionSafe(snapshot.observed)) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + if (!aubo_internal::isMotionSafe(snapshot.observed)) { + return Result::failure( + ArmErrorCode::Timeout, + "[AuboArm] unlockProtectiveStop failed: safety mode did not return to Normal/Reduced"); + } + if (monitor->robot_mode.load() != + static_cast(RobotModeType::Running)) { + return Result::failure( + ArmErrorCode::RobotNotPowered, + "[AuboArm] protective stop was unlocked, but torqueOn is required to complete safety recovery"); + } + command_rpc_lock.unlock(); + return completeSafetyRecovery_( + "unlockProtectiveStop", recovery_epoch); + } catch (const std::exception& e) { + return Result::failure( + ArmErrorCode::CommandFailed, + std::string("[AuboArm] unlockProtectiveStop failed: ") + + e.what()); + } +} + Result AuboArm::loadProgram(const std::string& program_name) { if (program_name.empty()) { @@ -1561,12 +2978,33 @@ Result AuboArm::playProgram() if (!ready.ok()) { return ready; } + std::uint64_t safety_epoch = 0; + const auto safety_ready = ensureMotionReady_( + "playProgram", safety_epoch); + if (!safety_ready.ok()) { + return safety_ready; + } + const auto safety_monitor = sdk_->safety_monitor; + const aubo_internal::SafetyPermit safety_permit{safety_epoch}; try { + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] playProgram cancelled by hardware safety before submission"); + } const int ret = sdk_->rpc_client->getRuntimeMachine()->runProgram(); if (ret != 0) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] playProgram failed: ret=" + std::to_string(ret)); } + safety_monitor->runtime_state.store( + static_cast(RuntimeState::Running)); + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + (void)sdk_->rpc_client->getRuntimeMachine()->abort(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] playProgram cancelled by hardware safety event"); + } return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, @@ -1586,6 +3024,10 @@ Result AuboArm::pauseProgram() return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] pauseProgram failed: ret=" + std::to_string(ret)); } + if (sdk_->safety_monitor) { + sdk_->safety_monitor->runtime_state.store( + static_cast(RuntimeState::Paused)); + } return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, @@ -1605,6 +3047,11 @@ Result AuboArm::stopProgram() return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] stopProgram failed: ret=" + std::to_string(ret)); } + if (sdk_->safety_monitor) { + sdk_->safety_monitor->runtime_state.store( + static_cast(RuntimeState::Stopped)); + sdk_->safety_monitor->runtime_abort_required.store(false); + } return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, @@ -1701,6 +3148,119 @@ bool AuboArm::validDof_(const std::size_t size, std::string& error) const return true; } +Result AuboArm::completeSafetyRecovery_( + const std::string& context, + const std::uint64_t expected_safety_epoch) +{ + std::shared_ptr rpc_client; + std::shared_ptr monitor; + { + std::lock_guard lock(mutex_); + const auto ready = ensureConnected_(context); + if (!ready.ok()) { + return ready; + } + rpc_client = sdk_->rpc_client; + monitor = sdk_->safety_monitor; + } + + try { + if (!monitor) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + + " failed: hardware safety monitor is unavailable"); + } + std::unique_lock command_rpc_lock( + monitor->command_rpc_mutex); + Result interface_result; + auto robot_interface = getPrimaryRobotInterface( + rpc_client, context, interface_result); + if (!interface_result.ok()) { + return interface_result; + } + refreshSafetySample(rpc_client, monitor, robot_interface); + if (monitor->emergency_stop_source.load() != 0) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "[AuboArm] " + context + + " rejected: hardware emergency-stop input is active"); + } + + const auto snapshot = monitor->safety_state->snapshot(); + if (snapshot.epoch != expected_safety_epoch) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] " + context + + " rejected: a newer safety event superseded this recovery request"); + } + if (!snapshot.latched) { + return aubo_internal::isMotionSafe(snapshot.observed) + ? Result::success() + : Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + + " failed: safety state is " + + safetyConditionName(snapshot.observed)); + } + if (!aubo_internal::isMotionSafe(snapshot.observed)) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + + " rejected: hardware safety state is " + + safetyConditionName(snapshot.observed)); + } + if (monitor->robot_mode.load() != + static_cast(RobotModeType::Running)) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + + " rejected: robot must be Running before the safety latch can be cleared"); + } + + const auto recovery_token = + monitor->safety_state->beginRecovery( + expected_safety_epoch); + if (!recovery_token.has_value()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] " + context + + " rejected: another recovery is active or the safety state changed"); + } + SafetyRecoveryGuard recovery_guard{ + monitor->safety_state, *recovery_token}; + cancelForSafetyTransition(monitor); + if (!enforceControllerTermination(rpc_client, monitor)) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] " + context + + " failed: old motion/program could not be terminated and verified"); + } + + refreshSafetySample(rpc_client, monitor, robot_interface); + const bool robot_running = + monitor->robot_mode.load() == + static_cast(RobotModeType::Running); + if (!recovery_guard.complete( + robot_running, + true, + monitor->cancellation_confirmed.load())) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] " + context + + " failed: safety state changed while recovery was being verified"); + } + emergency_stopped_.store(false); + servo_mode_.store(false); + return Result::success(); + } catch (const std::exception& e) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] " + context + + " failed during safety recovery: " + e.what()); + } +} + Result AuboArm::ensureConnected_(const std::string& context) const { if (!connected_.load()) { @@ -1714,4 +3274,87 @@ Result AuboArm::ensureConnected_(const std::string& context) const return Result::success(); } +Result AuboArm::ensureMotionReady_( + const std::string& context, + std::uint64_t& safety_epoch) const +{ + const auto connected = ensureConnected_(context); + if (!connected.ok()) { + return connected; + } + if (!sdk_->safety_monitor) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + + " rejected: hardware safety monitor is unavailable"); + } + + const auto monitor = sdk_->safety_monitor; + if (!safetySampleFresh(monitor)) { + publishSafetyUnavailable(monitor, "sample is stale"); + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + + " rejected: hardware safety state is unavailable or stale"); + } + + if (monitor->emergency_stop_source.load() != 0) { + const auto previous = monitor->safety_state->snapshot(); + monitor->safety_state->observe( + aubo_internal::SafetyCondition::RobotEmergencyStop); + if (!previous.latched || + previous.observed != + aubo_internal::SafetyCondition::RobotEmergencyStop) { + cancelForSafetyTransition(monitor); + } + } + + const auto snapshot = monitor->safety_state->snapshot(); + const auto condition = snapshot.latched + ? snapshot.latched_reason + : snapshot.observed; + if (snapshot.latched || + !aubo_internal::isMotionSafe(snapshot.observed)) { + ArmErrorCode code = ArmErrorCode::RobotNotReady; + if (condition == + aubo_internal::SafetyCondition::RobotEmergencyStop || + condition == + aubo_internal::SafetyCondition::SystemEmergencyStop) { + code = ArmErrorCode::RobotInEmergencyStop; + } else if ( + condition == aubo_internal::SafetyCondition::ProtectiveStop || + condition == aubo_internal::SafetyCondition::SafeguardStop) { + code = ArmErrorCode::RobotInProtectiveStop; + } else if ( + condition == aubo_internal::SafetyCondition::Fault || + condition == aubo_internal::SafetyCondition::Violation) { + code = ArmErrorCode::RobotInFault; + } + return Result::failure( + code, + "[AuboArm] " + context + + " rejected: hardware safety latch is " + + safetyConditionName(condition) + + "; clear the hardware condition and perform explicit recovery"); + } + + if (monitor->robot_mode.load() != + static_cast(RobotModeType::Running)) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + + " rejected: robot is not in Running mode"); + } + + const auto permit = monitor->safety_state->tryPermit(); + if (!permit.has_value()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + + " rejected: no valid hardware safety permit"); + } + safety_epoch = permit->epoch; + return Result::success(); +} + } // namespace cmvr::device diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h index 6f05af2d..e708c748 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h @@ -2,6 +2,7 @@ #define CMVR_ES_AUBO_ARM_H #include +#include #include #include #include @@ -30,19 +31,19 @@ public: JointGroupState getJointState() const override; CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override; RobotMode getRobotMode() const override; - SafetyMode getSafetyMode() const override { return SafetyMode::Normal; } - ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; } + SafetyMode getSafetyMode() const override; + ControlMode getControlMode() const override; Result torqueOn() override; Result torqueOff() override; Result calibrateZeroQ(const std::string& joint_name) override; Result emergencyStop() override; - Result protectiveStop() override { return emergencyStop(); } + Result protectiveStop() override; Result setSpeedScaling(double scaling) override; double getSpeedScaling() const override { return speed_scaling_; } - bool isProtectiveStopped() const override { return false; } - bool isEmergencyStopped() const override { return emergency_stopped_; } - bool isFault() const override { return false; } + bool isProtectiveStopped() const override; + bool isEmergencyStopped() const override; + bool isFault() const override; Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override; Result speedJ(const JointVelocityCommand& velocity, double acceleration, double duration) override; @@ -66,8 +67,8 @@ public: Result powerOff() override { return torqueOff(); } Result brakeRelease() override { return torqueOn(); } Result shutdown() override; - Result clearFault() override { return Result::success(); } - Result unlockProtectiveStop() override { return Result::success(); } + Result clearFault() override; + Result unlockProtectiveStop() override; Result loadProgram(const std::string& program_name) override; Result playProgram() override; Result pauseProgram() override; @@ -92,6 +93,12 @@ private: 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 ensureMotionReady_(const std::string& context, + std::uint64_t& safety_epoch) const; + Result completeSafetyRecovery_(const std::string& context, + std::uint64_t expected_safety_epoch); + Result unlockProtectiveStop_( + std::optional expected_safety_epoch); Result stopMotion_(MotionStopKind kind, double acceleration); struct SdkState; @@ -109,7 +116,7 @@ private: std::atomic connected_{false}; std::atomic busy_{false}; std::atomic servo_mode_{false}; - bool emergency_stopped_{false}; + std::atomic emergency_stopped_{false}; mutable std::mutex mutex_; std::unique_ptr sdk_; diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h b/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h index 76409c90..f3012c89 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h @@ -66,6 +66,12 @@ struct StopRequest { } }; +struct SafetyCancelResult { + MotionKind kind{MotionKind::None}; + MotionToken active_token; + bool tracked_motion{false}; +}; + // 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 @@ -183,6 +189,31 @@ public: tracked_motion}; } + SafetyCancelResult cancelActiveForSafety() + { + std::lock_guard lock(mutex_); + const MotionToken active = owner_active_ + ? active_token_ + : MotionToken{}; + if (active.valid()) { + cancelled_generation_ = std::max( + cancelled_generation_, active.generation); + } + + const MotionKind kind = active.valid() + ? active.kind + : last_kind_; + if (kind != MotionKind::None) { + last_kind_ = kind; + } + // This block is intentionally independent of stop_in_progress_. The + // monitor may observe the safety event while a software Stop owns the + // stop transaction; either way no new motion may enter. + blocked_ = true; + owner_finished_cv_.notify_all(); + return {kind, active, active.valid() || kind != MotionKind::None}; + } + bool cancelled(const MotionToken& token) const { std::lock_guard lock(mutex_); diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h b/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h new file mode 100644 index 00000000..8c13b4d8 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h @@ -0,0 +1,184 @@ +#ifndef CMVR_ES_AUBO_SAFETY_STATE_H +#define CMVR_ES_AUBO_SAFETY_STATE_H + +#include +#include +#include + +namespace cmvr::device::aubo_internal { + +// This is deliberately richer than RobotArm::SafetyMode. Recovery and +// Violation have no lossless public mapping, but both must remain fail-closed. +enum class SafetyCondition { + Unknown, + Normal, + Reduced, + Recovery, + Violation, + ProtectiveStop, + SafeguardStop, + SystemEmergencyStop, + RobotEmergencyStop, + Fault, +}; + +inline bool isMotionSafe(const SafetyCondition condition) noexcept +{ + return condition == SafetyCondition::Normal || + condition == SafetyCondition::Reduced; +} + +inline SafetyCondition effectiveSafetyCondition( + const SafetyCondition reported_condition, + const int robot_emergency_stop_source) noexcept +{ + if (robot_emergency_stop_source < 0) { + return SafetyCondition::Unknown; + } + if (robot_emergency_stop_source != 0) { + return SafetyCondition::RobotEmergencyStop; + } + return reported_condition; +} + +inline bool needsProtectiveUnlock( + const SafetyCondition condition) noexcept +{ + return condition == SafetyCondition::ProtectiveStop || + condition == SafetyCondition::Violation; +} + +inline bool needsInterfaceBoardRestart( + const SafetyCondition condition) noexcept +{ + return condition == SafetyCondition::SystemEmergencyStop || + condition == SafetyCondition::RobotEmergencyStop || + condition == SafetyCondition::Fault; +} + +struct SafetyPermit { + std::uint64_t epoch{0}; + + bool valid() const noexcept { return epoch != 0; } +}; + +struct RecoveryToken { + std::uint64_t epoch{0}; + + bool valid() const noexcept { return epoch != 0; } +}; + +struct SafetySnapshot { + SafetyCondition observed{SafetyCondition::Unknown}; + SafetyCondition latched_reason{SafetyCondition::Unknown}; + std::uint64_t epoch{0}; + bool latched{false}; + bool recovery_in_progress{false}; +}; + +// Hardware safety is an event, not a level. Once an unsafe state has been +// observed, returning to Normal only changes the observed level. A separate, +// explicit recovery must prove that the old controller operation has been +// cancelled before new motion permits can be issued. +class SafetyState final { +public: + SafetyState() = default; + + void observe(const SafetyCondition condition) + { + std::lock_guard lock(mutex_); + const bool changed = observed_ != condition; + observed_ = condition; + if (isMotionSafe(condition)) { + return; + } + + if (!latched_ || recovery_in_progress_ || changed) { + ++epoch_; + } + latched_ = true; + recovery_in_progress_ = false; + latched_reason_ = condition; + } + + std::optional tryPermit() const + { + std::lock_guard lock(mutex_); + if (latched_ || !isMotionSafe(observed_)) { + return std::nullopt; + } + return SafetyPermit{epoch_}; + } + + bool validate(const SafetyPermit permit) const + { + std::lock_guard lock(mutex_); + return permit.valid() && permit.epoch == epoch_ && !latched_ && + isMotionSafe(observed_); + } + + std::optional beginRecovery( + const std::uint64_t expected_epoch) + { + std::lock_guard lock(mutex_); + if (expected_epoch == 0 || expected_epoch != epoch_ || !latched_ || + recovery_in_progress_ || + !isMotionSafe(observed_)) { + return std::nullopt; + } + recovery_in_progress_ = true; + return RecoveryToken{epoch_}; + } + + bool completeRecovery( + const RecoveryToken token, + const bool robot_running, + const bool controller_idle, + const bool cancellation_confirmed) + { + std::lock_guard lock(mutex_); + if (!token.valid() || token.epoch != epoch_ || !latched_ || + !recovery_in_progress_ || !isMotionSafe(observed_) || + !robot_running || !controller_idle || + !cancellation_confirmed) { + return false; + } + + latched_ = false; + recovery_in_progress_ = false; + latched_reason_ = SafetyCondition::Unknown; + ++epoch_; + return true; + } + + void failRecovery(const RecoveryToken token) + { + std::lock_guard lock(mutex_); + if (token.valid() && token.epoch == epoch_) { + recovery_in_progress_ = false; + } + } + + SafetySnapshot snapshot() const + { + std::lock_guard lock(mutex_); + return { + observed_, + latched_reason_, + epoch_, + latched_, + recovery_in_progress_}; + } + +private: + mutable std::mutex mutex_; + SafetyCondition observed_{SafetyCondition::Unknown}; + SafetyCondition latched_reason_{SafetyCondition::Unknown}; + std::uint64_t epoch_{1}; + bool latched_{false}; + bool recovery_in_progress_{false}; +}; + +} // namespace cmvr::device::aubo_internal + +#endif // CMVR_ES_AUBO_SAFETY_STATE_H diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp index 3bb12777..bbeb9169 100644 --- a/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp +++ b/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp @@ -123,5 +123,34 @@ int main() CHECK_TRUE(recovered.started()); state.finish(recovered.token, MotionFinishMode::Clear); + const auto safety_motion = state.begin(MotionKind::Linear); + CHECK_TRUE(safety_motion.started()); + const auto safety_cancel = state.cancelActiveForSafety(); + CHECK_TRUE(safety_cancel.kind == MotionKind::Linear); + CHECK_TRUE(safety_cancel.tracked_motion); + CHECK_TRUE(state.cancelled(safety_motion.token)); + CHECK_TRUE(state.begin(MotionKind::Joint).status == + MotionStartStatus::Blocked); + state.finish(safety_motion.token); + const auto safety_stop = state.beginStop(); + CHECK_TRUE(safety_stop.started()); + CHECK_TRUE(safety_stop.kind == MotionKind::Linear); + CHECK_TRUE(state.completeStop()); + + const auto retained_speed = state.begin(MotionKind::Joint); + CHECK_TRUE(retained_speed.started()); + state.finish(retained_speed.token, MotionFinishMode::Retain); + const auto retained_cancel = state.cancelActiveForSafety(); + CHECK_TRUE(retained_cancel.kind == MotionKind::Joint); + CHECK_TRUE(retained_cancel.tracked_motion); + CHECK_TRUE(!retained_cancel.active_token.valid()); + CHECK_TRUE(state.begin(MotionKind::Linear).status == + MotionStartStatus::Blocked); + const auto retained_stop = state.beginStop(); + CHECK_TRUE(retained_stop.started()); + CHECK_TRUE(retained_stop.kind == MotionKind::Joint); + CHECK_TRUE(retained_stop.tracked_motion); + CHECK_TRUE(state.completeStop()); + return 0; } diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_safety_state_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_safety_state_test.cpp new file mode 100644 index 00000000..b38fc6c2 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/tests/aubo_safety_state_test.cpp @@ -0,0 +1,88 @@ +#include "devices/arm/aubo_arm/aubo_safety_state.h" + +#include + +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; + + SafetyState state; + CHECK_TRUE(!state.tryPermit().has_value()); + + state.observe(SafetyCondition::Normal); + const auto initial_permit = state.tryPermit(); + CHECK_TRUE(initial_permit.has_value()); + CHECK_TRUE(state.validate(*initial_permit)); + + CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Normal, 1) == + SafetyCondition::RobotEmergencyStop); + CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Normal, -1) == + SafetyCondition::Unknown); + CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Reduced, 0) == + SafetyCondition::Reduced); + CHECK_TRUE(needsProtectiveUnlock(SafetyCondition::ProtectiveStop)); + CHECK_TRUE(needsProtectiveUnlock(SafetyCondition::Violation)); + CHECK_TRUE(!needsProtectiveUnlock(SafetyCondition::SafeguardStop)); + CHECK_TRUE(needsInterfaceBoardRestart( + SafetyCondition::RobotEmergencyStop)); + CHECK_TRUE(needsInterfaceBoardRestart( + SafetyCondition::SystemEmergencyStop)); + CHECK_TRUE(needsInterfaceBoardRestart(SafetyCondition::Fault)); + CHECK_TRUE(!needsInterfaceBoardRestart(SafetyCondition::Recovery)); + + state.observe(SafetyCondition::RobotEmergencyStop); + CHECK_TRUE(!state.validate(*initial_permit)); + CHECK_TRUE(state.snapshot().latched); + CHECK_TRUE(!state.beginRecovery(state.snapshot().epoch).has_value()); + + // Releasing the hardware switch must not unlock motion by itself. + state.observe(SafetyCondition::Normal); + CHECK_TRUE(state.snapshot().latched); + CHECK_TRUE(!state.tryPermit().has_value()); + + const auto recovery = state.beginRecovery(state.snapshot().epoch); + CHECK_TRUE(recovery.has_value()); + CHECK_TRUE(!state.completeRecovery(*recovery, true, true, false)); + state.failRecovery(*recovery); + + const auto retry = state.beginRecovery(state.snapshot().epoch); + CHECK_TRUE(retry.has_value()); + CHECK_TRUE(state.completeRecovery(*retry, true, true, true)); + const auto recovered_permit = state.tryPermit(); + CHECK_TRUE(recovered_permit.has_value()); + CHECK_TRUE(state.validate(*recovered_permit)); + + // A new safety event invalidates an in-flight recovery token. + state.observe(SafetyCondition::ProtectiveStop); + state.observe(SafetyCondition::Reduced); + const auto stale_recovery = state.beginRecovery( + state.snapshot().epoch); + CHECK_TRUE(stale_recovery.has_value()); + state.observe(SafetyCondition::SafeguardStop); + state.observe(SafetyCondition::Normal); + CHECK_TRUE(!state.completeRecovery( + *stale_recovery, true, true, true)); + CHECK_TRUE(state.snapshot().latched); + + // An old API call must not begin recovery for a newer safety event. + const auto stale_epoch = state.snapshot().epoch; + state.observe(SafetyCondition::RobotEmergencyStop); + state.observe(SafetyCondition::Normal); + CHECK_TRUE(!state.beginRecovery(stale_epoch).has_value()); + CHECK_TRUE(!state.snapshot().recovery_in_progress); + + return 0; +}