diff --git a/cmvr-es/devices/arm/huayan_arm/CMakeLists.txt b/cmvr-es/devices/arm/huayan_arm/CMakeLists.txt index 617fab7d..6001a5d7 100644 --- a/cmvr-es/devices/arm/huayan_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/huayan_arm/CMakeLists.txt @@ -1,5 +1,7 @@ add_library(huayan_arm SHARED huayan_arm.cpp) +find_package(Threads REQUIRED) + set(HUAYAN_ARM_SDK_DIR ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/huayan_arm/v1.0) target_include_directories(huayan_arm @@ -20,9 +22,56 @@ target_link_libraries(huayan_arm PRIVATE HR_Pro glog + Threads::Threads ) add_library(cmvr_es::device::huayan_arm ALIAS huayan_arm) install(TARGETS huayan_arm LIBRARY DESTINATION lib) -install(FILES ${HUAYAN_ARM_SDK_DIR}/lib/libHR_Pro.so DESTINATION lib) \ No newline at end of file +install(FILES ${HUAYAN_ARM_SDK_DIR}/lib/libHR_Pro.so DESTINATION lib) + +if(BUILD_TESTING) + add_executable(huayan_lifecycle_state_test + tests/huayan_lifecycle_state_test.cpp + ) + target_include_directories(huayan_lifecycle_state_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(huayan_lifecycle_state_test + PRIVATE + Threads::Threads + ) + add_test( + NAME huayan_lifecycle_state_test + COMMAND huayan_lifecycle_state_test + ) + set_tests_properties(huayan_lifecycle_state_test PROPERTIES TIMEOUT 10) + + if(UNIX AND NOT APPLE) + add_executable(huayan_arm_sdk_test + tests/huayan_arm_sdk_test.cpp + ) + target_include_directories(huayan_arm_sdk_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${HUAYAN_ARM_SDK_DIR}/include + ) + target_link_libraries(huayan_arm_sdk_test + PRIVATE + cmvr_es::device::huayan_arm + Threads::Threads + ) + # Export the fake HRIF_* definitions so libhuayan_arm resolves its SDK + # calls to the deterministic test controller instead of real hardware. + target_link_options(huayan_arm_sdk_test PRIVATE -Wl,--export-dynamic) + add_test( + NAME huayan_arm_sdk_test + COMMAND huayan_arm_sdk_test + ) + set_tests_properties(huayan_arm_sdk_test PROPERTIES + TIMEOUT 20 + ENVIRONMENT "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + endif() +endif() diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp index 74ef039f..dbc2093a 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp @@ -2,20 +2,37 @@ #include #include +#include #include +#include +#include #include +#include -#include "huayan_arm/v1.0/include/HR_Pro.h" #include "common/base/logging/logger.h" +#include "huayan_arm/v1.0/include/HR_Pro.h" namespace cmvr::device { namespace { +using huayan_internal::MotionFinishMode; +using huayan_internal::MotionKind; +using huayan_internal::MotionStartStatus; +using huayan_internal::SafetyCondition; + constexpr double kPi = 3.14159265358979323846; constexpr double kDefaultMoveJVelocityDeg = 30.0; constexpr double kDefaultMoveJAccelerationDeg = 60.0; constexpr double kDefaultMoveLVelocityMm = 100.0; constexpr double kDefaultMoveLAccelerationMm = 200.0; +constexpr double kJointTargetToleranceRad = 0.002; +constexpr double kTcpPositionToleranceM = 0.0005; +constexpr double kTcpRotationToleranceRad = 0.003; +constexpr double kIdleVelocityToleranceRad = 0.01; +constexpr auto kSafetyPollPeriod = std::chrono::milliseconds(50); +constexpr auto kControllerStopTimeout = std::chrono::milliseconds(3000); +constexpr auto kOwnerExitTimeout = std::chrono::milliseconds(3000); +constexpr auto kCompletionCorrelationGrace = std::chrono::milliseconds(250); double radToDeg(const double value) { @@ -37,6 +54,18 @@ double mmToMeters(const double value) return value / 1000.0; } +double angularDistance(const double lhs, const double rhs) +{ + return std::abs(std::remainder(lhs - rhs, 2.0 * kPi)); +} + +std::int64_t monotonicNowNs() +{ + return std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + std::vector defaultJointNames(const std::size_t dof) { std::vector names; @@ -47,11 +76,13 @@ std::vector defaultJointNames(const std::size_t dof) return names; } -std::array toSix(const std::vector& values, const double fill = 0.0) +std::array toSix( + const std::vector& values, + const double fill = 0.0) { std::array out{fill, fill, fill, fill, fill, fill}; - const auto n = std::min(out.size(), values.size()); - for (std::size_t i = 0; i < n; ++i) { + const auto count = std::min(out.size(), values.size()); + for (std::size_t i = 0; i < count; ++i) { out[i] = values[i]; } return out; @@ -59,12 +90,13 @@ std::array toSix(const std::vector& values, const double fill std::vector poseToHrCoord(const CartesianPose& pose) { - return {metersToMm(pose.x), - metersToMm(pose.y), - metersToMm(pose.z), - radToDeg(pose.rx), - radToDeg(pose.ry), - radToDeg(pose.rz)}; + return { + metersToMm(pose.x), + metersToMm(pose.y), + metersToMm(pose.z), + radToDeg(pose.rx), + radToDeg(pose.ry), + radToDeg(pose.rz)}; } std::vector zeroHrFrame() @@ -72,8 +104,58 @@ std::vector zeroHrFrame() return {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; } +SafetyMode safetyModeFromCondition(const SafetyCondition condition) +{ + switch (condition) { + case SafetyCondition::Normal: + return SafetyMode::Normal; + case SafetyCondition::EmergencyStop: + case SafetyCondition::SoftwareEmergencyStop: + return SafetyMode::EmergencyStop; + case SafetyCondition::SafeguardStop: + return SafetyMode::SafeguardStop; + case SafetyCondition::SoftwareProtectiveStop: + return SafetyMode::ProtectiveStop; + case SafetyCondition::RobotFault: + case SafetyCondition::EmergencySignalFault: + case SafetyCondition::SafeguardSignalFault: + return SafetyMode::Fault; + case SafetyCondition::Unknown: + return SafetyMode::Unknown; + } + return SafetyMode::Unknown; +} + +bool isEmergencyCondition(const SafetyCondition condition) +{ + return condition == SafetyCondition::EmergencyStop || + condition == SafetyCondition::SoftwareEmergencyStop || + condition == SafetyCondition::EmergencySignalFault; +} + +bool isProtectiveCondition(const SafetyCondition condition) +{ + return condition == SafetyCondition::SafeguardStop || + condition == SafetyCondition::SoftwareProtectiveStop || + condition == SafetyCondition::SafeguardSignalFault; +} + } // namespace +struct HuayanRobot::RuntimeState { + huayan_internal::MotionState motion; + huayan_internal::SafetyState safety; + std::atomic monitor_running{true}; + std::atomic program_active{false}; + std::atomic termination_confirmed{false}; + std::atomic last_valid_sample_ns{0}; + std::atomic speed_completion_not_before_ns{0}; + mutable std::recursive_mutex termination_mutex; + mutable std::recursive_mutex submission_mutex; + mutable std::mutex wait_mutex; + std::condition_variable wait_cv; +}; + HuayanRobot::HuayanRobot(const config::RobotArmConfig& cfg) : cfg_(cfg) { @@ -84,14 +166,24 @@ HuayanRobot::HuayanRobot(const config::RobotArmConfig& cfg) ip_ = vendor_cfg_.ip(); port_ = vendor_cfg_.port() > 0 ? vendor_cfg_.port() : 10003; - tcp_name_ = vendor_cfg_.tool_frame().empty() ? "TCP" : vendor_cfg_.tool_frame(); - ucs_name_ = vendor_cfg_.base_frame().empty() ? "Base" : vendor_cfg_.base_frame(); + tcp_name_ = vendor_cfg_.tool_frame().empty() + ? "TCP" + : vendor_cfg_.tool_frame(); + ucs_name_ = vendor_cfg_.base_frame().empty() + ? "Base" + : vendor_cfg_.base_frame(); - const auto dof = vendor_cfg_.dof() > 0 ? static_cast(vendor_cfg_.dof()) : 6U; - model_.name = vendor_cfg_.model().empty() ? "HuayanRobot" : vendor_cfg_.model(); + const auto dof = vendor_cfg_.dof() > 0 + ? static_cast(vendor_cfg_.dof()) + : 6U; + model_.name = vendor_cfg_.model().empty() + ? "HuayanRobot" + : vendor_cfg_.model(); model_.manufacturer = "Huayan"; model_.dof = dof; - model_.joint_names.assign(vendor_cfg_.joint_names().begin(), vendor_cfg_.joint_names().end()); + model_.joint_names.assign( + vendor_cfg_.joint_names().begin(), + vendor_cfg_.joint_names().end()); if (model_.joint_names.empty()) { model_.joint_names = defaultJointNames(dof); } @@ -103,7 +195,23 @@ HuayanRobot::HuayanRobot(const config::RobotArmConfig& cfg) HuayanRobot::~HuayanRobot() { - (void)disconnect(); + const auto result = disconnect(); + if (result.ok()) { + return; + } + CMVR_LOG(ERROR) << "[HuayanRobot] destructor forced a best-effort disconnect " + << "after safe disconnect failed: " << result.message; + const auto runtime = runtimeSnapshot_(); + if (runtime) { + runtime->monitor_running.store(false); + runtime->wait_cv.notify_all(); + } + if (safety_monitor_thread_.joinable()) { + safety_monitor_thread_.join(); + } + std::lock_guard sdk_lock(sdk_mutex_); + (void)HRIF_DisConnect(box_id_); + connected_.store(false); } bool HuayanRobot::init() @@ -117,7 +225,12 @@ bool HuayanRobot::init() CMVR_LOG(ERROR) << "[HuayanRobot] init failed: " << result.message; return false; } - setSpeedScaling(1); + const auto scaling_result = setSpeedScaling(1.0); + if (!scaling_result.ok()) { + CMVR_LOG(ERROR) << "[HuayanRobot] set speed scaling failed: " + << scaling_result.message; + return false; + } return true; } @@ -129,20 +242,48 @@ bool HuayanRobot::stop() ArmState HuayanRobot::getRobotState() const { const auto hr_state = readHrState_(); + const auto runtime = runtimeSnapshot_(); + const auto safety = runtime + ? runtime->safety.snapshot() + : huayan_internal::SafetySnapshot{}; + const auto motion = runtime + ? runtime->motion.snapshot() + : huayan_internal::MotionSnapshot{}; ArmState state; state.connected = isConnected(); - state.powered_on = hr_state.valid ? hr_state.electrified != 0 : state.connected; - state.brake_released = hr_state.valid ? hr_state.brake != 0 : state.connected; - state.moving = hr_state.valid ? hr_state.moving != 0 : busy_.load(); - state.program_running = state.moving; - state.protective_stopped = hr_state.valid ? hr_state.safeguard != 0 : false; - state.emergency_stopped = hr_state.valid ? hr_state.emergency_stop != 0 : false; - state.fault = hr_state.valid ? hr_state.error != 0 : false; - state.robot_mode = getRobotMode(); - state.safety_mode = getSafetyMode(); + state.powered_on = hr_state.valid && hr_state.electrified != 0; + state.brake_released = hr_state.valid && hr_state.brake != 0; + state.moving = hr_state.valid && hr_state.moving != 0; + state.program_running = runtime && runtime->program_active.load(); + state.protective_stopped = safety.latched && + isProtectiveCondition(safety.latched_reason); + state.emergency_stopped = safety.latched && + isEmergencyCondition(safety.latched_reason); + state.fault = !hr_state.valid || hr_state.error != 0 || + (safety.latched && safetyModeFromCondition(safety.latched_reason) == SafetyMode::Fault); + if (!state.connected) { + state.robot_mode = RobotMode::Disconnected; + } else if (!hr_state.valid) { + state.robot_mode = RobotMode::Unknown; + } else if (state.fault) { + state.robot_mode = RobotMode::Fault; + } else if (safety.latched) { + state.robot_mode = RobotMode::Stopped; + } else if (hr_state.paused != 0) { + state.robot_mode = RobotMode::Paused; + } else if (hr_state.electrified == 0) { + state.robot_mode = RobotMode::PowerOff; + } else if (hr_state.moving != 0 || state.program_running || motion.owner_active) { + state.robot_mode = RobotMode::Running; + } else { + state.robot_mode = RobotMode::Idle; + } + state.safety_mode = safety.latched + ? safetyModeFromCondition(safety.latched_reason) + : safetyModeFromCondition(safety.observed); state.control_mode = getControlMode(); - state.speed_scaling = speed_scaling_; + state.speed_scaling = speed_scaling_.load(); state.actual_joint_state = getJointState(); state.target_joint_state = state.actual_joint_state; state.actual_tcp_pose = readTcpPose_(); @@ -153,15 +294,20 @@ ArmState HuayanRobot::getRobotState() const JointGroupState HuayanRobot::getJointState() const { JointGroupState state; - state.position = readJointPositionRad_(); - state.velocity = readJointVelocityRad_(); + state.position_valid = readJointPositionSample_(state.position); + state.velocity_valid = readJointVelocitySample_(state.velocity); state.effort.assign(model_.dof, 0.0); + state.effort_valid = false; + state.sample_monotonic_ns = monotonicNowNs(); return state; } CartesianPose HuayanRobot::getTcpPose(const FrameType frame) const { - (void)frame; + if (frame != FrameType::Base) { + CMVR_LOG(ERROR) << "[HuayanRobot] getTcpPose supports Base frame only"; + return {}; + } return readTcpPose_(); } @@ -170,42 +316,69 @@ RobotMode HuayanRobot::getRobotMode() const if (!isConnected()) { return RobotMode::Disconnected; } - const auto state = readHrState_(); if (!state.valid) { - return busy_.load() ? RobotMode::Running : RobotMode::Idle; + return RobotMode::Unknown; + } + const auto runtime = runtimeSnapshot_(); + if (runtime) { + const auto safety = runtime->safety.snapshot(); + if (safety.latched) { + const auto mode = safetyModeFromCondition(safety.latched_reason); + return mode == SafetyMode::Fault ? RobotMode::Fault : RobotMode::Stopped; + } } if (state.error != 0) { return RobotMode::Fault; } - if (state.emergency_stop != 0) { - return RobotMode::Stopped; - } if (state.paused != 0) { return RobotMode::Paused; } if (state.electrified == 0) { return RobotMode::PowerOff; } - return state.moving != 0 ? RobotMode::Running : RobotMode::Idle; + if (state.moving != 0 || (runtime && runtime->program_active.load())) { + return RobotMode::Running; + } + return RobotMode::Idle; } SafetyMode HuayanRobot::getSafetyMode() const { - const auto state = readHrState_(); - if (!state.valid) { + const auto runtime = runtimeSnapshot_(); + if (!runtime) { return SafetyMode::Unknown; } - if (state.error != 0) { - return SafetyMode::Fault; + (void)readHrState_(); + const auto safety = runtime->safety.snapshot(); + return safetyModeFromCondition( + safety.latched ? safety.latched_reason : safety.observed); +} + +ControlMode HuayanRobot::getControlMode() const +{ + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return ControlMode::None; } - if (state.emergency_stop != 0) { - return SafetyMode::EmergencyStop; + const auto motion = runtime->motion.snapshot(); + const auto kind = motion.owner_active + ? motion.active_kind + : motion.retained_kind; + switch (kind) { + case MotionKind::Joint: + case MotionKind::Linear: + return ControlMode::Position; + case MotionKind::SpeedJoint: + case MotionKind::SpeedLinear: + return ControlMode::Velocity; + case MotionKind::Servo: + return ControlMode::Servo; + case MotionKind::None: + case MotionKind::Program: + return ControlMode::None; } - if (state.safeguard != 0) { - return SafetyMode::SafeguardStop; - } - return SafetyMode::Normal; + return ControlMode::None; } Result HuayanRobot::torqueOn() @@ -214,20 +387,127 @@ Result HuayanRobot::torqueOn() if (!ready.ok()) { return ready; } - std::lock_guard lock(mutex_); - return hrResult_(HRIF_GrpEnable(box_id_, robot_id_), "GrpEnable"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] torqueOn failed: runtime is unavailable"); + } + + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + const auto safety = runtime->safety.snapshot(); + if (safety.latched) { + if (!isEmergencyCondition(safety.latched_reason)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] torqueOn cannot clear this safety latch; use the typed recovery API"); + } + return completeSafetyRecovery_( + "torqueOn", + runtime, + safety.epoch, + true, + software_protective_stopped_.load()); + } + if (!state.valid) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] torqueOn failed: safety state is unavailable"); + } + + int ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + ret = HRIF_GrpEnable(box_id_, robot_id_); + } + const auto result = hrResult_(ret, "GrpEnable"); + if (!result.ok()) { + return result; + } + const auto after = sampleHrState_(runtime); + publishHrState_(runtime, after); + if (!after.valid || after.enabled == 0 || after.electrified == 0 || + runtime->safety.snapshot().latched) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] torqueOn failed: enabled state was not confirmed"); + } + return Result::success(); } Result HuayanRobot::torqueOff() { - const auto ready = ensureConnected_("torqueOff"); if (!ready.ok()) { return ready; } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] torqueOff failed: runtime is unavailable"); + } + std::lock_guard termination_lock( + runtime->termination_mutex); + const auto stop_result = stopMotion(); + if (!stop_result.ok()) { + return stop_result; + } - std::lock_guard lock(mutex_); - return hrResult_(HRIF_GrpDisable(box_id_, robot_id_), "GrpDisable"); + huayan_internal::StopRequest poweroff_barrier; + int ret = 0; + { + // Keep a Stop generation active through GrpDisable. A command that + // raced the first Stop is cancelled here before it can submit. + std::lock_guard submission_lock( + runtime->submission_mutex); + poweroff_barrier = runtime->motion.beginStop(); + if (!poweroff_barrier.started()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] torqueOff failed: could not establish the power-off barrier"); + } + runtime->wait_cv.notify_all(); + if (!terminateController_( + runtime, + kControllerStopTimeout, + runtime->program_active.load() || + poweroff_barrier.kind == MotionKind::Program)) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] torqueOff failed: final controller Stop was not confirmed"); + } + std::lock_guard sdk_lock(sdk_mutex_); + ret = HRIF_GrpDisable(box_id_, robot_id_); + } + if (ret != 0) { + runtime->motion.failStop(); + return hrResult_(ret, "GrpDisable"); + } + const bool owner_exited = runtime->motion.waitForOwnerExit( + poweroff_barrier.active_token, kOwnerExitTimeout); + bool disabled = false; + const auto deadline = std::chrono::steady_clock::now() + + kControllerStopTimeout; + while (owner_exited && std::chrono::steady_clock::now() < deadline) { + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + if (state.valid && state.enabled == 0 && state.electrified == 0) { + disabled = true; + break; + } + std::unique_lock wait_lock(runtime->wait_mutex); + runtime->wait_cv.wait_for(wait_lock, kSafetyPollPeriod); + } + if (!owner_exited || !disabled || !runtime->motion.completeStop()) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] torqueOff failed: disabled state was not confirmed"); + } + return Result::success(); } Result HuayanRobot::calibrateZeroQ(const std::string& joint_name) @@ -238,105 +518,287 @@ Result HuayanRobot::calibrateZeroQ(const std::string& joint_name) Result HuayanRobot::emergencyStop() { + const auto ready = ensureConnected_("emergencyStop"); + if (!ready.ok()) { + return ready; + } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] emergencyStop failed: runtime is unavailable"); + } + software_emergency_stopped_.store(true); + { + std::lock_guard submission_lock( + runtime->submission_mutex); + runtime->safety.observe(SafetyCondition::SoftwareEmergencyStop); + runtime->termination_confirmed.store(false); + (void)runtime->motion.cancelActiveForSafety(); + } + runtime->wait_cv.notify_all(); + return stopMotion(); +} + +Result HuayanRobot::protectiveStop() +{ + const auto ready = ensureConnected_("protectiveStop"); + if (!ready.ok()) { + return ready; + } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] protectiveStop failed: runtime is unavailable"); + } + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + // Publish/cancel the local safety event before the vendor call while + // excluding every motion submission. No previously permitted command + // can slip in after EnterSafetyGuard and escape local cancellation. + software_protective_stopped_.store(true); + runtime->safety.observe(SafetyCondition::SoftwareProtectiveStop); + runtime->termination_confirmed.store(false); + (void)runtime->motion.cancelActiveForSafety(); + std::lock_guard sdk_lock(sdk_mutex_); + ret = HRIF_EnterSafetyGuard(box_id_, robot_id_, 1); + } + const auto result = hrResult_(ret, "EnterSafetyGuard"); + if (!result.ok()) { + // The event remains latched because the physical outcome is uncertain. + runtime->wait_cv.notify_all(); + return result; + } + runtime->wait_cv.notify_all(); return stopMotion(); } Result HuayanRobot::setSpeedScaling(const double scaling) { if (scaling < 0.0 || scaling > 1.0) { - return Result::failure(ArmErrorCode::InvalidArgument, "speed scaling must be in [0, 1]"); + return Result::failure( + ArmErrorCode::InvalidArgument, + "speed scaling must be in [0, 1]"); } - speed_scaling_ = scaling; - if (isConnected()) { - return hrResult_(HRIF_SetOverride(box_id_, robot_id_, scaling), "SetOverride"); + if (!isConnected()) { + speed_scaling_.store(scaling); + return Result::success(); } - return Result::success(); + int ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + ret = HRIF_SetOverride(box_id_, robot_id_, scaling); + } + const auto result = hrResult_(ret, "SetOverride"); + if (result.ok()) { + speed_scaling_.store(scaling); + } + return result; } bool HuayanRobot::isProtectiveStopped() const { - const auto state = readHrState_(); - return state.valid && state.safeguard != 0; + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return false; + } + (void)readHrState_(); + const auto safety = runtime->safety.snapshot(); + return safety.latched && isProtectiveCondition(safety.latched_reason); } bool HuayanRobot::isEmergencyStopped() const { - const auto state = readHrState_(); - return state.valid && state.emergency_stop != 0; + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return false; + } + (void)readHrState_(); + const auto safety = runtime->safety.snapshot(); + return safety.latched && isEmergencyCondition(safety.latched_reason); } bool HuayanRobot::isFault() const { + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return false; + } const auto state = readHrState_(); - return state.valid && state.error != 0; + const auto safety = runtime->safety.snapshot(); + return !state.valid || state.error != 0 || + (safety.latched && + safetyModeFromCondition(safety.latched_reason) == SafetyMode::Fault); } -Result HuayanRobot::moveJ(const JointPositionCommand& target, const MotionOptions& options) +Result HuayanRobot::moveJ( + const JointPositionCommand& target, + const MotionOptions& options) { std::string error; if (!validDof_(target.position.size(), error)) { return Result::failure(ArmErrorCode::InvalidDof, error); } - const auto ready = ensureConnected_("moveJ"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] moveJ failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("moveJ", runtime, permit); if (!ready.ok()) { return ready; } - if (busy_.exchange(true)) { - return Result::failure(ArmErrorCode::RobotNotReady, "[HuayanRobot] arm is busy: " + id_); + const auto start = runtime->motion.begin(MotionKind::Joint); + if (!start.started()) { + return motionStartFailure_("moveJ", start.status); + } + + if (targetReached_(&target.position, nullptr) && + controllerIdleStable_(runtime, std::chrono::milliseconds(300))) { + if (!runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] moveJ cancelled by a safety transition"); + } + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::success(); } std::array q_deg{}; - for (std::size_t i = 0; i < std::min(target.position.size(), q_deg.size()); ++i) { + for (std::size_t i = 0; + i < std::min(target.position.size(), q_deg.size()); + ++i) { q_deg[i] = radToDeg(target.position[i]); } - - const double velocity = options.velocity > 0.0 ? radToDeg(options.velocity) : kDefaultMoveJVelocityDeg; - const double acceleration = options.acceleration > 0.0 ? radToDeg(options.acceleration) : kDefaultMoveJAccelerationDeg; + const double velocity = options.velocity > 0.0 + ? radToDeg(options.velocity) + : kDefaultMoveJVelocityDeg; + const double acceleration = options.acceleration > 0.0 + ? radToDeg(options.acceleration) + : kDefaultMoveJAccelerationDeg; const double blend = metersToMm(options.blend_radius); - const std::string command_id = nextCommandId_(); + const auto command_id = nextCommandId_(); - const int ret = HRIF_MoveJ(box_id_, robot_id_, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5], - tcp_name_, ucs_name_, velocity * speed_scaling_, acceleration, blend, - 1, 0, 0, 0, command_id); + if (!runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] moveJ cancelled before submission by a safety transition"); + } + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] moveJ cancelled before submission"); + } + ret = HRIF_MoveJ( + box_id_, robot_id_, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5], + tcp_name_, ucs_name_, velocity * speed_scaling_.load(), acceleration, + blend, 1, 0, 0, 0, command_id); + } if (ret != 0) { - busy_.store(false); + runtime->motion.finish(start.token, MotionFinishMode::Clear); return hrResult_(ret, "moveJ"); } - const auto wait_result = waitMotionDone_("moveJ", 60000); - busy_.store(false); - return wait_result; + admission_lock.unlock(); + return waitMotionDone_( + "moveJ", runtime, start.token, permit, command_id, + &target.position, nullptr, 60000); } -Result HuayanRobot::speedJ(const JointVelocityCommand& velocity, const double acceleration, const double duration) +Result HuayanRobot::speedJ( + const JointVelocityCommand& velocity, + const double acceleration, + const double duration) { std::string error; if (!validDof_(velocity.velocity.size(), error)) { return Result::failure(ArmErrorCode::InvalidDof, error); } - const auto ready = ensureConnected_("speedJ"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] speedJ failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("speedJ", runtime, permit); if (!ready.ok()) { return ready; } + const auto start = runtime->motion.begin(MotionKind::SpeedJoint); + if (!start.started()) { + return motionStartFailure_("speedJ", start.status); + } + + const bool zero_command = std::all_of( + velocity.velocity.begin(), velocity.velocity.end(), + [](const double value) { return std::abs(value) < 1e-12; }); + if (zero_command) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::success(); + } std::array qd_deg{}; - for (std::size_t i = 0; i < std::min(velocity.velocity.size(), qd_deg.size()); ++i) { + for (std::size_t i = 0; + i < std::min(velocity.velocity.size(), qd_deg.size()); + ++i) { qd_deg[i] = radToDeg(velocity.velocity[i]); } - const double acc_deg = acceleration > 0.0 ? radToDeg(acceleration) : kDefaultMoveJAccelerationDeg; - const double runtime = duration > 0.0 ? duration : 0.1; - - const int ret = HRIF_SpeedJ(box_id_, robot_id_, - qd_deg[0], qd_deg[1], qd_deg[2], qd_deg[3], qd_deg[4], qd_deg[5], - acc_deg, runtime); + const double acceleration_deg = acceleration > 0.0 + ? radToDeg(acceleration) + : kDefaultMoveJAccelerationDeg; + const double run_time = duration > 0.0 ? duration : 0.1; + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] speedJ cancelled before submission by a safety transition"); + } + ret = HRIF_SpeedJ( + box_id_, robot_id_, + qd_deg[0], qd_deg[1], qd_deg[2], + qd_deg[3], qd_deg[4], qd_deg[5], + acceleration_deg, run_time); + if (ret == 0) { + runtime->speed_completion_not_before_ns.store( + monotonicNowNs() + + static_cast(run_time * 1e9)); + } + } if (ret != 0) { - busy_.store(false); + runtime->speed_completion_not_before_ns.store(0); + runtime->motion.finish(start.token, MotionFinishMode::Clear); return hrResult_(ret, "SpeedJ"); } - const auto wait_result = waitMotionDone_("SpeedJ", 60000); - busy_.store(false); - return wait_result; + admission_lock.unlock(); + return waitMotionDone_( + "SpeedJ", runtime, start.token, permit, {}, nullptr, nullptr, + std::max(3000, static_cast(run_time * 1000.0) + 3000)); } Result HuayanRobot::stopJ(const double acceleration) @@ -345,76 +807,191 @@ Result HuayanRobot::stopJ(const double acceleration) return stopMotion(); } -Result HuayanRobot::moveL(const CartesianPose& target, const MotionOptions& options, const FrameType frame) +Result HuayanRobot::moveL( + const CartesianPose& target, + const MotionOptions& options, + const FrameType frame) { - (void)frame; - const auto ready = ensureConnected_("moveL"); + if (frame != FrameType::Base) { + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "[HuayanRobot] moveL supports Base frame only"); + } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] moveL failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("moveL", runtime, permit); if (!ready.ok()) { return ready; } - if (busy_.exchange(true)) { - return Result::failure(ArmErrorCode::RobotNotReady, "[HuayanRobot] arm is busy: " + id_); + const auto start = runtime->motion.begin(MotionKind::Linear); + if (!start.started()) { + return motionStartFailure_("moveL", start.status); } - const auto pose = poseToHrCoord(target); - const auto q_deg = toSix(currentJointPositionDeg_()); - const double velocity = options.velocity > 0.0 ? metersToMm(options.velocity) : kDefaultMoveLVelocityMm; - const double acceleration = options.acceleration > 0.0 ? metersToMm(options.acceleration) : kDefaultMoveLAccelerationMm; - const double blend = metersToMm(options.blend_radius); - const std::string command_id = nextCommandId_(); + if (targetReached_(nullptr, &target) && + controllerIdleStable_(runtime, std::chrono::milliseconds(300))) { + if (!runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] moveL cancelled by a safety transition"); + } + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::success(); + } - const int ret = HRIF_MoveL(box_id_, robot_id_, - pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], - q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5], - tcp_name_, ucs_name_, velocity * speed_scaling_, acceleration, blend, - 0, 0, 0, command_id); + std::vector q_rad; + if (!readJointPositionSample_(q_rad)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] moveL failed: unable to read reference joints"); + } + for (auto& value : q_rad) { + value = radToDeg(value); + } + const auto q_deg = toSix(q_rad); + const auto pose = poseToHrCoord(target); + const double velocity = options.velocity > 0.0 + ? metersToMm(options.velocity) + : kDefaultMoveLVelocityMm; + const double acceleration = options.acceleration > 0.0 + ? metersToMm(options.acceleration) + : kDefaultMoveLAccelerationMm; + const double blend = metersToMm(options.blend_radius); + const auto command_id = nextCommandId_(); + + if (!runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] moveL cancelled before submission by a safety transition"); + } + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] moveL cancelled before submission"); + } + ret = HRIF_MoveL( + box_id_, robot_id_, + pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], + q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5], + tcp_name_, ucs_name_, velocity * speed_scaling_.load(), acceleration, + blend, 0, 0, 0, command_id); + } if (ret != 0) { - busy_.store(false); + runtime->motion.finish(start.token, MotionFinishMode::Clear); return hrResult_(ret, "moveL"); } - const auto wait_result = waitMotionDone_("moveL", 60000); - busy_.store(false); - return wait_result; + admission_lock.unlock(); + return waitMotionDone_( + "moveL", runtime, start.token, permit, command_id, + nullptr, &target, 60000); } -Result HuayanRobot::speedL(const CartesianVelocity& velocity, - const double acceleration, - const double duration, - const FrameType frame) +Result HuayanRobot::speedL( + const CartesianVelocity& velocity, + const double acceleration, + const double duration, + const FrameType frame) { - const auto ready = ensureConnected_("speedL"); + if (frame != FrameType::Base) { + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "[HuayanRobot] speedL supports Base frame only"); + } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] speedL failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("speedL", runtime, permit); if (!ready.ok()) { return ready; } - const double vx_mm = metersToMm(velocity.vx); - const double vy_mm = metersToMm(velocity.vy); - const double vz_mm = metersToMm(velocity.vz); - const double wx_deg = radToDeg(velocity.wx); - const double wy_deg = radToDeg(velocity.wy); - const double wz_deg = radToDeg(velocity.wz); + const auto start = runtime->motion.begin(MotionKind::SpeedLinear); + if (!start.started()) { + return motionStartFailure_("speedL", start.status); + } - const double linear_acc_mm = - acceleration > 0.0 ? metersToMm(acceleration) : kDefaultMoveLAccelerationMm; + const bool zero_command = + std::abs(velocity.vx) < 1e-12 && + std::abs(velocity.vy) < 1e-12 && + std::abs(velocity.vz) < 1e-12 && + std::abs(velocity.wx) < 1e-12 && + std::abs(velocity.wy) < 1e-12 && + std::abs(velocity.wz) < 1e-12; + if (zero_command) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::success(); + } - const double angular_acc_deg = - acceleration > 0.0 ? radToDeg(acceleration) : kDefaultMoveJAccelerationDeg; - - const double runtime = duration > 0.0 ? duration : 0.5; - - std::lock_guard lock(mutex_); - servo_mode_.store(false); - const int ret = HRIF_SpeedL(box_id_, robot_id_, vx_mm, vy_mm, vz_mm, - wx_deg, wy_deg, wz_deg, linear_acc_mm, angular_acc_deg, runtime); + const double linear_acceleration = acceleration > 0.0 + ? metersToMm(acceleration) + : kDefaultMoveLAccelerationMm; + const double angular_acceleration = acceleration > 0.0 + ? radToDeg(acceleration) + : kDefaultMoveJAccelerationDeg; + const double run_time = duration > 0.0 ? duration : 0.5; + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] speedL cancelled before submission by a safety transition"); + } + ret = HRIF_SpeedL( + box_id_, robot_id_, + metersToMm(velocity.vx), + metersToMm(velocity.vy), + metersToMm(velocity.vz), + radToDeg(velocity.wx), + radToDeg(velocity.wy), + radToDeg(velocity.wz), + linear_acceleration, + angular_acceleration, + run_time); + if (ret == 0) { + runtime->speed_completion_not_before_ns.store( + monotonicNowNs() + + static_cast(run_time * 1e9)); + } + } if (ret != 0) { - busy_.store(false); + runtime->speed_completion_not_before_ns.store(0); + runtime->motion.finish(start.token, MotionFinishMode::Clear); return hrResult_(ret, "SpeedL"); } - const auto wait_result = waitMotionDone_("SpeedL", 60000); - busy_.store(false); - return wait_result; + admission_lock.unlock(); + return waitMotionDone_( + "SpeedL", runtime, start.token, permit, {}, nullptr, nullptr, + std::max(3000, static_cast(run_time * 1000.0) + 3000)); } -Result HuayanRobot::stopL(std::optional acceleration = std::nullopt) +Result HuayanRobot::stopL(const std::optional acceleration) { (void)acceleration; return stopMotion(); @@ -422,31 +999,113 @@ Result HuayanRobot::stopL(std::optional acceleration = std::nullopt) Result HuayanRobot::stopMotion() { - if (!isConnected()) { - busy_.store(false); + if (!connected_.load()) { servo_mode_.store(false); return Result::success(); } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] stopMotion failed: runtime is unavailable"); + } - const auto result = hrResult_(HRIF_GrpStop(box_id_, robot_id_), "GrpStop"); - busy_.store(false); + std::lock_guard termination_lock( + runtime->termination_mutex); + huayan_internal::StopRequest request; + bool stopped = false; + { + // Linearize cancellation and the vendor Stop with command submission, + // then release this lock before waiting for the old owner. The owner + // may need publishHrState_ (and therefore submission_mutex) in order to + // observe cancellation and exit. + std::lock_guard submission_lock( + runtime->submission_mutex); + request = runtime->motion.beginStop(); + if (!request.started()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] stopMotion rejected: another Stop is in progress"); + } + runtime->wait_cv.notify_all(); + stopped = terminateController_( + runtime, + kControllerStopTimeout, + runtime->program_active.load() || request.kind == MotionKind::Program); + } + const bool owner_exited = runtime->motion.waitForOwnerExit( + request.active_token, + kOwnerExitTimeout); + if (!stopped || !owner_exited || !runtime->motion.completeStop()) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] stopMotion failed: controller idle was not confirmed"); + } servo_mode_.store(false); - return result; + return Result::success(); } Result HuayanRobot::startServoMode(const ServoOptions& options) { - const auto ready = ensureConnected_("startServoMode"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] startServoMode failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("startServoMode", runtime, permit); if (!ready.ok()) { return ready; } - const double period = options.period > 0.0 ? options.period : 0.008; - const double lookahead = options.lookahead_time > 0.0 ? options.lookahead_time : 0.1; - const auto result = hrResult_(HRIF_StartServo(box_id_, robot_id_, period, lookahead), "StartServo"); - if (result.ok()) { - servo_mode_.store(true); + if (servo_mode_.load()) { + return Result::success(); } - return result; + const auto start = runtime->motion.begin(MotionKind::Servo); + if (!start.started()) { + return motionStartFailure_("startServoMode", start.status); + } + const double period = options.period > 0.0 ? options.period : 0.008; + const double lookahead = options.lookahead_time > 0.0 + ? options.lookahead_time + : 0.1; + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] startServoMode cancelled by a safety transition"); + } + ret = HRIF_StartServo(box_id_, robot_id_, period, lookahead); + } + if (ret != 0) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return hrResult_(ret, "StartServo"); + } + { + std::lock_guard submission_lock( + runtime->submission_mutex); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + (void)runtime->motion.cancelActiveForSafety(); + runtime->motion.finish(start.token, MotionFinishMode::Clear); + runtime->wait_cv.notify_all(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] startServoMode cancelled by a safety transition"); + } + servo_mode_.store(true); + runtime->motion.finish(start.token, MotionFinishMode::Retain); + } + return Result::success(); } Result HuayanRobot::servoJ(const JointPositionCommand& target) @@ -455,32 +1114,137 @@ Result HuayanRobot::servoJ(const JointPositionCommand& target) if (!validDof_(target.position.size(), error)) { return Result::failure(ArmErrorCode::InvalidDof, error); } - const auto ready = ensureConnected_("servoJ"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] servoJ failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + if (!servo_mode_.load()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] servoJ failed: servo mode is not active"); + } + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("servoJ", runtime, permit); if (!ready.ok()) { return ready; } - + const auto start = runtime->motion.begin(MotionKind::Servo, true); + if (!start.started()) { + return motionStartFailure_("servoJ", start.status); + } std::array q_deg{}; - for (std::size_t i = 0; i < std::min(target.position.size(), q_deg.size()); ++i) { + for (std::size_t i = 0; + i < std::min(target.position.size(), q_deg.size()); + ++i) { q_deg[i] = radToDeg(target.position[i]); } - return hrResult_(HRIF_PushServoJ(box_id_, robot_id_, - q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5]), - "servoJ"); + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] servoJ cancelled by a safety transition"); + } + ret = HRIF_PushServoJ( + box_id_, robot_id_, + q_deg[0], q_deg[1], q_deg[2], + q_deg[3], q_deg[4], q_deg[5]); + } + if (ret != 0) { + runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); + return hrResult_(ret, "servoJ"); + } + { + std::lock_guard submission_lock( + runtime->submission_mutex); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); + runtime->wait_cv.notify_all(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] servoJ cancelled by a safety transition"); + } + runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); + } + return Result::success(); } -Result HuayanRobot::servoL(const CartesianPose& target, const FrameType frame) +Result HuayanRobot::servoL( + const CartesianPose& target, + const FrameType frame) { - (void)frame; - const auto ready = ensureConnected_("servoL"); + if (frame != FrameType::Base) { + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "[HuayanRobot] servoL supports Base frame only"); + } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] servoL failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + if (!servo_mode_.load()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] servoL failed: servo mode is not active"); + } + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("servoL", runtime, permit); if (!ready.ok()) { return ready; } - + const auto start = runtime->motion.begin(MotionKind::Servo, true); + if (!start.started()) { + return motionStartFailure_("servoL", start.status); + } auto coord = poseToHrCoord(target); auto ucs = zeroHrFrame(); auto tcp = zeroHrFrame(); - return hrResult_(HRIF_PushServoP(box_id_, robot_id_, coord, ucs, tcp), "servoL"); + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] servoL cancelled by a safety transition"); + } + ret = HRIF_PushServoP(box_id_, robot_id_, coord, ucs, tcp); + } + if (ret != 0) { + runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); + return hrResult_(ret, "servoL"); + } + { + std::lock_guard submission_lock( + runtime->submission_mutex); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); + runtime->wait_cv.notify_all(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] servoL cancelled by a safety transition"); + } + runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); + } + return Result::success(); } Result HuayanRobot::servoSpeedJ(const JointVelocityCommand& velocity) @@ -489,7 +1253,9 @@ Result HuayanRobot::servoSpeedJ(const JointVelocityCommand& velocity) return unsupported_("servoSpeedJ"); } -Result HuayanRobot::servoSpeedL(const CartesianVelocity& velocity, const FrameType frame) +Result HuayanRobot::servoSpeedL( + const CartesianVelocity& velocity, + const FrameType frame) { (void)velocity; (void)frame; @@ -498,52 +1264,221 @@ Result HuayanRobot::servoSpeedL(const CartesianVelocity& velocity, const FrameTy Result HuayanRobot::stopServoMode() { - servo_mode_.store(false); return stopMotion(); } Result HuayanRobot::connect(const std::string& ip, const int port) { - if (connected_.load()) { + if (isConnected()) { return Result::success(); } if (ip.empty()) { - return Result::failure(ArmErrorCode::InvalidArgument, "[HuayanRobot] ip is empty"); + return Result::failure( + ArmErrorCode::InvalidArgument, + "[HuayanRobot] ip is empty"); } - std::lock_guard lock(mutex_); - const int use_port = port > 0 ? port : 10003; - const auto result = hrResult_(HRIF_Connect(box_id_, ip.c_str(), static_cast(use_port)), - "Connect"); - if (!result.ok()) { - connected_.store(false); - return result; + // A dropped transport can leave the local flag, monitor and an old waiter + // alive. Cancel and join that generation before a new SDK session can be + // created; otherwise the old waiter could issue HRIF reads against it. + const auto stale_runtime = runtimeSnapshot_(); + if (stale_runtime) { + huayan_internal::SafetyCancelResult cancelled; + { + std::unique_lock termination_lock( + stale_runtime->termination_mutex); + std::lock_guard submission_lock( + stale_runtime->submission_mutex); + cancelled = stale_runtime->motion.cancelActiveForSafety(); + connected_.store(false); + stale_runtime->monitor_running.store(false); + stale_runtime->wait_cv.notify_all(); + } + const bool owner_exited = stale_runtime->motion.waitForOwnerExit( + cancelled.active_token, kOwnerExitTimeout); + if (safety_monitor_thread_.joinable()) { + safety_monitor_thread_.join(); + } + if (!owner_exited) { + stale_runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] reconnect failed: the old command owner did not exit"); + } + { + std::lock_guard sdk_lock(sdk_mutex_); + (void)HRIF_DisConnect(box_id_); + } + { + std::lock_guard state_lock(mutex_); + connected_.store(false); + servo_mode_.store(false); + runtime_.reset(); + } } - ip_ = ip; - port_ = use_port; - connected_.store(true); + + const int use_port = port > 0 ? port : 10003; + int ret = 0; + { + std::lock_guard state_lock(mutex_); + std::lock_guard sdk_lock(sdk_mutex_); + ret = HRIF_Connect( + box_id_, ip.c_str(), static_cast(use_port)); + if (ret == 0) { + ip_ = ip; + port_ = use_port; + runtime_ = std::make_shared(); + connected_.store(true); + servo_mode_.store(false); + software_emergency_stopped_.store(false); + software_protective_stopped_.store(false); + } + } + if (ret != 0) { + connected_.store(false); + return hrResult_(ret, "Connect"); + } + + const auto runtime = runtimeSnapshot_(); + const auto initial_state = sampleHrState_(runtime); + publishHrState_(runtime, initial_state); + if (!initial_state.valid) { + { + std::lock_guard sdk_lock(sdk_mutex_); + (void)HRIF_DisConnect(box_id_); + } + std::lock_guard state_lock(mutex_); + connected_.store(false); + runtime_.reset(); + return Result::failure( + ArmErrorCode::ConnectionFailed, + "[HuayanRobot] Connect failed: initial safety state is unavailable"); + } + + // A reconnect must not replace the software generation state while an old + // controller waypoint or box-wide script can still resume. Stop both and + // require three stable idle samples before the first motion permit exists. + if (!terminateController_(runtime, kControllerStopTimeout, true)) { + { + std::lock_guard sdk_lock(sdk_mutex_); + (void)HRIF_DisConnect(box_id_); + } + std::lock_guard state_lock(mutex_); + connected_.store(false); + runtime_.reset(); + return Result::failure( + ArmErrorCode::ConnectionFailed, + "[HuayanRobot] Connect failed: stale controller work could not be cleared"); + } + + safety_monitor_thread_ = std::thread( + &HuayanRobot::safetyMonitorLoop_, this, runtime); return Result::success(); } Result HuayanRobot::disconnect() { - if (connected_.load() || HRIF_IsConnected(box_id_)) { - const auto result = hrResult_(HRIF_DisConnect(box_id_), "DisConnect"); - connected_.store(false); - busy_.store(false); - servo_mode_.store(false); - return result; + const auto runtime = runtimeSnapshot_(); + if (!runtime && !connected_.load()) { + return Result::success(); } - connected_.store(false); - busy_.store(false); - servo_mode_.store(false); - return Result::success(); + const bool transport_connected = isConnected(); + huayan_internal::StopRequest disconnect_barrier; + bool owner_exited = true; + bool stop_confirmed = !connected_.load(); + int disconnect_ret = 0; + std::unique_lock termination_lock; + if (runtime) { + termination_lock = std::unique_lock( + runtime->termination_mutex); + } + + if (connected_.load() && runtime && transport_connected) { + const auto stop_result = stopMotion(); + if (!stop_result.ok()) { + // Keep the monitor, safety latch and runtime ownership alive. A + // failed Stop must not be hidden by throwing away local state. + return stop_result; + } + stop_confirmed = true; + } + + if (runtime) { + { + std::lock_guard submission_lock( + runtime->submission_mutex); + disconnect_barrier = runtime->motion.beginStop(); + if (!disconnect_barrier.started()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] disconnect failed: could not establish the disconnect barrier"); + } + runtime->wait_cv.notify_all(); + if (transport_connected) { + stop_confirmed = terminateController_( + runtime, + kControllerStopTimeout, + runtime->program_active.load() || + disconnect_barrier.kind == MotionKind::Program); + if (!stop_confirmed) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] disconnect failed: final controller Stop was not confirmed"); + } + } + std::lock_guard sdk_lock(sdk_mutex_); + if (connected_.load() || HRIF_IsConnected(box_id_)) { + disconnect_ret = HRIF_DisConnect(box_id_); + } + if (disconnect_ret != 0 && transport_connected) { + runtime->motion.failStop(); + return hrResult_(disconnect_ret, "DisConnect"); + } + connected_.store(false); + runtime->monitor_running.store(false); + } + runtime->wait_cv.notify_all(); + owner_exited = runtime->motion.waitForOwnerExit( + disconnect_barrier.active_token, kOwnerExitTimeout); + if (stop_confirmed && owner_exited) { + (void)runtime->motion.completeStop(); + } else { + runtime->motion.failStop(); + } + } else { + connected_.store(false); + } + + if (termination_lock.owns_lock()) { + termination_lock.unlock(); + } + if (safety_monitor_thread_.joinable()) { + safety_monitor_thread_.join(); + } + { + std::lock_guard state_lock(mutex_); + servo_mode_.store(false); + software_emergency_stopped_.store(false); + software_protective_stopped_.store(false); + runtime_.reset(); + } + if (!stop_confirmed || !owner_exited) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] disconnect completed locally, but controller Stop was not confirmed before transport loss"); + } + return hrResult_(disconnect_ret, "DisConnect"); } bool HuayanRobot::isConnected() const { - return connected_.load() && HRIF_IsConnected(box_id_); + if (!connected_.load()) { + return false; + } + std::lock_guard sdk_lock(sdk_mutex_); + return HRIF_IsConnected(box_id_); } Result HuayanRobot::shutdown() @@ -551,11 +1486,81 @@ Result HuayanRobot::shutdown() if (!isConnected()) { return Result::success(); } - auto result = hrResult_(HRIF_ShutdownRobot(box_id_), "ShutdownRobot"); - if (!result.ok()) { - return result; + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] shutdown failed: runtime is unavailable"); } - return disconnect(); + const auto poweroff_result = torqueOff(); + if (!poweroff_result.ok()) { + return poweroff_result; + } + + std::unique_lock termination_lock( + runtime->termination_mutex); + huayan_internal::StopRequest shutdown_barrier; + int shutdown_ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + shutdown_barrier = runtime->motion.beginStop(); + if (!shutdown_barrier.started()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] shutdown failed: could not establish the shutdown barrier"); + } + runtime->wait_cv.notify_all(); + if (!terminateController_( + runtime, + kControllerStopTimeout, + runtime->program_active.load() || + shutdown_barrier.kind == MotionKind::Program)) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] shutdown failed: final controller Stop was not confirmed"); + } + std::lock_guard sdk_lock(sdk_mutex_); + shutdown_ret = HRIF_ShutdownRobot(box_id_); + if (shutdown_ret == 0) { + connected_.store(false); + runtime->monitor_running.store(false); + } + } + if (shutdown_ret != 0) { + runtime->motion.failStop(); + return hrResult_(shutdown_ret, "ShutdownRobot"); + } + runtime->wait_cv.notify_all(); + const bool owner_exited = runtime->motion.waitForOwnerExit( + shutdown_barrier.active_token, kOwnerExitTimeout); + if (owner_exited) { + (void)runtime->motion.completeStop(); + } else { + runtime->motion.failStop(); + } + termination_lock.unlock(); + if (safety_monitor_thread_.joinable()) { + safety_monitor_thread_.join(); + } + { + std::lock_guard sdk_lock(sdk_mutex_); + (void)HRIF_DisConnect(box_id_); + } + { + std::lock_guard state_lock(mutex_); + servo_mode_.store(false); + software_emergency_stopped_.store(false); + software_protective_stopped_.store(false); + runtime_.reset(); + } + if (!owner_exited) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] shutdown succeeded, but a raced command owner did not exit cleanly"); + } + return Result::success(); } Result HuayanRobot::clearFault() @@ -564,22 +1569,169 @@ Result HuayanRobot::clearFault() if (!ready.ok()) { return ready; } - return hrResult_(HRIF_GrpReset(box_id_, robot_id_), "GrpReset"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] clearFault failed: runtime is unavailable"); + } + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + const auto safety = runtime->safety.snapshot(); + if (safety.latched) { + if (safety.latched_reason == SafetyCondition::RobotFault || + safety.latched_reason == SafetyCondition::Unknown) { + return completeSafetyRecovery_( + "clearFault", + runtime, + safety.epoch, + false, + false); + } + // Resetting a controller fault is useful after a physical E-stop, but + // this API deliberately does not clear that differently typed latch. + int reset_ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + reset_ret = HRIF_GrpReset(box_id_, robot_id_); + } + return hrResult_(reset_ret, "GrpReset"); + } + int ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + ret = HRIF_GrpReset(box_id_, robot_id_); + } + return hrResult_(ret, "GrpReset"); +} + +Result HuayanRobot::unlockProtectiveStop() +{ + const auto ready = ensureConnected_("unlockProtectiveStop"); + if (!ready.ok()) { + return ready; + } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] unlockProtectiveStop failed: runtime is unavailable"); + } + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + const auto safety = runtime->safety.snapshot(); + if (!safety.latched) { + return Result::success(); + } + if (!isProtectiveCondition(safety.latched_reason) && + !software_protective_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "[HuayanRobot] unlockProtectiveStop rejected: the latched stop is not protective"); + } + return completeSafetyRecovery_( + "unlockProtectiveStop", + runtime, + safety.epoch, + false, + software_protective_stopped_.load()); } Result HuayanRobot::loadProgram(const std::string& program_name) { - (void)program_name; - return Result::success(); + if (program_name.empty()) { + return Result::failure( + ArmErrorCode::InvalidArgument, + "[HuayanRobot] loadProgram failed: program name is empty"); + } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] loadProgram failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("loadProgram", runtime, permit); + if (!ready.ok()) { + return ready; + } + const auto start = runtime->motion.begin(MotionKind::Program); + if (!start.started()) { + return motionStartFailure_("loadProgram", start.status); + } + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] loadProgram cancelled by a safety transition"); + } + ret = HRIF_SwitchScript(box_id_, robot_id_, program_name); + runtime->motion.finish(start.token, MotionFinishMode::Clear); + } + return hrResult_(ret, "SwitchScript"); } Result HuayanRobot::playProgram() { - const auto ready = ensureConnected_("playProgram"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] playProgram failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("playProgram", runtime, permit); if (!ready.ok()) { return ready; } - return hrResult_(HRIF_StartScript(box_id_), "StartScript"); + const auto start = runtime->motion.begin(MotionKind::Program); + if (!start.started()) { + return motionStartFailure_("playProgram", start.status); + } + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] playProgram cancelled by a safety transition"); + } + ret = HRIF_StartScript(box_id_); + } + if (ret != 0) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return hrResult_(ret, "StartScript"); + } + { + std::lock_guard submission_lock( + runtime->submission_mutex); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + (void)runtime->motion.cancelActiveForSafety(); + runtime->motion.finish(start.token, MotionFinishMode::Clear); + runtime->wait_cv.notify_all(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] playProgram cancelled by a safety transition"); + } + runtime->program_active.store(true); + runtime->motion.finish(start.token, MotionFinishMode::Retain); + } + return Result::success(); } Result HuayanRobot::pauseProgram() @@ -588,7 +1740,38 @@ Result HuayanRobot::pauseProgram() if (!ready.ok()) { return ready; } - return hrResult_(HRIF_PauseScript(box_id_), "PauseScript"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] pauseProgram failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + if (!runtime || !runtime->program_active.load()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] pauseProgram failed: no tracked program is active"); + } + const auto safety = runtime->safety.tryPermit(); + if (!safety) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] pauseProgram rejected by the safety latch"); + } + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (!runtime->safety.validate(*safety)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] pauseProgram cancelled by a safety transition"); + } + ret = HRIF_PauseScript(box_id_); + } + return hrResult_(ret, "PauseScript"); } Result HuayanRobot::stopProgram() @@ -597,12 +1780,45 @@ Result HuayanRobot::stopProgram() if (!ready.ok()) { return ready; } - return hrResult_(HRIF_StopScript(box_id_), "StopScript"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] stopProgram failed: runtime is unavailable"); + } + std::lock_guard termination_lock( + runtime->termination_mutex); + huayan_internal::StopRequest request; + bool stopped = false; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + request = runtime->motion.beginStop(MotionKind::Program); + if (!request.started()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] stopProgram rejected: another Stop is in progress"); + } + runtime->wait_cv.notify_all(); + stopped = terminateController_( + runtime, kControllerStopTimeout, true); + } + const bool owner_exited = runtime->motion.waitForOwnerExit( + request.active_token, kOwnerExitTimeout); + if (!stopped || !owner_exited || !runtime->motion.completeStop()) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] stopProgram failed: controller idle was not confirmed"); + } + servo_mode_.store(false); + return Result::success(); } -std::vector HuayanRobot::ik(const std::string& base_link, - const std::string& ee_link, - const CartesianPose& pose) +std::vector HuayanRobot::ik( + const std::string& base_link, + const std::string& ee_link, + const CartesianPose& pose) { (void)base_link; (void)ee_link; @@ -611,7 +1827,9 @@ std::vector HuayanRobot::ik(const std::string& base_link, return {}; } -CartesianPose HuayanRobot::fk(const std::string& base_link, const std::string& ee_link) +CartesianPose HuayanRobot::fk( + const std::string& base_link, + const std::string& ee_link) { (void)base_link; (void)ee_link; @@ -629,50 +1847,163 @@ CartesianVelocity HuayanRobot::getSpeedLCommandTwistBase() const return readTcpVelocity_(); } +bool HuayanRobot::busy() const +{ + const auto runtime = runtimeSnapshot_(); + return runtime && runtime->motion.busy(); +} + Result HuayanRobot::ensureConnected_(const std::string& context) const { if (!isConnected()) { - return Result::failure(ArmErrorCode::NotConnected, - "[HuayanRobot] " + context + " failed: arm is not connected"); + return Result::failure( + ArmErrorCode::NotConnected, + "[HuayanRobot] " + context + " failed: arm is not connected"); } return Result::success(); } +Result HuayanRobot::ensureMotionReady_( + const std::string& context, + const std::shared_ptr& runtime, + huayan_internal::SafetyPermit& permit) const +{ + const auto connected = ensureConnected_(context); + if (!connected.ok()) { + return connected; + } + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] " + context + " failed: runtime is unavailable"); + } + if (runtimeSnapshot_().get() != runtime.get()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " rejected: the controller session changed before admission"); + } + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + if (!state.valid) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] " + context + + " failed: current safety state is unavailable"); + } + + const auto safety = runtime->safety.snapshot(); + if (safety.latched || + !huayan_internal::isMotionSafe(safety.observed)) { + const auto reason = safety.latched + ? safety.latched_reason + : safety.observed; + ArmErrorCode code = ArmErrorCode::CommandRejected; + if (isEmergencyCondition(reason)) { + code = ArmErrorCode::RobotInEmergencyStop; + } else if (isProtectiveCondition(reason)) { + code = ArmErrorCode::RobotInProtectiveStop; + } else if (safetyModeFromCondition(reason) == SafetyMode::Fault) { + code = ArmErrorCode::RobotInFault; + } + return Result::failure( + code, + "[HuayanRobot] " + context + + " rejected: safety event is latched; explicit recovery is required"); + } + if (state.error != 0) { + return Result::failure( + ArmErrorCode::RobotInFault, + "[HuayanRobot] " + context + " failed: robot error, code=" + + std::to_string(state.error_code)); + } + if (state.electrified == 0 || state.enabled == 0) { + return Result::failure( + ArmErrorCode::RobotNotPowered, + "[HuayanRobot] " + context + " failed: robot is not enabled"); + } + const auto maybe_permit = runtime->safety.tryPermit(); + if (!maybe_permit) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + " rejected by the safety state"); + } + permit = *maybe_permit; + return Result::success(); +} + Result HuayanRobot::unsupported_(const std::string& name) const { - const std::string message = "[HuayanRobot] " + name + " is not implemented"; + const std::string message = + "[HuayanRobot] " + name + " is not implemented"; CMVR_LOG(ERROR) << message; return Result::failure(ArmErrorCode::UnsupportedCommand, message); } -Result HuayanRobot::hrResult_(const int code, const std::string& context) const +Result HuayanRobot::hrResult_( + const int code, + const std::string& context) const { if (code == 0) { return Result::success(); } std::string sdk_message; - (void)HRIF_GetErrorCodeStr(box_id_, code, sdk_message); - std::ostringstream oss; - oss << "[HuayanRobot] " << context << " failed, code=" << code; - if (!sdk_message.empty()) { - oss << ", message=" << sdk_message; + { + std::lock_guard sdk_lock(sdk_mutex_); + (void)HRIF_GetErrorCodeStr(box_id_, code, sdk_message); } - const auto message = oss.str(); - CMVR_LOG(ERROR) << message; - return Result::failure(ArmErrorCode::CommandFailed, message); + std::ostringstream message; + message << "[HuayanRobot] " << context << " failed, code=" << code; + if (!sdk_message.empty()) { + message << ", message=" << sdk_message; + } + CMVR_LOG(ERROR) << message.str(); + return Result::failure(ArmErrorCode::CommandFailed, message.str()); } -bool HuayanRobot::validDof_(const std::size_t size, std::string& error) const +Result HuayanRobot::motionStartFailure_( + const std::string& context, + const MotionStartStatus status) const +{ + ArmErrorCode code = ArmErrorCode::RobotNotReady; + std::string reason = "arm is busy"; + switch (status) { + case MotionStartStatus::Stopping: + reason = "a Stop operation is in progress"; + break; + case MotionStartStatus::Blocked: + code = ArmErrorCode::CommandRejected; + reason = "controller ownership is blocked until explicit recovery"; + break; + case MotionStartStatus::Invalid: + code = ArmErrorCode::InvalidArgument; + reason = "invalid motion kind"; + break; + case MotionStartStatus::Busy: + break; + case MotionStartStatus::Started: + return Result::success(); + } + return Result::failure( + code, + "[HuayanRobot] " + context + " rejected: " + reason); +} + +bool HuayanRobot::validDof_( + const std::size_t size, + std::string& error) const { if (size != model_.dof) { - error = "[HuayanRobot] command dof mismatch, expected=" + std::to_string(model_.dof) + - ", actual=" + std::to_string(size); + error = "[HuayanRobot] command dof mismatch, expected=" + + std::to_string(model_.dof) + ", actual=" + + std::to_string(size); CMVR_LOG(ERROR) << error; return false; } if (model_.dof > 6) { - error = "[HuayanRobot] command dof exceeds SDK limit: " + std::to_string(model_.dof); + error = "[HuayanRobot] command dof exceeds SDK limit: " + + std::to_string(model_.dof); CMVR_LOG(ERROR) << error; return false; } @@ -680,132 +2011,142 @@ bool HuayanRobot::validDof_(const std::size_t size, std::string& error) const } HuayanRobot::HrState HuayanRobot::readHrState_() const +{ + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return {}; + } + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + return state; +} + +HuayanRobot::HrState HuayanRobot::sampleHrState_( + const std::shared_ptr& runtime) const { HrState state; - if (!isConnected()) { + if (!runtime || !connected_.load()) { return state; } - const int ret = HRIF_ReadRobotState(box_id_, robot_id_, - state.moving, - state.enabled, - state.error, - state.error_code, - state.error_axis, - state.brake, - state.paused, - state.emergency_stop, - state.safeguard, - state.electrified, - state.connected_to_box, - state.blending_done, - state.in_pos); - state.valid = ret == 0; - if (ret != 0) { - CMVR_LOG(ERROR) << "[HuayanRobot] read robot state failed, code=" << ret; + int state_ret = -1; + int safety_ret = -1; + { + std::lock_guard sdk_lock(sdk_mutex_); + if (!HRIF_IsConnected(box_id_)) { + return state; + } + state_ret = HRIF_ReadRobotState( + box_id_, robot_id_, + state.moving, + state.enabled, + state.error, + state.error_code, + state.error_axis, + state.brake, + state.paused, + state.emergency_stop, + state.safeguard, + state.electrified, + state.connected_to_box, + state.blending_done, + state.in_pos); + safety_ret = HRIF_ReadEmergencyInfo( + box_id_, robot_id_, + state.emergency_signal_fault, + state.emergency_input, + state.safeguard_signal_fault, + state.safeguard_input); } + state.valid = state_ret == 0 && safety_ret == 0 && + state.connected_to_box != 0; return state; } +void HuayanRobot::publishHrState_( + const std::shared_ptr& runtime, + const HrState& state) const +{ + if (!runtime) { + return; + } + std::lock_guard submission_lock( + runtime->submission_mutex); + const auto before = runtime->safety.snapshot(); + huayan_internal::RawSafetyState raw; + raw.valid = state.valid; + raw.emergency_signal_fault = state.emergency_signal_fault; + raw.emergency_stop = state.emergency_stop != 0 || + state.emergency_input != 0; + raw.safeguard_signal_fault = state.safeguard_signal_fault; + raw.safeguard_stop = state.safeguard != 0 || + state.safeguard_input != 0; + raw.robot_fault = state.error; + raw.software_emergency_stop = software_emergency_stopped_.load(); + raw.software_protective_stop = software_protective_stopped_.load(); + runtime->safety.observe(raw); + const auto after = runtime->safety.snapshot(); + if (state.valid) { + runtime->last_valid_sample_ns.store(monotonicNowNs()); + } + if (after.latched && + (!before.latched || after.epoch != before.epoch || + after.observed != before.observed)) { + runtime->termination_confirmed.store(false); + (void)runtime->motion.cancelActiveForSafety(); + runtime->wait_cv.notify_all(); + } +} + std::vector HuayanRobot::readJointPositionRad_() const { - std::vector q(model_.dof, 0.0); - if (!isConnected()) { - return q; + std::vector values; + if (!readJointPositionSample_(values)) { + values.assign(model_.dof, 0.0); } - - double j1 = 0.0; - double j2 = 0.0; - double j3 = 0.0; - double j4 = 0.0; - double j5 = 0.0; - double j6 = 0.0; - const int ret = HRIF_ReadActJointPos(box_id_, robot_id_, j1, j2, j3, j4, j5, j6); - if (ret != 0) { - CMVR_LOG(ERROR) << "[HuayanRobot] read joint position failed, code=" << ret; - return q; - } - - const std::array values{j1, j2, j3, j4, j5, j6}; - for (std::size_t i = 0; i < std::min(q.size(), values.size()); ++i) { - q[i] = degToRad(values[i]); - } - return q; + return values; } std::vector HuayanRobot::readJointVelocityRad_() const { - std::vector qd(model_.dof, 0.0); - if (!isConnected()) { - return qd; + std::vector values; + if (!readJointVelocitySample_(values)) { + values.assign(model_.dof, 0.0); } - - double j1 = 0.0; - double j2 = 0.0; - double j3 = 0.0; - double j4 = 0.0; - double j5 = 0.0; - double j6 = 0.0; - const int ret = HRIF_ReadActJointVel(box_id_, robot_id_, j1, j2, j3, j4, j5, j6); - if (ret != 0) { - CMVR_LOG(ERROR) << "[HuayanRobot] read joint velocity failed, code=" << ret; - return qd; - } - - const std::array values{j1, j2, j3, j4, j5, j6}; - for (std::size_t i = 0; i < std::min(qd.size(), values.size()); ++i) { - qd[i] = degToRad(values[i]); - } - return qd; + return values; } CartesianPose HuayanRobot::readTcpPose_() const { CartesianPose pose; - if (!isConnected()) { - return pose; - } - - double x = 0.0; - double y = 0.0; - double z = 0.0; - double rx = 0.0; - double ry = 0.0; - double rz = 0.0; - const int ret = HRIF_ReadActTcpPos(box_id_, robot_id_, x, y, z, rx, ry, rz); - if (ret != 0) { - CMVR_LOG(ERROR) << "[HuayanRobot] read tcp pose failed, code=" << ret; - return pose; - } - - pose.x = mmToMeters(x); - pose.y = mmToMeters(y); - pose.z = mmToMeters(z); - pose.rx = degToRad(rx); - pose.ry = degToRad(ry); - pose.rz = degToRad(rz); + (void)readTcpPoseSample_(pose); return pose; } CartesianVelocity HuayanRobot::readTcpVelocity_() const { CartesianVelocity velocity; - if (!isConnected()) { + if (!connected_.load()) { return velocity; } - double x = 0.0; double y = 0.0; double z = 0.0; double rx = 0.0; double ry = 0.0; double rz = 0.0; - const int ret = HRIF_ReadActTcpVel(box_id_, robot_id_, x, y, z, rx, ry, rz); + int ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + if (!HRIF_IsConnected(box_id_)) { + return velocity; + } + ret = HRIF_ReadActTcpVel( + box_id_, robot_id_, x, y, z, rx, ry, rz); + } if (ret != 0) { - CMVR_LOG(ERROR) << "[HuayanRobot] read tcp velocity failed, code=" << ret; return velocity; } - velocity.vx = mmToMeters(x); velocity.vy = mmToMeters(y); velocity.vz = mmToMeters(z); @@ -815,14 +2156,114 @@ CartesianVelocity HuayanRobot::readTcpVelocity_() const return velocity; } +bool HuayanRobot::readJointPositionSample_( + std::vector& values) const +{ + values.assign(model_.dof, 0.0); + if (!connected_.load()) { + return false; + } + double j1 = 0.0; + double j2 = 0.0; + double j3 = 0.0; + double j4 = 0.0; + double j5 = 0.0; + double j6 = 0.0; + int ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + if (!HRIF_IsConnected(box_id_)) { + return false; + } + ret = HRIF_ReadActJointPos( + box_id_, robot_id_, j1, j2, j3, j4, j5, j6); + } + if (ret != 0) { + return false; + } + const std::array sample{j1, j2, j3, j4, j5, j6}; + for (std::size_t i = 0; + i < std::min(values.size(), sample.size()); + ++i) { + values[i] = degToRad(sample[i]); + } + return true; +} + +bool HuayanRobot::readJointVelocitySample_( + std::vector& values) const +{ + values.assign(model_.dof, 0.0); + if (!connected_.load()) { + return false; + } + double j1 = 0.0; + double j2 = 0.0; + double j3 = 0.0; + double j4 = 0.0; + double j5 = 0.0; + double j6 = 0.0; + int ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + if (!HRIF_IsConnected(box_id_)) { + return false; + } + ret = HRIF_ReadActJointVel( + box_id_, robot_id_, j1, j2, j3, j4, j5, j6); + } + if (ret != 0) { + return false; + } + const std::array sample{j1, j2, j3, j4, j5, j6}; + for (std::size_t i = 0; + i < std::min(values.size(), sample.size()); + ++i) { + values[i] = degToRad(sample[i]); + } + return true; +} + +bool HuayanRobot::readTcpPoseSample_(CartesianPose& pose) const +{ + pose = {}; + if (!connected_.load()) { + return false; + } + double x = 0.0; + double y = 0.0; + double z = 0.0; + double rx = 0.0; + double ry = 0.0; + double rz = 0.0; + int ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + if (!HRIF_IsConnected(box_id_)) { + return false; + } + ret = HRIF_ReadActTcpPos( + box_id_, robot_id_, x, y, z, rx, ry, rz); + } + if (ret != 0) { + return false; + } + pose.x = mmToMeters(x); + pose.y = mmToMeters(y); + pose.z = mmToMeters(z); + pose.rx = degToRad(rx); + pose.ry = degToRad(ry); + pose.rz = degToRad(rz); + return true; +} + std::vector HuayanRobot::currentJointPositionDeg_() const { - const auto q_rad = readJointPositionRad_(); - std::vector q_deg(q_rad.size(), 0.0); - for (std::size_t i = 0; i < q_rad.size(); ++i) { - q_deg[i] = radToDeg(q_rad[i]); + auto values = readJointPositionRad_(); + for (auto& value : values) { + value = radToDeg(value); } - return q_deg; + return values; } std::string HuayanRobot::nextCommandId_() const @@ -830,56 +2271,423 @@ std::string HuayanRobot::nextCommandId_() const return id_ + "_" + std::to_string(++command_seq_); } -Result HuayanRobot::waitMotionDone_(const std::string& context, const int timeout_ms) const +Result HuayanRobot::waitMotionDone_( + const std::string& context, + const std::shared_ptr& runtime, + const huayan_internal::MotionToken motion_token, + const huayan_internal::SafetyPermit safety_permit, + const std::string& command_id, + const std::vector* joint_target, + const CartesianPose* tcp_target, + const int timeout_ms) const { - const auto start = std::chrono::steady_clock::now(); - while (true) { + const auto started_at = std::chrono::steady_clock::now(); + bool saw_motion = false; + bool saw_command_id = false; + int stable_completion_samples = 0; + + while (runtime->monitor_running.load()) { + if (runtime->motion.cancelled(motion_token) || + !runtime->safety.validate(safety_permit)) { + runtime->motion.finish(motion_token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " cancelled by Stop or a safety transition"); + } + bool done = false; - const int ret = HRIF_IsMotionDone(box_id_, robot_id_, done); - if (ret != 0) { - return hrResult_(ret, "IsMotionDone(" + context + ")"); - } - const auto state = readHrState_(); - if (state.valid) { - if (state.error != 0) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] " + context + " failed: robot error, code=" + - std::to_string(state.error_code)); - } - - if (state.emergency_stop != 0) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] " + context + " failed: emergency stop"); - } - - if (state.safeguard != 0) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] " + context + " failed: safeguard stop"); + std::string current_command_id; + int done_ret = 0; + int id_ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + done_ret = HRIF_IsMotionDone(box_id_, robot_id_, done); + if (!command_id.empty()) { + id_ret = HRIF_ReadCurWaypointID( + box_id_, robot_id_, current_command_id); } } + if (done_ret != 0 || id_ret != 0) { + const bool stopped = terminateController_( + runtime, kControllerStopTimeout, false); + if (stopped) { + runtime->motion.finish(motion_token, MotionFinishMode::Clear); + } else { + runtime->motion.failMotion(motion_token); + } + return hrResult_( + done_ret != 0 ? done_ret : id_ret, + done_ret != 0 + ? "IsMotionDone(" + context + ")" + : "ReadCurWaypointID(" + context + ")"); + } - if (done) { - return Result::success(); + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + if (!state.valid) { + continue; + } + saw_motion = saw_motion || state.moving != 0 || !done; + saw_command_id = saw_command_id || + (!command_id.empty() && current_command_id == command_id); + + if (state.error != 0 || state.emergency_stop != 0 || + state.safeguard != 0 || state.emergency_input != 0 || + state.safeguard_input != 0) { + continue; } const auto elapsed = std::chrono::duration_cast( - std::chrono::steady_clock::now() - start).count(); - - if (elapsed > timeout_ms) { - { - std::lock_guard lock(mutex_); - (void)HRIF_GrpStop(box_id_, robot_id_); + std::chrono::steady_clock::now() - started_at); + const bool correlated = command_id.empty() + ? (saw_motion || + monotonicNowNs() >= + runtime->speed_completion_not_before_ns.load()) + : (saw_motion || saw_command_id || + elapsed >= kCompletionCorrelationGrace); + const bool at_target = targetReached_(joint_target, tcp_target); + if (done && state.moving == 0 && correlated && at_target) { + ++stable_completion_samples; + if (stable_completion_samples >= 2) { + runtime->motion.finish(motion_token, MotionFinishMode::Clear); + return Result::success(); } + } else { + stable_completion_samples = 0; + } + if (elapsed.count() > timeout_ms) { + const bool stopped = terminateController_( + runtime, kControllerStopTimeout, false); + if (stopped) { + runtime->motion.finish(motion_token, MotionFinishMode::Clear); + } else { + runtime->motion.failMotion(motion_token); + } return Result::failure( - ArmErrorCode::CommandFailed, + ArmErrorCode::Timeout, "[HuayanRobot] " + context + " timeout"); } - std::this_thread::sleep_for(std::chrono::milliseconds(500)); + std::unique_lock wait_lock(runtime->wait_mutex); + runtime->wait_cv.wait_for(wait_lock, kSafetyPollPeriod); + } + + runtime->motion.failMotion(motion_token); + return Result::failure( + ArmErrorCode::NotConnected, + "[HuayanRobot] " + context + " cancelled by disconnect"); +} + +bool HuayanRobot::targetReached_( + const std::vector* joint_target, + const CartesianPose* tcp_target) const +{ + if (joint_target) { + std::vector current; + if (!readJointPositionSample_(current) || + current.size() != joint_target->size()) { + return false; + } + for (std::size_t i = 0; i < current.size(); ++i) { + if (angularDistance(current[i], (*joint_target)[i]) > + kJointTargetToleranceRad) { + return false; + } + } + } + if (tcp_target) { + CartesianPose current; + if (!readTcpPoseSample_(current)) { + return false; + } + if (std::abs(current.x - tcp_target->x) > kTcpPositionToleranceM || + std::abs(current.y - tcp_target->y) > kTcpPositionToleranceM || + std::abs(current.z - tcp_target->z) > kTcpPositionToleranceM || + angularDistance(current.rx, tcp_target->rx) > kTcpRotationToleranceRad || + angularDistance(current.ry, tcp_target->ry) > kTcpRotationToleranceRad || + angularDistance(current.rz, tcp_target->rz) > kTcpRotationToleranceRad) { + return false; + } + } + return true; +} + +bool HuayanRobot::controllerIdleStable_( + const std::shared_ptr& runtime, + const std::chrono::milliseconds timeout) const +{ + const auto deadline = std::chrono::steady_clock::now() + timeout; + int stable_samples = 0; + while (std::chrono::steady_clock::now() < deadline) { + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + bool done = false; + int done_ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + done_ret = HRIF_IsMotionDone(box_id_, robot_id_, done); + } + std::vector velocity; + const bool velocity_valid = readJointVelocitySample_(velocity); + const bool velocity_zero = velocity_valid && std::all_of( + velocity.begin(), velocity.end(), + [](const double value) { + return std::abs(value) <= kIdleVelocityToleranceRad; + }); + if (state.valid && done_ret == 0 && done && + state.moving == 0 && velocity_zero) { + ++stable_samples; + if (stable_samples >= 3) { + return true; + } + } else { + stable_samples = 0; + } + std::unique_lock wait_lock(runtime->wait_mutex); + runtime->wait_cv.wait_for(wait_lock, kSafetyPollPeriod); + } + return false; +} + +bool HuayanRobot::terminateController_( + const std::shared_ptr& runtime, + const std::chrono::milliseconds timeout, + const bool stop_program) const +{ + std::lock_guard termination_lock( + runtime->termination_mutex); + std::lock_guard submission_lock( + runtime->submission_mutex); + runtime->termination_confirmed.store(false); + int stop_ret = 0; + int script_ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + stop_ret = HRIF_GrpStop(box_id_, robot_id_); + if (stop_program) { + script_ret = HRIF_StopScript(box_id_); + } + } + if (stop_ret != 0 || script_ret != 0) { + (void)hrResult_(stop_ret != 0 ? stop_ret : script_ret, + stop_ret != 0 ? "GrpStop" : "StopScript"); + return false; + } + if (stop_program) { + runtime->program_active.store(false); + } + servo_mode_.store(false); + const bool idle = controllerIdleStable_(runtime, timeout); + runtime->termination_confirmed.store(idle); + return idle; +} + +Result HuayanRobot::completeSafetyRecovery_( + const std::string& context, + const std::shared_ptr& runtime, + const std::uint64_t expected_epoch, + const bool enable_robot, + const bool release_software_guard) +{ + if (!runtime || expected_epoch == 0 || + runtime->safety.snapshot().epoch != expected_epoch) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " rejected: safety event changed before recovery"); + } + std::lock_guard termination_lock( + runtime->termination_mutex); + + if (release_software_guard) { + int ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + ret = HRIF_EnterSafetyGuard(box_id_, robot_id_, 0); + } + const auto result = hrResult_(ret, "ExitSafetyGuard"); + if (!result.ok()) { + return result; + } + } + software_protective_stopped_.store(false); + software_emergency_stopped_.store(false); + + auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + if (!state.valid || state.emergency_signal_fault != 0 || + state.emergency_input != 0 || state.emergency_stop != 0 || + state.safeguard_signal_fault != 0 || state.safeguard_input != 0 || + state.safeguard != 0) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " rejected: hardware safety input is still active or unreadable"); + } + if (runtime->safety.snapshot().epoch != expected_epoch) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " rejected: a newer safety event was observed"); + } + + huayan_internal::StopRequest stop_request; + bool stopped = false; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + stop_request = runtime->motion.beginStop(); + if (!stop_request.started()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " rejected: another Stop operation is in progress"); + } + runtime->wait_cv.notify_all(); + stopped = terminateController_( + runtime, + kControllerStopTimeout, + runtime->program_active.load() || + stop_request.kind == MotionKind::Program); + } + const bool owner_exited = runtime->motion.waitForOwnerExit( + stop_request.active_token, + kOwnerExitTimeout); + if (!stopped || !owner_exited) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] " + context + + " failed: controller termination was not confirmed"); + } + + int reset_ret = 0; + int enable_ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + reset_ret = HRIF_GrpReset(box_id_, robot_id_); + if (reset_ret == 0 && enable_robot) { + enable_ret = HRIF_GrpEnable(box_id_, robot_id_); + } + } + if (reset_ret != 0 || enable_ret != 0) { + runtime->motion.failStop(); + return hrResult_( + reset_ret != 0 ? reset_ret : enable_ret, + reset_ret != 0 ? "GrpReset" : "GrpEnable"); + } + + const auto recovery_deadline = + std::chrono::steady_clock::now() + kControllerStopTimeout; + bool robot_ready = false; + while (std::chrono::steady_clock::now() < recovery_deadline) { + state = sampleHrState_(runtime); + publishHrState_(runtime, state); + const auto safety = runtime->safety.snapshot(); + if (safety.epoch != expected_epoch) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " cancelled by a newer safety event"); + } + robot_ready = state.valid && state.error == 0 && + state.emergency_stop == 0 && state.emergency_input == 0 && + state.safeguard == 0 && state.safeguard_input == 0 && + (!enable_robot || + (state.enabled != 0 && state.electrified != 0)); + if (robot_ready && + huayan_internal::isMotionSafe(safety.observed)) { + break; + } + std::unique_lock wait_lock(runtime->wait_mutex); + runtime->wait_cv.wait_for(wait_lock, kSafetyPollPeriod); + } + if (!robot_ready) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] " + context + + " failed: safe robot state was not confirmed after reset"); + } + + const auto recovery = runtime->safety.beginRecovery(expected_epoch); + if (!recovery) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " rejected: safety recovery epoch is no longer valid"); + } + const bool idle = controllerIdleStable_(runtime, kControllerStopTimeout); + if (!idle || !runtime->motion.completeStop()) { + runtime->safety.failRecovery(*recovery); + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] " + context + + " failed: stable controller idle was not confirmed"); + } + if (!runtime->safety.completeRecovery( + *recovery, + robot_ready, + idle, + runtime->termination_confirmed.load())) { + (void)runtime->motion.cancelActiveForSafety(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " cancelled by a safety transition during recovery"); + } + return Result::success(); +} + +std::shared_ptr +HuayanRobot::runtimeSnapshot_() const +{ + std::lock_guard state_lock(mutex_); + return runtime_; +} + +void HuayanRobot::safetyMonitorLoop_( + const std::shared_ptr& runtime) +{ + while (runtime->monitor_running.load()) { + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + const auto safety = runtime->safety.snapshot(); + if (safety.latched && + (!runtime->termination_confirmed.load() || + !state.valid || state.moving != 0 || + runtime->program_active.load())) { + std::lock_guard termination_lock( + runtime->termination_mutex); + huayan_internal::SafetyCancelResult cancelled; + bool stopped = false; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + cancelled = runtime->motion.cancelActiveForSafety(); + stopped = terminateController_( + runtime, + std::chrono::milliseconds(1000), + runtime->program_active.load() || + cancelled.kind == MotionKind::Program); + } + const bool owner_exited = runtime->motion.waitForOwnerExit( + cancelled.active_token, + std::chrono::milliseconds(1000)); + runtime->termination_confirmed.store(stopped && owner_exited); + servo_mode_.store(false); + } + + std::unique_lock wait_lock(runtime->wait_mutex); + runtime->wait_cv.wait_for( + wait_lock, + kSafetyPollPeriod, + [runtime]() { return !runtime->monitor_running.load(); }); } } diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h index 92a3d1a3..010e0339 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h @@ -9,12 +9,17 @@ #define CMVR_ES_HUAYAN_ROBOT_H #include +#include +#include #include #include +#include #include +#include #include #include "cmvr/config/arm_config/arm_config.pb.h" +#include "devices/arm/huayan_arm/huayan_lifecycle_state.h" #include "devices/arm/robot_arm.h" namespace cmvr::device { @@ -35,15 +40,15 @@ public: CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override; RobotMode getRobotMode() const override; SafetyMode getSafetyMode() const override; - ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; } + 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_; } + double getSpeedScaling() const override { return speed_scaling_.load(); } bool isProtectiveStopped() const override; bool isEmergencyStopped() const override; bool isFault() const override; @@ -71,7 +76,7 @@ public: Result brakeRelease() override { return torqueOn(); } Result shutdown() override; Result clearFault() override; - Result unlockProtectiveStop() override { return clearFault(); } + Result unlockProtectiveStop() override; Result loadProgram(const std::string& program_name) override; Result playProgram() override; Result pauseProgram() override; @@ -84,7 +89,7 @@ public: CartesianPose fk(const std::string& base_link, const std::string& ee_link) override; CartesianPose fk(bool is_tcp = true) override; CartesianVelocity getSpeedLCommandTwistBase() const override; - bool busy() const override { return busy_.load(); } + bool busy() const override; private: struct HrState { @@ -101,21 +106,68 @@ private: int connected_to_box{0}; int blending_done{0}; int in_pos{0}; + int emergency_signal_fault{0}; + int emergency_input{0}; + int safeguard_signal_fault{0}; + int safeguard_input{0}; bool valid{false}; }; + struct RuntimeState; + Result ensureConnected_(const std::string& context) const; + Result ensureMotionReady_( + const std::string& context, + const std::shared_ptr& runtime, + huayan_internal::SafetyPermit& permit) const; Result unsupported_(const std::string& name) const; Result hrResult_(int code, const std::string& context) const; + Result motionStartFailure_( + const std::string& context, + huayan_internal::MotionStartStatus status) const; bool validDof_(std::size_t size, std::string& error) const; HrState readHrState_() const; + HrState sampleHrState_( + const std::shared_ptr& runtime) const; + void publishHrState_( + const std::shared_ptr& runtime, + const HrState& state) const; std::vector readJointPositionRad_() const; std::vector readJointVelocityRad_() const; CartesianPose readTcpPose_() const; CartesianVelocity readTcpVelocity_() const; + bool readJointPositionSample_(std::vector& values) const; + bool readJointVelocitySample_(std::vector& values) const; + bool readTcpPoseSample_(CartesianPose& pose) const; std::vector currentJointPositionDeg_() const; std::string nextCommandId_() const; - Result waitMotionDone_(const std::string& context, int timeout_ms) const; + Result waitMotionDone_( + const std::string& context, + const std::shared_ptr& runtime, + huayan_internal::MotionToken motion_token, + huayan_internal::SafetyPermit safety_permit, + const std::string& command_id, + const std::vector* joint_target, + const CartesianPose* tcp_target, + int timeout_ms) const; + bool targetReached_( + const std::vector* joint_target, + const CartesianPose* tcp_target) const; + bool controllerIdleStable_( + const std::shared_ptr& runtime, + std::chrono::milliseconds timeout) const; + bool terminateController_( + const std::shared_ptr& runtime, + std::chrono::milliseconds timeout, + bool stop_program) const; + Result completeSafetyRecovery_( + const std::string& context, + const std::shared_ptr& runtime, + std::uint64_t expected_epoch, + bool enable_robot, + bool release_software_guard); + std::shared_ptr runtimeSnapshot_() const; + void safetyMonitorLoop_(const std::shared_ptr& runtime); private: config::RobotArmConfig cfg_; @@ -127,12 +179,16 @@ private: unsigned int robot_id_{0}; std::string tcp_name_{"TCP"}; std::string ucs_name_{"Base"}; - double speed_scaling_{1.0}; + std::atomic speed_scaling_{1.0}; std::atomic connected_{false}; - std::atomic busy_{false}; - std::atomic servo_mode_{false}; + mutable std::atomic servo_mode_{false}; + std::atomic software_emergency_stopped_{false}; + std::atomic software_protective_stopped_{false}; mutable std::mutex mutex_; + mutable std::recursive_mutex sdk_mutex_; mutable std::atomic command_seq_{0}; + std::shared_ptr runtime_; + std::thread safety_monitor_thread_; }; } // namespace cmvr::device @@ -140,4 +196,4 @@ private: #endif // CMVR_ES_HUAYAN_ROBOT_H -#endif //CMVR_ES_HUAYAN_ARM_H \ No newline at end of file +#endif //CMVR_ES_HUAYAN_ARM_H diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_lifecycle_state.h b/cmvr-es/devices/arm/huayan_arm/huayan_lifecycle_state.h new file mode 100644 index 00000000..6475abd4 --- /dev/null +++ b/cmvr-es/devices/arm/huayan_arm/huayan_lifecycle_state.h @@ -0,0 +1,504 @@ +#ifndef CMVR_ES_HUAYAN_LIFECYCLE_STATE_H +#define CMVR_ES_HUAYAN_LIFECYCLE_STATE_H + +#include +#include +#include +#include +#include +#include + +namespace cmvr::device::huayan_internal { + +enum class MotionKind { + None, + Joint, + Linear, + SpeedJoint, + SpeedLinear, + Servo, + Program, +}; + +struct MotionToken { + std::uint64_t generation{0}; + MotionKind kind{MotionKind::None}; + + bool valid() const noexcept + { + return generation != 0 && kind != MotionKind::None; + } +}; + +enum class MotionStartStatus { + Started, + Invalid, + Busy, + Stopping, + Blocked, +}; + +struct MotionStartResult { + MotionStartStatus status{MotionStartStatus::Busy}; + MotionToken token; + + bool started() const noexcept + { + return status == MotionStartStatus::Started; + } +}; + +enum class MotionFinishMode { + RestorePrevious, + Clear, + Retain, +}; + +enum class StopStartStatus { + Started, + AlreadyStopping, +}; + +struct StopRequest { + StopStartStatus status{StopStartStatus::AlreadyStopping}; + MotionKind kind{MotionKind::None}; + MotionToken active_token; + bool tracked_motion{false}; + + bool started() const noexcept + { + return status == StopStartStatus::Started; + } +}; + +struct SafetyCancelResult { + MotionKind kind{MotionKind::None}; + MotionToken active_token; + bool tracked_motion{false}; +}; + +struct MotionSnapshot { + MotionKind active_kind{MotionKind::None}; + MotionKind retained_kind{MotionKind::None}; + std::uint64_t active_generation{0}; + bool owner_active{false}; + bool stop_in_progress{false}; + bool blocked{false}; +}; + +// Tracks a single Huayan controller operation owner. Generation tokens make +// completion from an older RPC harmless after Stop or a safety event has +// cancelled it. Servo and program operations may retain their kind after the +// submitting RPC returns; begin(kind, true) supports same-kind updates while +// that retained controller mode remains active. +class MotionState final { +public: + MotionStartResult begin( + const MotionKind kind, + const bool replace_retained_same_kind = false) + { + std::lock_guard lock(mutex_); + if (kind == MotionKind::None) { + return {MotionStartStatus::Invalid, {}}; + } + if (stop_in_progress_) { + return {MotionStartStatus::Stopping, {}}; + } + if (blocked_) { + return {MotionStartStatus::Blocked, {}}; + } + if (owner_active_) { + return {MotionStartStatus::Busy, {}}; + } + if (retained_kind_ != MotionKind::None && + (!replace_retained_same_kind || retained_kind_ != kind)) { + return {MotionStartStatus::Busy, {}}; + } + + const MotionToken token{++next_generation_, kind}; + owner_active_ = true; + active_token_ = token; + previous_kind_ = retained_kind_; + return {MotionStartStatus::Started, token}; + } + + void finish( + const MotionToken token, + const MotionFinishMode mode = MotionFinishMode::RestorePrevious) + { + std::lock_guard lock(mutex_); + if (!owner_active_ || + active_token_.generation != token.generation) { + return; + } + + owner_active_ = false; + active_token_ = {}; + if (!stop_in_progress_ && !blocked_) { + switch (mode) { + case MotionFinishMode::RestorePrevious: + retained_kind_ = previous_kind_; + break; + case MotionFinishMode::Clear: + retained_kind_ = MotionKind::None; + break; + case MotionFinishMode::Retain: + retained_kind_ = token.kind; + break; + } + } + previous_kind_ = MotionKind::None; + owner_finished_cv_.notify_all(); + } + + void failMotion(const MotionToken token) + { + std::lock_guard lock(mutex_); + if (!owner_active_ || + active_token_.generation != token.generation) { + return; + } + + owner_active_ = false; + active_token_ = {}; + retained_kind_ = token.kind; + previous_kind_ = MotionKind::None; + blocked_ = true; + owner_finished_cv_.notify_all(); + } + + StopRequest beginStop( + const MotionKind requested_kind = MotionKind::None) + { + std::lock_guard lock(mutex_); + if (stop_in_progress_) { + return {}; + } + + stop_in_progress_ = true; + const MotionToken active = owner_active_ + ? active_token_ + : MotionToken{}; + if (active.valid()) { + cancelled_generation_ = std::max( + cancelled_generation_, active.generation); + } + + MotionKind kind = MotionKind::None; + if (active.valid()) { + kind = active.kind; + } else if (retained_kind_ != MotionKind::None) { + kind = retained_kind_; + } else { + kind = requested_kind; + } + if (kind != MotionKind::None) { + retained_kind_ = kind; + } + + return { + StopStartStatus::Started, + kind, + active, + active.valid() || kind != MotionKind::None}; + } + + 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 + : retained_kind_; + if (kind != MotionKind::None) { + retained_kind_ = kind; + } + // A hardware safety transition is independent of a concurrent + // software Stop. New controller operations remain rejected until + // termination is positively confirmed. + 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_); + return token.valid() && + token.generation <= cancelled_generation_; + } + + bool waitForOwnerExit( + const MotionToken token, + const std::chrono::milliseconds timeout) + { + if (!token.valid()) { + return true; + } + std::unique_lock lock(mutex_); + return owner_finished_cv_.wait_for( + lock, + timeout, + [this, token]() { + return !owner_active_ || + active_token_.generation != token.generation; + }); + } + + bool ownerActive(const MotionToken token) const + { + if (!token.valid()) { + return false; + } + std::lock_guard lock(mutex_); + return owner_active_ && + active_token_.generation == token.generation; + } + + bool completeStop() + { + std::lock_guard lock(mutex_); + if (owner_active_) { + return false; + } + stop_in_progress_ = false; + blocked_ = false; + active_token_ = {}; + retained_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(); + } + + MotionSnapshot snapshot() const + { + std::lock_guard lock(mutex_); + return { + owner_active_ ? active_token_.kind : MotionKind::None, + retained_kind_, + owner_active_ ? active_token_.generation : 0, + owner_active_, + stop_in_progress_, + blocked_}; + } + + bool busy() const + { + const auto state = snapshot(); + return state.owner_active || state.stop_in_progress || + state.blocked || + state.retained_kind != MotionKind::None; + } + +private: + mutable std::mutex mutex_; + std::condition_variable owner_finished_cv_; + std::uint64_t next_generation_{0}; + std::uint64_t cancelled_generation_{0}; + MotionToken active_token_; + MotionKind retained_kind_{MotionKind::None}; + MotionKind previous_kind_{MotionKind::None}; + bool owner_active_{false}; + bool stop_in_progress_{false}; + bool blocked_{false}; +}; + +enum class SafetyCondition { + Unknown, + Normal, + EmergencyStop, + SafeguardStop, + RobotFault, + EmergencySignalFault, + SafeguardSignalFault, + SoftwareEmergencyStop, + SoftwareProtectiveStop, +}; + +struct RawSafetyState { + bool valid{false}; + int emergency_signal_fault{0}; + int emergency_stop{0}; + int safeguard_signal_fault{0}; + int safeguard_stop{0}; + int robot_fault{0}; + bool software_emergency_stop{false}; + bool software_protective_stop{false}; +}; + +inline SafetyCondition classifySafetyCondition( + const RawSafetyState& state) noexcept +{ + if (!state.valid) { + return SafetyCondition::Unknown; + } + if (state.emergency_signal_fault != 0) { + return SafetyCondition::EmergencySignalFault; + } + if (state.safeguard_signal_fault != 0) { + return SafetyCondition::SafeguardSignalFault; + } + if (state.emergency_stop != 0) { + return SafetyCondition::EmergencyStop; + } + if (state.safeguard_stop != 0) { + return SafetyCondition::SafeguardStop; + } + if (state.robot_fault != 0) { + return SafetyCondition::RobotFault; + } + if (state.software_emergency_stop) { + return SafetyCondition::SoftwareEmergencyStop; + } + if (state.software_protective_stop) { + return SafetyCondition::SoftwareProtectiveStop; + } + return SafetyCondition::Normal; +} + +inline bool isMotionSafe(const SafetyCondition condition) noexcept +{ + return condition == SafetyCondition::Normal; +} + +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}; +}; + +// Safety inputs are events, not merely levels. Returning to Normal never +// clears a prior unsafe event. Explicit recovery is tied atomically to the +// event epoch, so a second event invalidates an older in-flight recovery. +class SafetyState final { +public: + void observe(const RawSafetyState& raw_state) + { + observe(classifySafetyCondition(raw_state)); + } + + 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_ready, + 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_ready || !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::huayan_internal + +#endif // CMVR_ES_HUAYAN_LIFECYCLE_STATE_H diff --git a/cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp b/cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp new file mode 100644 index 00000000..b2569995 --- /dev/null +++ b/cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp @@ -0,0 +1,834 @@ +#include "devices/arm/huayan_arm/huayan_arm.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "HR_Pro.h" + +namespace { + +using Clock = std::chrono::steady_clock; +using namespace std::chrono_literals; + +constexpr double kPi = 3.14159265358979323846; + +double radToDeg(const double value) +{ + return value * 180.0 / kPi; +} + +struct FakeSdkState final { + std::mutex mutex; + bool connected{false}; + bool enabled{true}; + bool electrified{true}; + bool robot_error{false}; + bool paused{false}; + bool emergency_input{false}; + bool emergency_signal_fault{false}; + bool safeguard_input{false}; + bool safeguard_signal_fault{false}; + bool software_safeguard{false}; + + bool motion_active{false}; + bool motion_is_joint{true}; + bool stop_pending{false}; + bool hold_next_motion{false}; + bool stale_done_once{false}; + Clock::time_point completion_at{}; + Clock::time_point stop_complete_at{}; + std::array joint_position_deg{}; + std::array joint_target_deg{}; + std::array tcp_position_hr{}; + std::array tcp_target_hr{}; + std::string waypoint_id; + + bool servo_started{false}; + bool program_running{false}; + std::string selected_program; + + int move_j_calls{0}; + int move_l_calls{0}; + int speed_j_calls{0}; + int speed_l_calls{0}; + int group_stop_calls{0}; + int group_reset_calls{0}; + int start_servo_calls{0}; + int stop_script_calls{0}; + int idle_velocity_reads_after_stop{0}; + bool count_idle_reads{false}; + + void refreshLocked() + { + const auto now = Clock::now(); + const bool safety_active = emergency_input || safeguard_input || + software_safeguard; + + if (stop_pending && now >= stop_complete_at) { + stop_pending = false; + motion_active = false; + stale_done_once = false; + count_idle_reads = true; + } + + if (motion_active && !stop_pending && !safety_active && + completion_at != Clock::time_point{} && now >= completion_at) { + motion_active = false; + stale_done_once = false; + if (motion_is_joint) { + joint_position_deg = joint_target_deg; + } else { + tcp_position_hr = tcp_target_hr; + } + } + } + + bool movingLocked() + { + refreshLocked(); + return motion_active && !emergency_input && !safeguard_input && + !software_safeguard; + } + + bool doneLocked() + { + refreshLocked(); + return !motion_active; + } + + void startMotionLocked(const bool joint) + { + motion_active = true; + motion_is_joint = joint; + stop_pending = false; + count_idle_reads = false; + stale_done_once = true; + if (hold_next_motion) { + // The fallback deadline keeps a failed test from leaving a worker + // blocked for the production 60 second timeout. + completion_at = Clock::now() + 3s; + hold_next_motion = false; + } else { + completion_at = Clock::now() + 120ms; + } + } +}; + +FakeSdkState g_sdk; + +void resetFakeSdk() +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.connected = false; + g_sdk.enabled = true; + g_sdk.electrified = true; + g_sdk.robot_error = false; + g_sdk.paused = false; + g_sdk.emergency_input = false; + g_sdk.emergency_signal_fault = false; + g_sdk.safeguard_input = false; + g_sdk.safeguard_signal_fault = false; + g_sdk.software_safeguard = false; + g_sdk.motion_active = false; + g_sdk.motion_is_joint = true; + g_sdk.stop_pending = false; + g_sdk.hold_next_motion = false; + g_sdk.stale_done_once = false; + g_sdk.completion_at = {}; + g_sdk.stop_complete_at = {}; + g_sdk.joint_position_deg = {}; + g_sdk.joint_target_deg = {}; + g_sdk.tcp_position_hr = {}; + g_sdk.tcp_target_hr = {}; + g_sdk.waypoint_id.clear(); + g_sdk.servo_started = false; + g_sdk.program_running = false; + g_sdk.selected_program.clear(); + g_sdk.move_j_calls = 0; + g_sdk.move_l_calls = 0; + g_sdk.speed_j_calls = 0; + g_sdk.speed_l_calls = 0; + g_sdk.group_stop_calls = 0; + g_sdk.group_reset_calls = 0; + g_sdk.start_servo_calls = 0; + g_sdk.stop_script_calls = 0; + g_sdk.idle_velocity_reads_after_stop = 0; + g_sdk.count_idle_reads = false; +} + +void holdNextMotion() +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.hold_next_motion = true; +} + +void setHardwareEmergencyStop(const bool active) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.emergency_input = active; + // If the wrapper never sends a real group Stop, releasing the switch makes + // the pending fake waypoint move again. This models the field failure. +} + +void setEmergencySignalFault(const bool active) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.emergency_signal_fault = active; +} + +void dropFakeTransport() +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.connected = false; +} + +template +bool waitUntil(Predicate&& predicate, + const std::chrono::milliseconds timeout = 2s) +{ + const auto deadline = Clock::now() + timeout; + while (Clock::now() < deadline) { + if (predicate()) { + return true; + } + std::this_thread::sleep_for(10ms); + } + return predicate(); +} + +cmvr::config::RobotArmConfig makeConfig() +{ + cmvr::config::RobotArmConfig cfg; + cfg.set_id("huayan_fake_sdk"); + auto* vendor = cfg.mutable_vendor(); + vendor->set_brand(cmvr::config::VENDOR_ROBOT_ARM_BRAND_HUAYAN_ARM); + vendor->set_ip("127.0.0.1"); + vendor->set_port(10003); + vendor->set_model("HuayanFake"); + vendor->set_dof(6); + vendor->set_base_frame("Base"); + vendor->set_tool_frame("TCP"); + for (int i = 1; i <= 6; ++i) { + vendor->add_joint_names("joint_" + std::to_string(i)); + } + return cfg; +} + +int failures = 0; + +#define CHECK_TRUE(condition) \ + do { \ + if (!(condition)) { \ + std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \ + << #condition << std::endl; \ + ++failures; \ + } \ + } while (false) + +} // namespace + +// The test executable exports these strong symbols. On ELF platforms they +// interpose the real SDK definitions used by libhuayan_arm, giving the test a +// deterministic controller without opening a network connection. +extern "C" { + +int HRIF_Connect(unsigned int, const char*, unsigned short) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.connected = true; + return 0; +} + +int HRIF_DisConnect(unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.connected = false; + g_sdk.motion_active = false; + return 0; +} + +bool HRIF_IsConnected(unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + return g_sdk.connected; +} + +int HRIF_GetErrorCodeStr(unsigned int, int error_code, std::string& message) +{ + message = "fake SDK error " + std::to_string(error_code); + return 0; +} + +int HRIF_GrpEnable(unsigned int, unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + if (g_sdk.emergency_input || g_sdk.safeguard_input || + g_sdk.software_safeguard) { + return 101; + } + g_sdk.enabled = true; + g_sdk.electrified = true; + return 0; +} + +int HRIF_GrpDisable(unsigned int, unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.enabled = false; + g_sdk.electrified = false; + return 0; +} + +int HRIF_GrpReset(unsigned int, unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + ++g_sdk.group_reset_calls; + if (g_sdk.emergency_input || g_sdk.safeguard_input || + g_sdk.software_safeguard) { + return 102; + } + g_sdk.robot_error = false; + return 0; +} + +int HRIF_GrpStop(unsigned int, unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + ++g_sdk.group_stop_calls; + g_sdk.idle_velocity_reads_after_stop = 0; + g_sdk.count_idle_reads = false; + if (g_sdk.motion_active) { + g_sdk.stop_pending = true; + g_sdk.stop_complete_at = Clock::now() + 120ms; + g_sdk.completion_at = {}; + } else { + g_sdk.stop_pending = false; + g_sdk.count_idle_reads = true; + } + g_sdk.servo_started = false; + return 0; +} + +int HRIF_SetOverride(unsigned int, unsigned int, double) +{ + return 0; +} + +int HRIF_ReadRobotState(unsigned int, unsigned int, + int& moving, int& enabled, int& error, + int& error_code, int& error_axis, int& brake, + int& paused, int& emergency_stop, int& safeguard, + int& electrified, int& connected_to_box, + int& blending_done, int& in_position) +{ + std::lock_guard lock(g_sdk.mutex); + if (!g_sdk.connected) { + return 201; + } + moving = g_sdk.movingLocked() ? 1 : 0; + enabled = g_sdk.enabled ? 1 : 0; + error = g_sdk.robot_error ? 1 : 0; + error_code = g_sdk.robot_error ? 9001 : 0; + error_axis = 0; + brake = g_sdk.enabled ? 1 : 0; + paused = g_sdk.paused ? 1 : 0; + emergency_stop = g_sdk.emergency_input ? 1 : 0; + safeguard = (g_sdk.safeguard_input || g_sdk.software_safeguard) ? 1 : 0; + electrified = g_sdk.electrified ? 1 : 0; + connected_to_box = 1; + blending_done = moving == 0 ? 1 : 0; + in_position = g_sdk.doneLocked() ? 1 : 0; + return 0; +} + +int HRIF_ReadEmergencyInfo(unsigned int, unsigned int, + int& emergency_signal_fault, + int& emergency_input, + int& safeguard_signal_fault, + int& safeguard_input) +{ + std::lock_guard lock(g_sdk.mutex); + if (!g_sdk.connected) { + return 202; + } + emergency_signal_fault = g_sdk.emergency_signal_fault ? 1 : 0; + emergency_input = g_sdk.emergency_input ? 1 : 0; + safeguard_signal_fault = g_sdk.safeguard_signal_fault ? 1 : 0; + safeguard_input = + (g_sdk.safeguard_input || g_sdk.software_safeguard) ? 1 : 0; + return 0; +} + +int HRIF_ReadCurWaypointID(unsigned int, unsigned int, std::string& waypoint) +{ + std::lock_guard lock(g_sdk.mutex); + waypoint = g_sdk.waypoint_id; + return 0; +} + +int HRIF_IsMotionDone(unsigned int, unsigned int, bool& done) +{ + std::lock_guard lock(g_sdk.mutex); + if (g_sdk.stale_done_once) { + g_sdk.stale_done_once = false; + done = true; + } else { + done = g_sdk.doneLocked(); + } + return 0; +} + +int HRIF_ReadActJointPos(unsigned int, unsigned int, + double& j1, double& j2, double& j3, + double& j4, double& j5, double& j6) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.refreshLocked(); + j1 = g_sdk.joint_position_deg[0]; + j2 = g_sdk.joint_position_deg[1]; + j3 = g_sdk.joint_position_deg[2]; + j4 = g_sdk.joint_position_deg[3]; + j5 = g_sdk.joint_position_deg[4]; + j6 = g_sdk.joint_position_deg[5]; + return 0; +} + +int HRIF_ReadActJointVel(unsigned int, unsigned int, + double& j1, double& j2, double& j3, + double& j4, double& j5, double& j6) +{ + std::lock_guard lock(g_sdk.mutex); + const double velocity = g_sdk.movingLocked() ? 5.0 : 0.0; + j1 = j2 = j3 = j4 = j5 = j6 = velocity; + if (velocity == 0.0 && g_sdk.count_idle_reads) { + ++g_sdk.idle_velocity_reads_after_stop; + } + return 0; +} + +int HRIF_ReadActTcpPos(unsigned int, unsigned int, + double& x, double& y, double& z, + double& rx, double& ry, double& rz) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.refreshLocked(); + x = g_sdk.tcp_position_hr[0]; + y = g_sdk.tcp_position_hr[1]; + z = g_sdk.tcp_position_hr[2]; + rx = g_sdk.tcp_position_hr[3]; + ry = g_sdk.tcp_position_hr[4]; + rz = g_sdk.tcp_position_hr[5]; + return 0; +} + +int HRIF_ReadActTcpVel(unsigned int, unsigned int, + double& x, double& y, double& z, + double& rx, double& ry, double& rz) +{ + std::lock_guard lock(g_sdk.mutex); + const double velocity = g_sdk.movingLocked() ? 5.0 : 0.0; + x = y = z = rx = ry = rz = velocity; + return 0; +} + +int HRIF_MoveJ(unsigned int, unsigned int, + double, double, double, double, double, double, + double j1, double j2, double j3, + double j4, double j5, double j6, + std::string, std::string, double, double, double, + int, int, int, int, std::string command_id) +{ + std::lock_guard lock(g_sdk.mutex); + ++g_sdk.move_j_calls; + g_sdk.joint_target_deg = {j1, j2, j3, j4, j5, j6}; + g_sdk.waypoint_id = std::move(command_id); + g_sdk.startMotionLocked(true); + return 0; +} + +int HRIF_MoveL(unsigned int, unsigned int, + double x, double y, double z, + double rx, double ry, double rz, + double, double, double, double, double, double, + std::string, std::string, double, double, double, + int, int, int, std::string command_id) +{ + std::lock_guard lock(g_sdk.mutex); + ++g_sdk.move_l_calls; + g_sdk.tcp_target_hr = {x, y, z, rx, ry, rz}; + g_sdk.waypoint_id = std::move(command_id); + g_sdk.startMotionLocked(false); + return 0; +} + +int HRIF_SpeedJ(unsigned int, unsigned int, + double, double, double, double, double, double, + double, double) +{ + std::lock_guard lock(g_sdk.mutex); + ++g_sdk.speed_j_calls; + g_sdk.startMotionLocked(true); + return 0; +} + +int HRIF_SpeedL(unsigned int, unsigned int, + double, double, double, double, double, double, + double, double, double) +{ + std::lock_guard lock(g_sdk.mutex); + ++g_sdk.speed_l_calls; + g_sdk.startMotionLocked(false); + return 0; +} + +int HRIF_StartServo(unsigned int, unsigned int, double, double) +{ + std::lock_guard lock(g_sdk.mutex); + ++g_sdk.start_servo_calls; + g_sdk.servo_started = true; + return 0; +} + +int HRIF_PushServoJ(unsigned int, unsigned int, + double j1, double j2, double j3, + double j4, double j5, double j6) +{ + std::lock_guard lock(g_sdk.mutex); + if (!g_sdk.servo_started) { + return 301; + } + g_sdk.joint_position_deg = {j1, j2, j3, j4, j5, j6}; + return 0; +} + +int HRIF_PushServoP(unsigned int, unsigned int, + std::vector& coord, + std::vector&, + std::vector&) +{ + std::lock_guard lock(g_sdk.mutex); + if (!g_sdk.servo_started || coord.size() < 6) { + return 302; + } + std::copy_n(coord.begin(), 6, g_sdk.tcp_position_hr.begin()); + return 0; +} + +int HRIF_SwitchScript(unsigned int, unsigned int, std::string script_name) +{ + std::lock_guard lock(g_sdk.mutex); + if (script_name.empty()) { + return 401; + } + g_sdk.selected_program = std::move(script_name); + return 0; +} + +int HRIF_StartScript(unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + if (g_sdk.selected_program.empty()) { + return 402; + } + g_sdk.program_running = true; + g_sdk.paused = false; + return 0; +} + +int HRIF_PauseScript(unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + if (!g_sdk.program_running) { + return 403; + } + g_sdk.paused = true; + return 0; +} + +int HRIF_StopScript(unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + ++g_sdk.stop_script_calls; + g_sdk.program_running = false; + g_sdk.paused = false; + return 0; +} + +int HRIF_EnterSafetyGuard(unsigned int, unsigned int, int flag) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.software_safeguard = flag != 0; + return 0; +} + +int HRIF_ShutdownRobot(unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.connected = false; + g_sdk.enabled = false; + g_sdk.electrified = false; + return 0; +} + +} // extern "C" + +int main() +{ + using namespace cmvr::device; + + resetFakeSdk(); + HuayanRobot arm(makeConfig()); + CHECK_TRUE(arm.connect("127.0.0.1", 10003).ok()); + + MotionOptions options; + options.velocity = 0.4; + options.acceleration = 0.8; + + // The first IsMotionDone read intentionally reports the preceding idle + // state. Completion must be correlated with the command/target. Once the + // target is reached, an identical command is an idempotent no-op. + JointPositionCommand joint_a{{0.10, -0.05, 0.08, 0.0, 0.02, -0.03}}; + CHECK_TRUE(arm.moveJ(joint_a, options).ok()); + int move_j_after_first = 0; + { + std::lock_guard lock(g_sdk.mutex); + move_j_after_first = g_sdk.move_j_calls; + } + CHECK_TRUE(arm.moveJ(joint_a, options).ok()); + { + std::lock_guard lock(g_sdk.mutex); + CHECK_TRUE(g_sdk.move_j_calls == move_j_after_first); + } + + CartesianPose pose_a; + pose_a.x = 0.31; + pose_a.y = -0.12; + pose_a.z = 0.42; + pose_a.rx = 0.08; + pose_a.ry = -0.04; + pose_a.rz = 0.12; + CHECK_TRUE(arm.moveL(pose_a, options).ok()); + int move_l_after_first = 0; + { + std::lock_guard lock(g_sdk.mutex); + move_l_after_first = g_sdk.move_l_calls; + } + CHECK_TRUE(arm.moveL(pose_a, options).ok()); + { + std::lock_guard lock(g_sdk.mutex); + CHECK_TRUE(g_sdk.move_l_calls == move_l_after_first); + } + + // Stop must cancel the old owner and wait until the controller reports + // stable idle; clearing the owner immediately after GrpStop would fail the + // elapsed-time and consecutive-idle checks below. + JointPositionCommand joint_b{{0.22, -0.08, 0.14, 0.03, 0.04, -0.01}}; + holdNextMotion(); + const int before_held_move = move_j_after_first; + auto held_move = std::async(std::launch::async, [&]() { + return arm.moveJ(joint_b, options); + }); + CHECK_TRUE(waitUntil([&]() { + std::lock_guard lock(g_sdk.mutex); + return g_sdk.move_j_calls > before_held_move; + })); + const auto stop_started = Clock::now(); + CHECK_TRUE(arm.stopMotion().ok()); + const auto stop_elapsed = Clock::now() - stop_started; + CHECK_TRUE(stop_elapsed >= 100ms); + CHECK_TRUE(held_move.wait_for(1s) == std::future_status::ready); + if (held_move.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(!held_move.get().ok()); + } + CHECK_TRUE(!arm.busy()); + { + std::lock_guard lock(g_sdk.mutex); + CHECK_TRUE(g_sdk.idle_velocity_reads_after_stop >= 3); + } + CHECK_TRUE(arm.moveJ(joint_b, options).ok()); + + // A hardware E-stop cancels and terminates the active waypoint. Releasing + // the switch does not clear the software latch or grant a new permit. + JointPositionCommand joint_c{{0.34, -0.02, 0.09, 0.05, -0.02, 0.07}}; + holdNextMotion(); + int before_estop_move = 0; + int before_estop_stop = 0; + { + std::lock_guard lock(g_sdk.mutex); + before_estop_move = g_sdk.move_j_calls; + before_estop_stop = g_sdk.group_stop_calls; + } + auto estop_move = std::async(std::launch::async, [&]() { + return arm.moveJ(joint_c, options); + }); + CHECK_TRUE(waitUntil([&]() { + std::lock_guard lock(g_sdk.mutex); + return g_sdk.move_j_calls > before_estop_move; + })); + setHardwareEmergencyStop(true); + CHECK_TRUE(waitUntil([&]() { + std::lock_guard lock(g_sdk.mutex); + return g_sdk.group_stop_calls > before_estop_stop; + })); + CHECK_TRUE(estop_move.wait_for(2s) == std::future_status::ready); + if (estop_move.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(!estop_move.get().ok()); + } + setHardwareEmergencyStop(false); + std::this_thread::sleep_for(150ms); + + int move_count_while_latched = 0; + { + std::lock_guard lock(g_sdk.mutex); + move_count_while_latched = g_sdk.move_j_calls; + } + const auto rejected_while_latched = arm.moveJ(joint_a, options); + CHECK_TRUE(!rejected_while_latched.ok()); + { + std::lock_guard lock(g_sdk.mutex); + CHECK_TRUE(g_sdk.move_j_calls == move_count_while_latched); + } + CHECK_TRUE(arm.clearFault().ok()); + CHECK_TRUE(arm.torqueOn().ok()); + CHECK_TRUE(arm.moveJ(joint_a, options).ok()); + + // Speed commands own the controller while waiting. A different motion is + // rejected, and Stop releases ownership only after termination. + holdNextMotion(); + JointVelocityCommand speed{{0.1, 0.0, 0.0, 0.0, 0.0, 0.0}}; + int speed_calls_before = 0; + { + std::lock_guard lock(g_sdk.mutex); + speed_calls_before = g_sdk.speed_j_calls; + } + auto speed_motion = std::async(std::launch::async, [&]() { + return arm.speedJ(speed, 0.5, 2.0); + }); + CHECK_TRUE(waitUntil([&]() { + std::lock_guard lock(g_sdk.mutex); + return g_sdk.speed_j_calls > speed_calls_before; + })); + CHECK_TRUE(!arm.moveL(pose_a, options).ok()); + CHECK_TRUE(arm.stopMotion().ok()); + CHECK_TRUE(speed_motion.wait_for(1s) == std::future_status::ready); + if (speed_motion.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(!speed_motion.get().ok()); + } + CHECK_TRUE(!arm.busy()); + + // SpeedL used to hold the SDK mutex while waiting, which deadlocked its + // own timeout/Stop path. A concurrent Stop must cancel it, settle the + // controller, and allow a genuinely new Move command afterwards. + holdNextMotion(); + CartesianVelocity line_speed; + line_speed.vx = 0.05; + int speed_l_calls_before = 0; + { + std::lock_guard lock(g_sdk.mutex); + speed_l_calls_before = g_sdk.speed_l_calls; + } + auto line_speed_motion = std::async(std::launch::async, [&]() { + return arm.speedL(line_speed, 0.5, 2.0, FrameType::Base); + }); + CHECK_TRUE(waitUntil([&]() { + std::lock_guard lock(g_sdk.mutex); + return g_sdk.speed_l_calls > speed_l_calls_before; + })); + CHECK_TRUE(arm.stopMotion().ok()); + CHECK_TRUE(line_speed_motion.wait_for(1s) == std::future_status::ready); + if (line_speed_motion.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(!line_speed_motion.get().ok()); + } + CHECK_TRUE(!arm.busy()); + CHECK_TRUE(arm.moveJ(joint_b, options).ok()); + + // A dual-channel emergency input mismatch is a typed emergency latch. It + // remains blocked after the wiring level is healthy and is recovered only + // through the emergency recovery path. + int stops_before_signal_fault = 0; + { + std::lock_guard lock(g_sdk.mutex); + stops_before_signal_fault = g_sdk.group_stop_calls; + } + setEmergencySignalFault(true); + CHECK_TRUE(waitUntil([&]() { + std::lock_guard lock(g_sdk.mutex); + return g_sdk.group_stop_calls > stops_before_signal_fault; + })); + setEmergencySignalFault(false); + std::this_thread::sleep_for(100ms); + CHECK_TRUE(!arm.moveJ(joint_c, options).ok()); + CHECK_TRUE(arm.torqueOn().ok()); + CHECK_TRUE(arm.moveJ(joint_c, options).ok()); + + // Power-off holds a terminal barrier through GrpDisable. Motion remains + // denied until an explicit enable confirms the powered state again. + CHECK_TRUE(arm.torqueOff().ok()); + CHECK_TRUE(!arm.moveJ(joint_a, options).ok()); + CHECK_TRUE(arm.torqueOn().ok()); + CHECK_TRUE(arm.moveJ(joint_a, options).ok()); + + // Servo and program modes retain ownership beyond the start call. Stop of + // a retained program must use StopScript as well as the group stop path. + ServoOptions servo_options; + CHECK_TRUE(arm.startServoMode(servo_options).ok()); + CHECK_TRUE(!arm.moveJ(joint_b, options).ok()); + CHECK_TRUE(arm.servoJ(joint_b).ok()); + CHECK_TRUE(arm.stopServoMode().ok()); + CHECK_TRUE(!arm.busy()); + + CHECK_TRUE(arm.loadProgram("fake_program.script").ok()); + CHECK_TRUE(arm.playProgram().ok()); + CHECK_TRUE(!arm.moveJ(joint_c, options).ok()); + int stop_script_calls_before = 0; + { + std::lock_guard lock(g_sdk.mutex); + stop_script_calls_before = g_sdk.stop_script_calls; + } + CHECK_TRUE(arm.stopMotion().ok()); + { + std::lock_guard lock(g_sdk.mutex); + CHECK_TRUE(g_sdk.stop_script_calls > stop_script_calls_before); + } + CHECK_TRUE(!arm.busy()); + + // Retire an in-flight stale generation after transport loss before + // reconnecting; no old waiter may issue SDK reads into the new session. + holdNextMotion(); + int moves_before_transport_loss = 0; + { + std::lock_guard lock(g_sdk.mutex); + moves_before_transport_loss = g_sdk.move_j_calls; + } + auto transport_lost_move = std::async(std::launch::async, [&]() { + return arm.moveJ(joint_c, options); + }); + CHECK_TRUE(waitUntil([&]() { + std::lock_guard lock(g_sdk.mutex); + return g_sdk.move_j_calls > moves_before_transport_loss; + })); + dropFakeTransport(); + CHECK_TRUE(arm.connect("127.0.0.1", 10003).ok()); + CHECK_TRUE(transport_lost_move.wait_for(1s) == std::future_status::ready); + if (transport_lost_move.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(!transport_lost_move.get().ok()); + } + CHECK_TRUE(arm.moveJ(joint_b, options).ok()); + CHECK_TRUE(arm.disconnect().ok()); + + resetFakeSdk(); + HuayanRobot shutdown_arm(makeConfig()); + CHECK_TRUE(shutdown_arm.connect("127.0.0.1", 10003).ok()); + CHECK_TRUE(shutdown_arm.shutdown().ok()); + CHECK_TRUE(!shutdown_arm.isConnected()); + return failures == 0 ? 0 : 1; +} diff --git a/cmvr-es/devices/arm/huayan_arm/tests/huayan_lifecycle_state_test.cpp b/cmvr-es/devices/arm/huayan_arm/tests/huayan_lifecycle_state_test.cpp new file mode 100644 index 00000000..8bb93ff7 --- /dev/null +++ b/cmvr-es/devices/arm/huayan_arm/tests/huayan_lifecycle_state_test.cpp @@ -0,0 +1,232 @@ +#include "devices/arm/huayan_arm/huayan_lifecycle_state.h" + +#include +#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::huayan_internal; + + MotionState motion; + CHECK_TRUE(motion.begin(MotionKind::None).status == + MotionStartStatus::Invalid); + + // A completed target does not poison an identical subsequent command, + // while an actually concurrent command is rejected. + const auto first_joint = motion.begin(MotionKind::Joint); + CHECK_TRUE(first_joint.started()); + CHECK_TRUE(motion.begin(MotionKind::Joint).status == + MotionStartStatus::Busy); + CHECK_TRUE(motion.begin(MotionKind::Linear).status == + MotionStartStatus::Busy); + motion.finish(first_joint.token); + const auto repeated_joint = motion.begin(MotionKind::Joint); + CHECK_TRUE(repeated_joint.started()); + CHECK_TRUE(repeated_joint.token.generation > + first_joint.token.generation); + motion.finish(first_joint.token); + CHECK_TRUE(motion.ownerActive(repeated_joint.token)); + motion.finish(repeated_joint.token); + CHECK_TRUE(!motion.busy()); + + // Stop cancels the current generation and cannot complete before its + // owner exits. + const auto linear = motion.begin(MotionKind::Linear); + CHECK_TRUE(linear.started()); + const auto stop_linear = motion.beginStop(); + CHECK_TRUE(stop_linear.started()); + CHECK_TRUE(stop_linear.kind == MotionKind::Linear); + CHECK_TRUE(stop_linear.active_token.generation == + linear.token.generation); + CHECK_TRUE(stop_linear.tracked_motion); + CHECK_TRUE(motion.cancelled(linear.token)); + CHECK_TRUE(motion.beginStop().status == + StopStartStatus::AlreadyStopping); + CHECK_TRUE(motion.begin(MotionKind::Joint).status == + MotionStartStatus::Stopping); + CHECK_TRUE(!motion.waitForOwnerExit( + linear.token, std::chrono::milliseconds(1))); + CHECK_TRUE(!motion.completeStop()); + motion.finish(linear.token); + CHECK_TRUE(motion.waitForOwnerExit( + linear.token, std::chrono::milliseconds(1))); + CHECK_TRUE(motion.completeStop()); + CHECK_TRUE(!motion.busy()); + + // An uncertain submission/completion remains fail-closed until a + // positively acknowledged Stop clears it. + const auto failed_speed = motion.begin(MotionKind::SpeedLinear); + CHECK_TRUE(failed_speed.started()); + motion.failMotion(failed_speed.token); + CHECK_TRUE(motion.snapshot().blocked); + CHECK_TRUE(motion.begin(MotionKind::Joint).status == + MotionStartStatus::Blocked); + const auto stop_failed_speed = motion.beginStop(); + CHECK_TRUE(stop_failed_speed.kind == MotionKind::SpeedLinear); + CHECK_TRUE(stop_failed_speed.tracked_motion); + CHECK_TRUE(motion.completeStop()); + + const auto failed_stop_motion = motion.begin(MotionKind::Joint); + CHECK_TRUE(failed_stop_motion.started()); + const auto failed_stop = motion.beginStop(); + CHECK_TRUE(failed_stop.kind == MotionKind::Joint); + motion.failStop(); + CHECK_TRUE(motion.snapshot().blocked); + CHECK_TRUE(motion.begin(MotionKind::Linear).status == + MotionStartStatus::Blocked); + motion.finish(failed_stop_motion.token); + const auto retry_failed_stop = motion.beginStop(); + CHECK_TRUE(retry_failed_stop.kind == MotionKind::Joint); + CHECK_TRUE(motion.completeStop()); + + // Servo and program modes remain owned after their start RPC returns. + const auto servo = motion.begin(MotionKind::Servo); + CHECK_TRUE(servo.started()); + motion.finish(servo.token, MotionFinishMode::Retain); + CHECK_TRUE(motion.snapshot().retained_kind == MotionKind::Servo); + CHECK_TRUE(motion.begin(MotionKind::Program).status == + MotionStartStatus::Busy); + const auto servo_update = motion.begin(MotionKind::Servo, true); + CHECK_TRUE(servo_update.started()); + motion.finish(servo_update.token); + CHECK_TRUE(motion.snapshot().retained_kind == MotionKind::Servo); + const auto stop_servo = motion.beginStop(); + CHECK_TRUE(stop_servo.kind == MotionKind::Servo); + CHECK_TRUE(stop_servo.tracked_motion); + CHECK_TRUE(motion.completeStop()); + + const auto program = motion.begin(MotionKind::Program); + CHECK_TRUE(program.started()); + motion.finish(program.token, MotionFinishMode::Retain); + const auto cancelled_program = motion.cancelActiveForSafety(); + CHECK_TRUE(cancelled_program.kind == MotionKind::Program); + CHECK_TRUE(cancelled_program.tracked_motion); + CHECK_TRUE(!cancelled_program.active_token.valid()); + CHECK_TRUE(motion.begin(MotionKind::Joint).status == + MotionStartStatus::Blocked); + const auto stop_program = motion.beginStop(); + CHECK_TRUE(stop_program.kind == MotionKind::Program); + CHECK_TRUE(motion.completeStop()); + + const auto safety_move = motion.begin(MotionKind::SpeedJoint); + CHECK_TRUE(safety_move.started()); + const auto cancelled_move = motion.cancelActiveForSafety(); + CHECK_TRUE(cancelled_move.kind == MotionKind::SpeedJoint); + CHECK_TRUE(cancelled_move.active_token.generation == + safety_move.token.generation); + CHECK_TRUE(motion.cancelled(safety_move.token)); + motion.finish(safety_move.token); + const auto stop_safety_move = motion.beginStop(); + CHECK_TRUE(stop_safety_move.kind == MotionKind::SpeedJoint); + CHECK_TRUE(motion.completeStop()); + + RawSafetyState raw; + CHECK_TRUE(classifySafetyCondition(raw) == SafetyCondition::Unknown); + raw.valid = true; + CHECK_TRUE(classifySafetyCondition(raw) == SafetyCondition::Normal); + raw.software_protective_stop = true; + CHECK_TRUE(classifySafetyCondition(raw) == + SafetyCondition::SoftwareProtectiveStop); + raw.software_emergency_stop = true; + CHECK_TRUE(classifySafetyCondition(raw) == + SafetyCondition::SoftwareEmergencyStop); + raw.robot_fault = 1; + CHECK_TRUE(classifySafetyCondition(raw) == + SafetyCondition::RobotFault); + raw.safeguard_stop = 1; + CHECK_TRUE(classifySafetyCondition(raw) == + SafetyCondition::SafeguardStop); + raw.emergency_stop = 1; + CHECK_TRUE(classifySafetyCondition(raw) == + SafetyCondition::EmergencyStop); + raw.safeguard_signal_fault = 1; + CHECK_TRUE(classifySafetyCondition(raw) == + SafetyCondition::SafeguardSignalFault); + raw.emergency_signal_fault = 1; + CHECK_TRUE(classifySafetyCondition(raw) == + SafetyCondition::EmergencySignalFault); + + SafetyState safety; + CHECK_TRUE(!safety.tryPermit().has_value()); + safety.observe(SafetyCondition::Normal); + const auto initial_permit = safety.tryPermit(); + CHECK_TRUE(initial_permit.has_value()); + CHECK_TRUE(safety.validate(*initial_permit)); + + safety.observe(SafetyCondition::EmergencyStop); + CHECK_TRUE(safety.snapshot().latched); + CHECK_TRUE(!safety.validate(*initial_permit)); + CHECK_TRUE(!safety.beginRecovery(safety.snapshot().epoch).has_value()); + const auto first_emergency_epoch = safety.snapshot().epoch; + safety.observe(SafetyCondition::EmergencyStop); + safety.observe(SafetyCondition::EmergencyStop); + CHECK_TRUE(safety.snapshot().epoch == first_emergency_epoch); + + // Releasing the hardware switch only changes the observed level; it does + // not clear the event latch or issue a new motion permit. + safety.observe(SafetyCondition::Normal); + CHECK_TRUE(safety.snapshot().latched); + CHECK_TRUE(!safety.tryPermit().has_value()); + + const auto not_ready = safety.beginRecovery(safety.snapshot().epoch); + CHECK_TRUE(not_ready.has_value()); + CHECK_TRUE(!safety.completeRecovery(*not_ready, false, true, true)); + CHECK_TRUE(safety.snapshot().latched); + safety.failRecovery(*not_ready); + + const auto not_idle = safety.beginRecovery(safety.snapshot().epoch); + CHECK_TRUE(not_idle.has_value()); + CHECK_TRUE(!safety.completeRecovery(*not_idle, true, false, true)); + CHECK_TRUE(safety.snapshot().latched); + safety.failRecovery(*not_idle); + + const auto not_cancelled = + safety.beginRecovery(safety.snapshot().epoch); + CHECK_TRUE(not_cancelled.has_value()); + CHECK_TRUE(!safety.completeRecovery(*not_cancelled, true, true, false)); + CHECK_TRUE(safety.snapshot().latched); + safety.failRecovery(*not_cancelled); + + const auto recovery_retry = + safety.beginRecovery(safety.snapshot().epoch); + CHECK_TRUE(recovery_retry.has_value()); + CHECK_TRUE(safety.completeRecovery( + *recovery_retry, true, true, true)); + const auto recovered_permit = safety.tryPermit(); + CHECK_TRUE(recovered_permit.has_value()); + CHECK_TRUE(safety.validate(*recovered_permit)); + + // A second safety event, including the same physical E-stop being pressed + // again, invalidates an older recovery token atomically. + safety.observe(SafetyCondition::EmergencyStop); + safety.observe(SafetyCondition::Normal); + const auto stale_recovery = + safety.beginRecovery(safety.snapshot().epoch); + CHECK_TRUE(stale_recovery.has_value()); + safety.observe(SafetyCondition::EmergencyStop); + safety.observe(SafetyCondition::Normal); + CHECK_TRUE(!safety.completeRecovery( + *stale_recovery, true, true, true)); + CHECK_TRUE(safety.snapshot().latched); + CHECK_TRUE(!safety.snapshot().recovery_in_progress); + + const auto stale_epoch = safety.snapshot().epoch; + safety.observe(SafetyCondition::SafeguardStop); + safety.observe(SafetyCondition::Normal); + CHECK_TRUE(!safety.beginRecovery(stale_epoch).has_value()); + + return 0; +}