[Bug] tool dropped in drawer in recovery mode

[kimseoungyun]

🐞 버그 설명

Recovery 모드가 켜진 상태에서 공구가 서랍 안에 있는 경우, 시스템이 해당 공구를 다시 잡으려고 시도하면서 recovery 흐름이 정상적으로 종료되지 않는 문제가 발생한다.

현재 recovery 로직은 공구가 서랍 내부에 있는 상황에서도 계속 grasp를 시도할 수 있다. 그러나 서랍 안에 있는 공구는 접근 가능성, 파지 가능성, 충돌 위험 등을 고려해야 하므로 recovery 모드에서는 무리하게 잡으려고 하지 않고, “못 잡는다”라고 판단한 뒤 서랍을 닫는 단계로 넘어가야 한다.

🔁 재현 방법

1. 작업 중 공구를 놓치거나 실패 상황을 발생시켜 recovery 모드로 진입한다.
2. Recovery 과정에서 대상 공구가 서랍 내부에 있는 상태로 인식된다.
3. 시스템이 서랍 안에 있는 공구를 다시 잡으려고 시도한다.
4. 공구를 잡지 못하거나 recovery 흐름이 멈춘다.

✅ 기대한 동작

Recovery 모드에서 대상 공구가 서랍 안에 있다고 판단되면, 시스템은 해당 공구를 다시 잡으려고 시도하지 않아야 한다.

대신 다음과 같이 동작해야 한다.

1. 공구가 서랍 안에 있음을 인식한다.
2. Recovery 모드에서는 현재 공구를 잡을 수 없다고 판단한다.
3. 사용자 또는 로그에 “공구가 서랍 안에 있어 잡을 수 없습니다”와 같은 상태 메시지를 남긴다.
4. Grasp 시도를 중단한다.
5. 서랍 닫기 단계로 넘어간다.
6. 서랍을 닫은 뒤 안전 상태 또는 실패 종료 상태로 전환한다.

❌ 실제 동작

Recovery 모드에서 공구가 서랍 안에 있는 경우에도 시스템이 공구를 다시 잡으려고 시도한다.

이로 인해 다음과 같은 문제가 발생할 수 있다.

• 서랍 내부 공구에 대해 불필요한 grasp 시도가 발생한다.
• 로봇팔 또는 그리퍼가 서랍 내부 구조물과 충돌할 위험이 있다.
• recovery 큐가 정상적으로 다음 단계로 넘어가지 못할 수 있다.
• 사용자는 recovery가 실패했는지, 진행 중인지 명확히 알기 어렵다.
• 서랍 닫기 단계가 실행되지 않고 recovery 과정이 멈출 수 있다.

🖥️ 실행 환경

• OS: Ubuntu 22.04
• ROS 2 버전: ROS2 Humble
• Python 버전: Python 3.10
• 레포지토리:
• 브랜치:
• 카메라 정보: Intel® RealSense™ Depth Camera D435I
• 로봇팔 정보: Doosan-Robotics-M0609
• 그리퍼 정보: OnRobot RG2

📋 로그 또는 에러 메시지

Recovery mode entered.
Target tool detected.
Target tool location: inside drawer.
Attempting recovery grasp.
Grasp failed or not reachable.
Recovery task did not proceed to drawer close step.

🎥 스크린샷/영상

해당 없음.

📎 추가 정보

수정 방향은 다음과 같다.

• recovery 모드에서 target tool의 위치가 서랍 내부인지 먼저 확인한다.
• 공구가 서랍 안에 있다고 판단되면 grasp task를 생성하지 않는다.
• 이 경우 recovery 결과를 UNREACHABLE, CANNOT_GRASP_IN_DRAWER, 또는 이에 준하는 실패 상태로 처리한다.
• 사용자에게 “공구가 서랍 안에 있어 잡을 수 없습니다”라는 상태 메시지를 전달한다.
• 이후 recovery 큐를 서랍 닫기 단계로 전환한다.
• 서랍 닫기 성공 후 전체 recovery를 실패 종료 또는 안전 종료 상태로 마무리한다.
• 관련 로그에 recovery mode, target tool, detected location, skip grasp reason, next task를 기록한다.

완료 조건은 다음과 같다.

• Recovery 모드에서 공구가 서랍 안에 있는지 판단한다.
• 공구가 서랍 안에 있으면 grasp를 시도하지 않는다.
• “공구가 서랍 안에 있어 잡을 수 없음” 상태가 로그 또는 메시지로 남는다.
• Grasp 시도 없이 서랍 닫기 단계로 넘어간다.
• 서랍 닫기 이후 recovery가 안전하게 종료된다.
• recovery 큐가 멈추지 않고 정상적으로 다음 단계로 진행된다.
• 서랍 내부 구조물과의 충돌 위험이 줄어든다.

[kimseoungyun]

rviz 방식이 편하기는 하나, yolo로 bbox 겹치는 정도로 서랍에 있다고 판단하고 배제하는 것도 가능.