G1 NavGrasp

g1act_ws/
├── README.md                     # 项目说明文档（中文）
├── README.en.md                 # 项目说明文档（英文）
├── CODEBUDDY.md                # AI 编程助手指引文档
├── requirements.txt             # Python 依赖列表
├── src/
│   ├── D455.md                 # D455 相机说明文档
│   ├── g1_yolo_nav_py/        # 主 ROS2 Python 包
│   │   ├── setup.py                # 包设置（entry_points 定义）
│   │   ├── package.xml            # ROS2 包描述
│   │   ├── setup.cfg              # 包配置
│   │   ├── yolo_v11x_best.pt     # YOLOv11x 自定义训练模型
│   │   ├── config/                # 参数配置文件
│   │   │   └── yolo_nav.yaml
│   │   ├── launch/                # ROS2 启动文件
│   │   │   ├── grasp_task.launch.py
│   │   │   └── yolo_nav.launch.py
│   │   ├── arm/                   # 手臂控制脚本（unitree_sdk2py，独立进程）
│   │   │   ├── arm_common.py        # 手臂公共模块
│   │   │   ├── arm.py              # 手臂控制演示
│   │   │   ├── armup.py            # 抓取动作：伸手→抬起→夹紧保持
│   │   │   └── armdown.py          # 放下动作：伸展→下垂→归零释放
│   │   └── g1_yolo_nav_py/       # Python 模块代码
│   │       ├── __init__.py
│   │       ├── _dds_compat.py      # DDS 兼容层
│   │       ├── _detection_utils.py  # 检测工具函数
│   │       ├── _grasp_state.py     # 抓取状态机Mixin
│   │       └── 其他功能节点
│   └── base/                   # 参考包（不直接调用）
│       ├── ctrl_keyboard/        # Loco API 参考实现
│       └── g1_description/      # URDF 模型（12/23/29 dof）

cd ~/g1act_ws/manact_ws
git pull
colcon build

cd ~/g1act_ws/manact_ws
source ~/g1act_venv/bin/activate
. install/setup.bash

# 1) 创建虚拟环境（允许访问系统已安装的 ROS2 / unitree_sdk2py）
python3 -m venv ~/g1act_venv --system-site-packages

# 2) 激活虚拟环境
source ~/g1act_venv/bin/activate

# 3) 安装项目依赖
cd ~/g1act_ws/manact_ws
pip install -r requirements.txt

# 4) 编译 ROS2 工作空间
colcon build

# --- 每次启动项目 ---
source ~/g1act_venv/bin/activate
cd ~/g1act_ws
. install/setup.bash

# 1. 启动 D455 相机（终端 1）
ros2 launch realsense2_camera rs_launch.py camera_namespace:=robot1 camera_name:=D455_1

# 2. 编译并启动 YOLO 检测节点（终端 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. 启动可视化节点（终端 3）
ros2 run g1_yolo_nav_py detection_visualizer

# 4. 查看标注图像（终端 4）
ros2 topic echo /g1/vision/detections

# 指定检测目标为 person
ros2 run g1_yolo_nav_py yolo_detector --ros-args -p target_classes:='["person"]'

# 指定自定义模型路径
ros2 run g1_yolo_nav_py yolo_detector --ros-args -p model_path:=/path/to/model.pt

# 终端 1：相机
ros2 launch realsense2_camera rs_launch.py \
    camera_namespace:=robot1 camera_name:=D455_1

# 终端 2：YOLO 检测
ros2 run g1_yolo_nav_py yolo_detector

# 终端 3：偏航对齐（Loco API SET_VELOCITY）
ros2 run g1_yolo_nav_py yaw_align

# 终端 4（可选）：可视化
ros2 run g1_yolo_nav_py detection_visualizer

# 每步旋转更大 — 增大步进速度或持续时间
ros2 run g1_yolo_nav_py yaw_align --ros-args \
  -p step_yaw_speed:=0.5 -p step_duration:=0.4

# 相机更新太慢 — 增加等待时间
ros2 run g1_yolo_nav_py yaw_align --ros-args -p camera_settle_time:=8.0

# 对齐精度不够 — 缩小居中容差
ros2 run g1_yolo_nav_py yaw_align --ros-args -p center_tolerance:=0.05

# 终端 1~3：同上（相机 + YOLO + yaw_align）

# 终端 4：前进控制（Loco API SET_VELOCITY）
ros2 run g1_yolo_nav_py loco_forward

# 首次调试用低速度确保安全
ros2 run g1_yolo_nav_py loco_forward --ros-args -p forward_speed:=0.15

# 调整停止距离（离目标更近才停下）
ros2 run g1_yolo_nav_py loco_forward --ros-args -p stop_distance:=0.3

# 关闭深度距离判断，只用 bbox 占比
ros2 run g1_yolo_nav_py loco_forward --ros-args -p use_depth_distance:=false

# 方式 A：手动分别启动（推荐调试，方便看各自日志）
# 终端 1: 相机
# 终端 2: ros2 run g1_yolo_nav_py yolo_detector
# 终端 3: ros2 run g1_yolo_nav_py yaw_align
# 终端 4: ros2 run g1_yolo_nav_py loco_forward

# 方式 B：launch 一键启动
ros2 launch g1_yolo_nav_py yolo_nav.launch.py enable_approach:=true

IDLE → [搜索] → SEARCHING（旋转找目标）
                  ↓ 目标出现
                ALIGNING（步进式偏航对齐）
                  ↓ 居中稳定
                APPROACHING（Loco API 前进）
                  ↓ 深度距离 ≤ 0.5m（或 bbox 占比 ≥ 0.45）
                GRABBING（执行 armup.py）
                  ↓ 抓取完成
                MENU（可放下/右转放下）
                  ↓ 放下完成
                IDLE

# 终端 1：YOLO 检测
ros2 run g1_yolo_nav_py yolo_detector

# 终端 2：控制面板
ros2 run g1_yolo_nav_py control_panel

# 终端 1：相机
ros2 launch realsense2_camera rs_launch.py \
    camera_namespace:=robot1 camera_name:=D455_1 align_depth.enable:=true

# 终端 2：YOLO 检测
ros2 run g1_yolo_nav_py yolo_detector

# 终端 3：抓取任务（Loco API 模式，无需 DDS）
ros2 run g1_yolo_nav_py grasp_task

==================================================
G1 抓取任务启动（Loco API 模式）
目标类别: 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]  旋转搜索目标...
[检测] 识别到目标: chair, 置信度=95%, u=0.32
[状态] SEARCHING → ALIGNING: 目标已找到
[对齐] 一步: u=0.320, 误差=-0.180, 旋转≈+5.2°, 等待5.0s...
[对齐] 等待结束，重新检测: u=0.480
[状态] ALIGNING → APPROACHING: 目标已居中
[状态] APPROACHING → GRABBING: 深度距离=0.48m <= 0.50m
[抓取] 执行 armup.py ...
[抓取] armup.py 执行完成

========================================
  G1 抓取任务 — 操作菜单
========================================
  1. 放下目标物
  2. 右转放下目标物
  3. 自定义控制（输入 xyz）
  4. 退出
========================================
请选择 [1-4]:

# 检测瓶子、降低速度
ros2 run g1_yolo_nav_py grasp_task --ros-args \
  -p target_class_id:=bottle \
  -p forward_speed:=0.15

# 自定义 arm 脚本目录
ros2 run g1_yolo_nav_py grasp_task --ros-args \
  -p arm_script_dir:=/home/unitree/my_arm_scripts

# 相机启动时需开启深度对齐
ros2 launch realsense2_camera rs_launch.py \
    camera_namespace:=robot1 camera_name:=D455_1 align_depth.enable:=true

# 运行采集（默认每 5s 一帧，持续 60s）
ros2 run g1_yolo_nav_py rgbd_capture

# 自定义参数
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(START=500) → SET_FSM_ID(WALK_RUN=801) → SET_BALANCE_MODE(CONTINUOUS_GAIT=1)

# 检查相机是否发布图像
ros2 topic hz /D455_1/color/image_raw

# 检查深度图是否发布
ros2 topic hz /D455_1/depth/image_rect_raw

# 检查 YOLO 检测结果
ros2 topic echo /g1/vision/detections --once

# 检查运动控制话题订阅者
ros2 topic info /api/sport/request

# 必须启用深度对齐
ros2 launch realsense2_camera rs_launch.py \
    camera_namespace:=robot1 camera_name:=D455_1 align_depth.enable:=true

# 单独测试深度图是否正常（应输出 16UC1 或 32FC1 编码）
ros2 topic echo /D455_1/depth/image_rect_raw --once | head -5

# 用 loco_forward 观察距离日志
ros2 run g1_yolo_nav_py loco_forward --ros-args -p forward_speed:=0.0
# forward_speed:=0.0 使机器人不移动，仅观察深度距离日志

# 如果深度相机不可用，可关闭深度距离判断
ros2 run g1_yolo_nav_py grasp_task --ros-args -p use_depth_distance:=false

Step 1: 相机 → 确认 /D455_1/color/image_raw 有数据
Step 2: YOLO → 确认 /g1/vision/detections 有检测结果
Step 3: 深度图 → 确认 /D455_1/depth/image_rect_raw 有数据
Step 4: yaw_align → 确认步进式对齐正常（观察日志）
Step 5: loco_forward → forward_speed:=0.0 观察深度距离
Step 6: loco_forward → 正常速度前进，确认到达停止
Step 7: grasp_task → 完整流程测试