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(
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;
auto robot_interface = getPrimaryRobotInterface(
rpc_client, "clearFault", interface_result);
@ -3330,18 +3343,15 @@ Result AuboArm::clearFault()
refreshSafetySample(rpc_client, monitor, robot_interface);
auto snapshot = monitor->safety_state->snapshot();
const std::uint64_t expected_safety_epoch = snapshot.epoch;
const int entry_robot_mode = monitor->robot_mode.load();
if (!snapshot.latched &&
aubo_internal::isMotionSafe(snapshot.observed) &&
monitor->robot_mode.load() !=
static_cast<int>(RobotModeType::Error)) {
entry_robot_mode != static_cast<int>(RobotModeType::Error)) {
if (entry_robot_mode ==
static_cast<int>(RobotModeType::Running)) {
return Result::success();
}
if (!snapshot.latched &&
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");
return recover_with_torque_on();
}
if (monitor->emergency_stop_source.load() != 0) {
return Result::failure(
@ -3353,22 +3363,38 @@ Result AuboArm::clearFault()
aubo_internal::needsProtectiveUnlock(
snapshot.latched_reason)) {
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() ==
static_cast<int>(RobotModeType::Running)) {
command_rpc_lock.unlock();
return completeSafetyRecovery_(
"clearFault", expected_safety_epoch);
}
return Result::failure(
ArmErrorCode::RobotNotPowered,
"[AuboArm] safety condition is clear, but torqueOn is required to verify the old queue and complete recovery");
return recover_with_torque_on();
}
if (!aubo_internal::needsInterfaceBoardRestart(
if (!controller_fault_requires_restart &&
!aubo_internal::needsInterfaceBoardRestart(
snapshot.observed)) {
const std::string guidance =
snapshot.observed ==
@ -3415,9 +3441,7 @@ Result AuboArm::clearFault()
return completeSafetyRecovery_(
"clearFault", expected_safety_epoch);
}
return Result::failure(
ArmErrorCode::RobotNotPowered,
"[AuboArm] controller fault was reset, but the safety latch remains until torqueOn verifies an empty queue in Running mode");
return recover_with_torque_on();
} catch (const std::exception& e) {
return Result::failure(
ArmErrorCode::CommandFailed,