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

fix(touch): detect stopped speedL controller during touch

Browse Source
This commit is contained in:
lgv 2026-09-18 16:39:40 +08:00
parent d51ceab46e
commit 7df07fba56
1 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp
View File

@ -1577,6 +1577,22 @@ bool TouchScreenTask::stepTouching() {
return true;
}
// A successful speedL submission does not report later worker failures.
// With duration=0 the controller must stay active until we request a stop
// or retract; otherwise waiting for contact/distance can continue forever.
if (!arm_->busy()) {
const double elapsed =
std::chrono::duration<double>(Clock::now() - phase_start_time_).count();
CMVR_LOG(ERROR) << "[TouchScreenTask][TOUCHING] speedL controller became idle before contact"
<< ", elapsed_s=" << elapsed
<< ", fz_N=" << last_touch_resultant_fz_
<< ", criterion_value_N=" << last_touch_pressure_sum_
<< ", threshold_N=" << config_.touch().tactile().force_threshold()
<< "; check preceding CartesianVelocityController/planner errors";
enterFailed(Status::ROBOT_COMMAND_FAILED);
return false;
}
const double speed_l_max_distance_m = config_.touch().speed_l().max_distance_m();
if (speed_l_max_distance_m > 0.0) {
if (!touch_start_position_valid_) {