# human_follower

**Repository Path**: ylzb-robot/human_follower

## Basic Information

- **Project Name**: human_follower
- **Description**: 跟随逻辑
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: main
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2025-11-14
- **Last Updated**: 2026-01-06

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# Human Follower - ROS2 人体跟随系统

基于 Hobot AI Detection 和 Nav2 行为树的 ROS2 人体跟随项目（无需深度相机）。

## 功能特性

- **Hobot AI 检测集成**: 订阅已有的 `/perception/detection/reid` 话题 (hobot_msgs/PerceptionTargets)
- **2D 姿态估算**: 基于检测框大小估算距离，无需深度相机
- **智能跟随**: 使用 Nav2 导航栈实现平滑、安全的人体跟随
- **Track ID 持续跟随**: 基于 track_id 锁定同一目标，避免多人场景中频繁切换
- **自定义行为树**: 基于 Nav2 行为树的跟随逻辑，支持恢复行为
- **多目标选择**: 支持跟随最近、最大或第一个检测到的目标

## 系统架构

```
Hobot AI 检测 → PerceptionTargets (/perception/detection/reid)
                        ↓
                2D姿态估算 → 目标姿态 → 跟随控制器 → Nav2 → 机器人运动
                (检测框大小)      ↓
                               TF发布
```

### 核心原理

**距离估算**: 使用检测框大小的反比关系
```
distance ≈ (reference_bbox_width / current_bbox_width) × reference_distance
```

**横向定位**: 根据检测框中心位置
```
lateral_offset = (center_x - image_width/2) × lateral_gain × distance
```

### 节点说明

1. **pose_2d_converter.py** (`nodes/pose_2d_converter.py:1`)
   - 订阅 Hobot PerceptionTargets 消息
   - 基于检测框大小估算距离
   - 根据检测框中心计算横向偏移
   - 发布目标 PoseStamped 和 TF

2. **follow_controller.py** (`nodes/follow_controller.py:1`)
   - 订阅目标姿态
   - 计算保持安全距离的跟随点
   - 通过 Nav2 发送导航目标

## 依赖项

### ROS2 包
```bash
sudo apt install ros-humble-nav2-bringup \
                 ros-humble-nav2-msgs
```

### Hobot Messages
```bash
# 需要安装 hobot_msgs 或 ai_msgs 包
# 通常由 Horizon Robotics 开发板 (RDK X3/X5) 提供
```

### Python 依赖
```bash
pip install -r requirements.txt
# 包含: numpy (无需 opencv-python 和 ultralytics)
```

### 硬件要求

- **相机**: 普通 RGB 相机即可（无需深度相机）
- **机器人**: 支持 Nav2 的移动机器人平台
- **检测**: Horizon Robotics 开发板或兼容系统（提供 PerceptionTargets 消息）

## 安装

1. 克隆到工作空间:
```bash
cd ~/ros2_ws/src
# 将 human_follower 文件夹复制到此处
```

2. 安装依赖:
```bash
cd ~/ros2_ws
rosdep install --from-paths src --ignore-src -r -y
pip install -r src/human_follower/requirements.txt
```

3. 构建:
```bash
colcon build --packages-select human_follower
source install/setup.bash
```

## 使用方法

### 前提条件

确保 Hobot AI 检测系统正在运行：

```bash
ros2 topic list | grep perception
ros2 topic echo /perception/detection/reid --once
```

### 启动系统

**基础版**（仅跟随节点）:
```bash
ros2 launch human_follower human_follower.launch.py
```

**完整版**（包含 Nav2）:
```bash
ros2 launch human_follower human_follower_full.launch.py
```

### 启动参数

- `params_file`: 参数文件路径
- `use_sim_time`: 是否使用仿真时间
- `behavior_tree`: 行为树 XML 文件路径
- `use_nav2`: 是否启动 Nav2 栈（仅 full 版本）
- `map`: 地图文件路径（可选）

## 标定步骤 ⚠️ 重要

系统需要标定才能准确估算距离：

### 1. 测量参考数据

让目标人站在**已知距离**（建议 1.5 米）

### 2. 查看检测框大小

```bash
ros2 topic echo /perception/detection/reid
```

记录检测框坐标：
```
roi.rect.points[0].x = 220  # 左上角 x
roi.rect.points[1].x = 420  # 右下角 x
bbox_width = 420 - 220 = 200 像素
```

### 3. 更新配置

编辑 `config/follow_params.yaml`:

```yaml
pose_2d_converter:
  ros__parameters:
    image_width: 640.0           # 你的相机分辨率
    image_height: 480.0
    reference_bbox_width: 200.0  # 步骤2测量的宽度
    reference_distance: 1.5      # 步骤1设定的距离
```

### 4. 验证

在不同距离测试，检查估算是否合理。

## 配置

主要配置文件: `config/follow_params.yaml`

### 姿态转换器配置

```yaml
pose_2d_converter:
  ros__parameters:
    detection_topic: "/perception/detection/reid"
    target_selection: "closest"    # closest/largest/first
    target_type_filter: ""         # 按类型过滤（如 "person"）

    # Track ID 持续跟随
    track_persistence: true        # 启用 track_id 跟随
    track_timeout: 2.0             # 目标丢失后等待时间（秒）

    # 图像参数（必须与相机匹配）
    image_width: 640.0
    image_height: 480.0

    # 标定参数
    reference_bbox_width: 200.0   # 参考检测框宽度（像素）
    reference_distance: 1.5       # 参考距离（米）

    # 距离限制
    min_distance: 0.8            # 最小距离（米）
    max_distance: 3.0            # 最大距离（米）

    # 横向增益
    lateral_gain: 0.002          # 米/像素
```

### 跟随控制器配置

```yaml
follow_controller:
  ros__parameters:
    follow_distance: 1.0        # 保持距离（米）
    update_threshold: 0.3       # 移动阈值（米）
    target_timeout: 2.0         # 超时时间（秒）
```

## 话题接口

### 订阅话题

- `/perception/detection/reid` (ai_msgs/PerceptionTargets): Hobot AI 检测结果

### 发布话题

- `/human_target_pose` (geometry_msgs/PoseStamped): 目标姿态

### TF 帧

- `human_target`: 跟随目标的 TF 帧

## 调试

### 查看检测消息

```bash
ros2 topic echo /perception/detection/reid
```

### 查看目标姿态

```bash
ros2 topic echo /human_target_pose
```

### 可视化 TF

```bash
rviz2
# 添加 TF display，查看 human_target 帧
```

### 查看节点日志

```bash
ros2 node info /pose_2d_converter
```

节点会输出详细日志：
```
Target [person] track_id:1 bbox_w:245px → pose: (1.22, -0.15) in base_link
```

## 常见问题

### 1. 检测话题不存在

确保 Hobot AI 系统运行：
```bash
ros2 topic list | grep perception
```

### 2. 距离估算不准确

**原因**: 标定参数不正确

**解决**:
1. 重新测量 `reference_bbox_width`
2. 确保 `image_width` 正确
3. 在多个距离验证

### 3. 机器人左右摇摆

**原因**: `lateral_gain` 太大

**解决**:
```yaml
lateral_gain: 0.001  # 减小增益
```

### 4. 机器人反应太慢

**解决**:
```yaml
lateral_gain: 0.003           # 增大增益
follow_controller:
  update_threshold: 0.2       # 减小阈值
```

### 5. 不同身高误差大

**原因**: 单一标定值

**建议**: 使用平均身高标定，或为不同目标类型设置不同参数

## 参数调优指南

### 图像尺寸（必须匹配相机）

```yaml
image_width: 640.0   # 常见: 640, 1280, 1920
image_height: 480.0  # 常见: 480, 720, 1080
```

### 距离范围（根据应用场景）

```yaml
# 室内狭窄空间
min_distance: 0.5
max_distance: 2.0

# 开阔空间
min_distance: 1.0
max_distance: 4.0
```

### 横向增益（控制灵敏度）

```yaml
lateral_gain: 0.001  # 慢速，稳定
lateral_gain: 0.002  # 默认
lateral_gain: 0.003  # 快速，可能抖动
```

### 目标选择

```yaml
target_selection: "closest"   # 跟随最近的（推荐）
target_selection: "largest"   # 跟随最大的
target_selection: "first"     # 跟随第一个
```

### Track ID 跟随

```yaml
# 启用后，机器人会持续跟随同一个 track_id 的目标
# 即使出现其他更近的人，也不会切换
track_persistence: true      # 启用持续跟随
track_timeout: 2.0          # 超时时间（秒）

# 禁用后，每次都重新选择目标（可能频繁切换）
track_persistence: false
```

**工作原理**:
1. 首次检测时，根据 `target_selection` 策略选择一个目标
2. 记录该目标的 `track_id`
3. 后续帧中优先跟随相同 `track_id` 的目标
4. 如果目标丢失超过 `track_timeout` 秒，则重新选择新目标

**适用场景**:
- ✅ 多人环境 - 避免频繁切换跟随对象
- ✅ 人群穿行 - 保持锁定原目标
- ❌ 单人场景 - 可以禁用以提高响应速度

## 优缺点

### ✅ 优点

- 无需深度相机 - 降低成本
- 普通RGB相机即可
- 计算简单快速
- 适合相对跟随场景

### ⚠️ 限制

- 精度较低（±15-25cm）
- 需要场景标定
- 受目标大小变化影响
- 不适合高精度应用

## 适用场景

### ✅ 推荐

- 室内引导机器人
- 展会演示机器人
- 相对跟随即可的应用
- 成本敏感项目

### ❌ 不推荐

- 需要厘米级精度
- 安全关键应用
- 复杂障碍环境
- 多目标复杂场景

## 行为树

项目提供两个行为树模板：

1. **follow_human_simple.xml**: 简化版（推荐）
2. **follow_human.xml**: 基础版

基于 Nav2 follow_point 模式，包含：
- 路径规划恢复
- 路径跟踪恢复
- 代价地图清理
- 旋转恢复

## 与 Hobot 系统集成

本项目设计用于 Horizon Robotics AI 系统：

- **RDK X3**: 地平线旭日X3开发板
- **RDK X5**: 地平线旭日X5开发板
- **hobot_msgs**: Horizon Robotics 消息定义

典型架构：
```
Hobot MIPI Camera → Hobot Codec → Hobot DNN → PerceptionTargets
                                                      ↓
                                              human_follower
```

## 扩展开发

### 使用 track_id 跟踪特定目标

修改 `pose_2d_converter.py:150` 的 `select_target` 方法。

### 添加滤波平滑

在 `estimate_distance` 方法中添加移动平均或卡尔曼滤波。

### 多尺度标定

为不同距离范围使用不同的标定参数。

## 参考资料

- [DEPTH_FREE_GUIDE.md](DEPTH_FREE_GUIDE.md) - 详细使用指南
- [QUICK_START_2D.md](QUICK_START_2D.md) - 快速参考
- [Nav2 Documentation](https://navigation.ros.org/)
- [Horizon Robotics Developer](https://developer.horizon.ai/)

## 许可证

Apache-2.0

## 贡献

欢迎提交 Issue 和 Pull Request!

## 更新日志

### v2.1.0 (当前)
- 🎯 添加 Track ID 持续跟随功能
- 🔒 基于 track_id 锁定目标，避免多人场景切换
- ⏱️ 可配置目标丢失超时时间
- 📝 更新文档说明新功能

### v2.0.0
- 🎯 简化为纯 2D 方案
- 🗑️ 移除深度相机依赖
- ✨ 基于检测框大小估算距离
- 📖 完善标定和使用文档

### v1.1.0
- 新增无深度相机方案
- 添加 pose_2d_converter 节点

### v1.0.0
- 集成 Hobot AI Messages
- 订阅 /perception/detection/reid 话题

---

**注意**:
- ⚠️ 使用前必须进行标定
- ⚠️ 精度低于深度方案，请充分测试
- ⚠️ 适合相对跟随，不适合高精度导航