### Project Structure

### Model Description

| File | Description |
| --- | --- |
| `yolo_v11x_best.pt` | YOLOv11x custom trained model weights for object detection inference |

## Quick Start

### Environment Setup (First Time)

> **Lab Requirement**: Each experimenter must use an independent Python virtual environment. Installing packages in the system Python is prohibited.

```bash
# 1) Create virtual environment (with access to system ROS2 / unitree_sdk2py)
python3 -m venv ~/g1act_venv --system-site-packages

# 2) Activate virtual environment
source ~/g1act_venv/bin/activate

# 3) Install project dependencies
cd ~/g1act_ws/manact_ws
pip install -r requirements.txt

# 4) Build ROS2 workspace
colcon build

# --- Every session ---
source ~/g1act_venv/bin/activate
cd ~/g1act_ws
. install/setup.bash

# 1. Launch D455 camera (Terminal 1)
ros2 launch realsense2_camera rs_launch.py camera_namespace:=robot1 camera_name:=D455_1

# 2. Build and launch YOLO detector (Terminal 2)
source ~/g1act_venv/bin/activate
colcon build --packages-select g1_yolo_nav_py && . install/setup.bash
ros2 run g1_yolo_nav_py yolo_detector

# 3. Launch visualizer (Terminal 3)
ros2 run g1_yolo_nav_py detection_visualizer

# 4. View detections (Terminal 4)
ros2 topic echo /g1/vision/detections

# Detect persons
ros2 run g1_yolo_nav_py yolo_detector --ros-args -p target_classes:='["person"]'

# Custom model path
ros2 run g1_yolo_nav_py yolo_detector --ros-args -p model_path:=/path/to/model.pt

# Terminal 1: Camera
ros2 launch realsense2_camera rs_launch.py \
    camera_namespace:=robot1 camera_name:=D455_1

# Terminal 2: YOLO detection
ros2 run g1_yolo_nav_py yolo_detector

# Terminal 3: Yaw alignment (Loco API SET_VELOCITY)
ros2 run g1_yolo_nav_py yaw_align

# Terminal 4 (optional): Visualizer
ros2 run g1_yolo_nav_py detection_visualizer

# Tracking too slow — increase kp
ros2 run g1_yolo_nav_py yaw_align --ros-args -p yaw_kp:=3.0

# Oscillating — decrease kp + widen tolerance
ros2 run g1_yolo_nav_py yaw_align --ros-args \
  -p yaw_kp:=1.0 -p center_tolerance:=0.12

# Limit max rotation speed
ros2 run g1_yolo_nav_py yaw_align --ros-args -p max_yaw_speed:=0.3

# Terminal 1~3: Same as above (camera + YOLO + yaw_align)

# Terminal 4: Forward control (Loco API SET_VELOCITY)
ros2 run g1_yolo_nav_py loco_forward

# Use low speed for first test
ros2 run g1_yolo_nav_py loco_forward --ros-args -p forward_speed:=0.15

# Get closer before stopping
ros2 run g1_yolo_nav_py loco_forward --ros-args -p arrive_bbox_ratio:=0.6

# Option A: Launch separately (recommended for debugging)
# Terminal 1: Camera
# Terminal 2: ros2 run g1_yolo_nav_py yolo_detector
# Terminal 3: ros2 run g1_yolo_nav_py yaw_align
# Terminal 4: ros2 run g1_yolo_nav_py loco_forward

# Option B: Launch file
ros2 launch g1_yolo_nav_py yolo_nav.launch.py enable_approach:=true

IDLE → [Search] → SEARCHING (rotate to find target)
                     ↓ Target detected
                   ALIGNING (yaw alignment to center)
                     ↓ Centered & stable
                   APPROACHING (Loco API forward)
                     ↓ Arrived at target
                   GRABBING (execute armup.py)
                     ↓ Grasp complete
                   MENU (put down / turn & put down)
                     ↓ Put down complete
                   IDLE

# Terminal 1: YOLO detection
ros2 run g1_yolo_nav_py yolo_detector

# Terminal 2: Control panel
ros2 run g1_yolo_nav_py control_panel

# Terminal 1: Camera
ros2 launch realsense2_camera rs_launch.py \
    camera_namespace:=robot1 camera_name:=D455_1 align_depth.enable:=true

# Terminal 2: YOLO detection
ros2 run g1_yolo_nav_py yolo_detector

# Terminal 3: Grasp task (Loco API mode, no DDS required)
ros2 run g1_yolo_nav_py grasp_task

==================================================
G1 Grasp Task Started (Loco API mode)
Target class: chair
armup: ~/g1act_ws/manact_ws/src/g1_yolo_nav_py/arm/armup.py
armdown: ~/g1act_ws/manact_ws/src/g1_yolo_nav_py/arm/armdown.py
==================================================
[SEARCHING]  Rotating to search for target...
[Detect] Target found: chair, confidence=95%, u=0.32
[State] SEARCHING → ALIGNING: Target found
[State] ALIGNING → APPROACHING: Target centered
[State] APPROACHING → GRABBING: Arrived at target! bbox=0.46 >= 0.45
[Grasp] Executing armup.py ...
[Grasp] armup.py completed

========================================
  G1 Grasp Task — Action Menu
========================================
  1. Put down object
  2. Turn & put down object
  3. Custom control (input xyz)
  4. Exit
========================================
Select [1-4]:

# Detect bottles, reduce speed
ros2 run g1_yolo_nav_py grasp_task --ros-args \
  -p target_class_id:=bottle \
  -p forward_speed:=0.15

# Custom arm script directory
ros2 run g1_yolo_nav_py grasp_task --ros-args \
  -p arm_script_dir:=/home/unitree/my_arm_scripts

# Camera must be launched with depth alignment
ros2 launch realsense2_camera rs_launch.py \
    camera_namespace:=robot1 camera_name:=D455_1 align_depth.enable:=true

# Run capture (default: 1 frame every 5s, 60s total)
ros2 run g1_yolo_nav_py rgbd_capture

# Custom parameters
ros2 run g1_yolo_nav_py rgbd_capture --ros-args \
  -p interval_sec:=2.0 -p duration_sec:=30.0

SET_FSM_ID(DAMP=1) → SET_FSM_ID(STAND_UP=4) → SET_FSM_ID(WALK_RUN=801) → SET_BALANCE_MODE(CONTINUOUS_GAIT=1)