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

fix aubo clearFault recovery

Browse Source
This commit is contained in:
xtkuang 2026-08-18 11:27:12 +08:00
parent e28a0a4381
commit 1dfe23641d
1 changed files with 43 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -3321,6 +3321,19 @@ Result AuboArm::clearFault()
} }
std::unique_lock<std::recursive_mutex> command_rpc_lock( std::unique_lock<std::recursive_mutex> command_rpc_lock(
monitor->command_rpc_mutex); monitor->command_rpc_mutex);
const auto recover_with_torque_on =
[this, &command_rpc_lock]() -> Result {
if (command_rpc_lock.owns_lock()) {
command_rpc_lock.unlock();
}
auto recovery = torqueOn();
if (!recovery.ok()) {
recovery.message =
"[AuboArm] clearFault recovery failed: " +
recovery.message;
}
return recovery;
};
Result interface_result; Result interface_result;
auto robot_interface = getPrimaryRobotInterface( auto robot_interface = getPrimaryRobotInterface(
rpc_client, "clearFault", interface_result); rpc_client, "clearFault", interface_result);
@ -3330,18 +3343,15 @@ Result AuboArm::clearFault()
refreshSafetySample(rpc_client, monitor, robot_interface); refreshSafetySample(rpc_client, monitor, robot_interface);
auto snapshot = monitor->safety_state->snapshot(); auto snapshot = monitor->safety_state->snapshot();
const std::uint64_t expected_safety_epoch = snapshot.epoch; const std::uint64_t expected_safety_epoch = snapshot.epoch;
const int entry_robot_mode = monitor->robot_mode.load();
if (!snapshot.latched && if (!snapshot.latched &&
aubo_internal::isMotionSafe(snapshot.observed) && aubo_internal::isMotionSafe(snapshot.observed) &&
monitor->robot_mode.load() != entry_robot_mode != static_cast<int>(RobotModeType::Error)) {
static_cast<int>(RobotModeType::Error)) { if (entry_robot_mode ==
static_cast<int>(RobotModeType::Running)) {
return Result::success(); return Result::success();
} }
if (!snapshot.latched && return recover_with_torque_on();
monitor->robot_mode.load() ==
static_cast<int>(RobotModeType::Error)) {
return Result::failure(
ArmErrorCode::RobotInFault,
"[AuboArm] clearFault rejected: RobotMode is Error even though the safety mode is Normal/Reduced");
} }
if (monitor->emergency_stop_source.load() != 0) { if (monitor->emergency_stop_source.load() != 0) {
return Result::failure( return Result::failure(
@ -3353,22 +3363,38 @@ Result AuboArm::clearFault()
aubo_internal::needsProtectiveUnlock( aubo_internal::needsProtectiveUnlock(
snapshot.latched_reason)) { snapshot.latched_reason)) {
command_rpc_lock.unlock(); command_rpc_lock.unlock();
return unlockProtectiveStop_(expected_safety_epoch); auto unlock_result =
unlockProtectiveStop_(expected_safety_epoch);
if (unlock_result.ok()) {
return monitor->robot_mode.load() ==
static_cast<int>(RobotModeType::Running)
? Result::success()
: recover_with_torque_on();
}
if (unlock_result.code == ArmErrorCode::RobotNotPowered) {
return recover_with_torque_on();
}
return unlock_result;
} }
if (aubo_internal::isMotionSafe(snapshot.observed)) { const bool controller_fault_requires_restart =
aubo_internal::isMotionSafe(snapshot.observed) &&
monitor->robot_mode.load() ==
static_cast<int>(RobotModeType::Error);
if (aubo_internal::isMotionSafe(snapshot.observed) &&
!controller_fault_requires_restart) {
if (monitor->robot_mode.load() == if (monitor->robot_mode.load() ==
static_cast<int>(RobotModeType::Running)) { static_cast<int>(RobotModeType::Running)) {
command_rpc_lock.unlock(); command_rpc_lock.unlock();
return completeSafetyRecovery_( return completeSafetyRecovery_(
"clearFault", expected_safety_epoch); "clearFault", expected_safety_epoch);
} }
return Result::failure( return recover_with_torque_on();
ArmErrorCode::RobotNotPowered,
"[AuboArm] safety condition is clear, but torqueOn is required to verify the old queue and complete recovery");
} }
if (!aubo_internal::needsInterfaceBoardRestart( if (!controller_fault_requires_restart &&
!aubo_internal::needsInterfaceBoardRestart(
snapshot.observed)) { snapshot.observed)) {
const std::string guidance = const std::string guidance =
snapshot.observed == snapshot.observed ==
@ -3415,9 +3441,7 @@ Result AuboArm::clearFault()
return completeSafetyRecovery_( return completeSafetyRecovery_(
"clearFault", expected_safety_epoch); "clearFault", expected_safety_epoch);
} }
return Result::failure( return recover_with_torque_on();
ArmErrorCode::RobotNotPowered,
"[AuboArm] controller fault was reset, but the safety latch remains until torqueOn verifies an empty queue in Running mode");
} catch (const std::exception& e) { } catch (const std::exception& e) {
return Result::failure( return Result::failure(
ArmErrorCode::CommandFailed, ArmErrorCode::CommandFailed,