diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp index 87e573c8..98374c3b 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp @@ -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(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_) {