실습 6-3: ROS 2 Cartesian Workspace Guard

목표

• 정책 목표와 로봇 현재 pose를 별도 토픽으로 관리한다.
• workspace, 최대 step, current-state watchdog을 시험한다.
• guard 출력이 모터 명령이 아니라 검증된 중간 표현임을 이해한다.

빌드

cd ebook/book/06_sim2real_deployment/code/ros2_ws
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install
source install/setup.bash
ros2 run sim2real_safety workspace_guard

정상 입력

터미널 B에서 live timestamp를 가진 현재 pose를 20 Hz로 발행한다. ros2 topic pub의 고정 YAML timestamp는 곧 stale해지므로 실습용 publisher를 사용한다.

ros2 run sim2real_safety stamped_pose_publisher --ros-args \
  -p topic:=/robot/ee_pose -p rate_hz:=20.0 \
  -p frame_id:=base_link -p xyz:="[0.20, 0.00, 0.12]"

터미널 C에서 2 cm 이내 목표를 2 Hz로 발행한다.

ros2 run sim2real_safety stamped_pose_publisher --ros-args \
  -p topic:=/policy/ee_target -p rate_hz:=2.0 \
  -p frame_id:=base_link -p xyz:="[0.21, 0.00, 0.12]"

터미널 D에서 상태를 본다.

ros2 topic echo /safety/state --qos-durability transient_local

실패 주입

1. x=0.9인 목표를 보내 workspace fault를 발생시킨다.
2. 노드를 재시작하고 5 cm step을 보내 target step too large를 확인한다.
3. current pose 발행을 중단해 watchdog fault를 확인한다.
4. target publisher의 frame_id:=camera_link로 바꾸고 frame mismatch가 latched FAULT가 되는지 확인한다. 좌표 숫자를 base_link로 재표기해서는 안 된다.
5. quaternion_xyzw:="[0.0, 0.0, 0.0, 0.0]"와 NaN 성분을 각각 보내 quaternion/finite 검사를 확인한다.
6. ros2 topic pub --once로 stamp가 0인 메시지를 보내 timestamp missing을 확인한다.
7. fault 뒤 정상 목표가 자동으로 통과하지 않는지 확인한다.

실물 연결 전 추가 항목

• orientation 유효성, quaternion normalization
• source timestamp freshness와 future timestamp tolerance
• frame mismatch 거부 또는 timestamped TF2 변환
• FK 기반 joint limit과 self-collision
• command/feedback sequence ID
• driver HOLD/torque-off acknowledgment
• 물리 E-stop 입력과 재승인 절차
• ROS 노드가 멈춰도 작동하는 독립 안전 회로

QoS 계약

• current pose는 sensor-data QoS로 최신값을 우선한다.
• policy target과 accepted target은 reliable depth 1을 사용한다.
• fault state는 transient-local reliable QoS로 late subscriber도 마지막 latched fault를 읽게 한다.
• 실제 driver 연결에서는 accepted target 수신만으로 실행 성공을 가정하지 말고 HOLD/torque-off acknowledgement를 별도 확인한다.