[Bug] Increased Delay During Drawer Ownership Searching Before Yaw Inference Movement

[kimseoungyun]

🐞 버그 설명

Yaw 추론 위치로 이동하기 전에 로봇팔이 서랍 위 위치를 먼저 경유한 뒤, yaw 추론 위치로 조금 더 이동하는 과정에서 이전보다 시간이 오래 걸리는 문제가 발생한다.

해당 구간은 서랍의 ownership을 판단하기 위한 searching 단계로 보이며, 기존에는 비교적 짧은 시간 안에 서랍 위 위치를 거쳐 yaw 추론 위치로 이동했지만, 최근에는 이 사이의 대기 시간 또는 이동 시간이 길어져 전체 작업 수행 시간이 증가하고 있다.

🔁 재현 방법

1. 사용자가 공구 pick 또는 서랍 내부 객체 탐색이 필요한 명령을 입력한다.
   예: 드라이버 가져다줘, 망치 찾아줘

2. 로봇팔이 yaw 추론 위치로 이동하기 전에 서랍 위 위치로 먼저 이동하는지 확인한다.

3. 서랍 위 위치에 도달한 이후 yaw 추론 위치로 이동하기까지의 시간이 이전보다 길어졌는지 확인한다.

✅ 기대한 동작

로봇팔은 서랍 위 위치를 경유한 뒤, 불필요한 대기 없이 yaw 추론 위치로 자연스럽게 이동해야 한다.

서랍 ownership 판단을 위한 searching 단계가 필요하더라도, 해당 단계는 최소한의 시간 안에 완료되어야 하며, perception 결과 또는 상태 판단이 완료되면 바로 다음 이동 단계로 넘어가야 한다.

기대 동작은 다음과 같다.

• 서랍 위 위치 도달
• ownership/searching 상태 판단
• yaw 추론 위치로 즉시 이동
• 불필요한 sleep, timeout, retry, blocking wait 없이 다음 단계 진행

❌ 실제 동작

로봇팔이 서랍 위 위치에 도달한 이후 yaw 추론 위치로 이동하기까지의 시간이 이전보다 오래 걸린다.

이로 인해 전체 pick/searching pipeline의 수행 시간이 증가하며, 사용자가 보기에는 로봇이 서랍 위에서 불필요하게 멈춰 있거나 늦게 반응하는 것처럼 보인다.

현재 원인은 명확하지 않지만, 다음과 같은 가능성이 있다.

• searching 단계에서 timeout 또는 retry 시간이 증가함
• ownership 판단 로직에서 perception 결과를 기다리는 시간이 길어짐
• 서랍 위 위치 도달 후 다음 state로 전환되는 조건이 늦게 만족됨
• 이전에는 없던 sleep, wait, stabilization delay가 추가됨
• motion 완료 판정 또는 feedback 수신이 지연됨
• yaw 추론 위치로 이동하기 전 불필요한 중간 상태가 추가됨

🖥️ 실행 환경

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

📋 로그 또는 에러 메시지

Yaw inference position 이동 전 searching 단계에서 지연 발생.

관찰된 흐름:
1. Move to drawer-top/searching position
2. Wait/search for drawer ownership
3. Move slightly to yaw inference position

문제:
- 1번 위치 도달 이후 3번 위치로 이동하기까지의 시간이 이전보다 오래 걸림
- 명확한 에러 로그는 없지만, searching/ownership 판단 단계 또는 state transition에서 지연이 발생하는 것으로 추정됨

🎥 스크린샷/영상

다음 자료를 첨부하면 원인 분석에 도움이 됨.

• 서랍 위 위치에 도달한 시점의 영상
• yaw 추론 위치로 이동하기 전까지 멈춰 있는 구간 영상
• 이전 정상 동작과 현재 지연 동작 비교 영상
• 각 state 진입/종료 시간이 찍힌 ROS 2 로그
• motion command 발행 시간과 완료 feedback 수신 시간 로그

📎 추가 정보

원인 파악을 위해 searching 단계와 yaw 추론 위치 이동 전후의 timestamp 로그를 추가할 필요가 있다.

확인해야 할 지점은 다음과 같다.

• 서랍 위 위치로 이동 명령을 보낸 시간
• 서랍 위 위치 도달 feedback을 받은 시간
• ownership/searching 단계 시작 시간
• ownership/searching 단계 종료 시간
• yaw 추론 위치 이동 명령을 보낸 시간
• yaw 추론 위치 도달 feedback을 받은 시간

특히 다음 항목을 우선적으로 확인해야 한다.

• searching 단계에 고정 sleep이 추가되었는지
• ownership 판단 timeout 값이 증가했는지
• perception 결과 대기 로직이 blocking 방식으로 동작하는지
• motion 완료 판정 조건이 과도하게 엄격해졌는지
• 이전 버전 대비 state machine 전환 조건이 변경되었는지
• yaw 추론 위치로 이동하기 전 불필요한 retry 또는 안정화 대기 시간이 추가되었는지

예상 수정 방향은 다음과 같다.

[drawer_top_arrived] timestamp 기록
[searching_start] timestamp 기록
[searching_done] timestamp 기록
[yaw_pose_command_sent] timestamp 기록
[yaw_pose_arrived] timestamp 기록

위 로그를 통해 지연이 motion 단계에서 발생하는지, searching/ownership 판단 단계에서 발생하는지, 또는 state transition 단계에서 발생하는지 분리해서 확인해야 한다.