# 人脸跟随和自动避障系统

## 功能概述

本系统实现了基于深度相机的人脸跟随和自动避障功能，适用于机器人在高级开发模式下的自主导航。

### 主要功能

1. **人脸检测与识别**：使用InsightFace库进行人脸检测和识别
2. **人脸位置跟踪**：根据人脸在图像中的位置调整机器人朝向
3. **自动避障**：基于深度相机数据检测障碍物并自动避开
4. **智能导航**：结合人脸跟随和避障功能，实现安全的人脸跟踪

## 系统架构

### 节点说明

1. **face_recognition_node**：人脸检测识别节点
   - 订阅彩色相机话题
   - 运行人脸检测和识别模型
   - 发布人脸名称和位置信息

2. **face_tracking_obstacle_avoidance_node**：人脸跟随和避障节点
   - 订阅人脸位置信息
   - 订阅深度相机数据
   - 计算并发布机器人速度命令

### 话题列表

| 话题名称 | 消息类型 | 方向 | 描述 |
|---------|---------|------|------|
| /camera/camera/color/image_raw | sensor_msgs/Image | 订阅 | 彩色相机图像 |
| /camera/camera/depth/image_rect_raw | sensor_msgs/Image | 订阅 | 深度相机图像 |
| /camera/camera/depth/camera_info | sensor_msgs/CameraInfo | 订阅 | 相机内参 |
| face_recognition/name | std_msgs/String | 订阅 | 人脸识别结果 |
| face_recognition/position | std_msgs/String | 订阅 | 人脸位置信息 |
| /motion/body_vel_cmd | interface_protocol/BodyVelCmd | 发布 | 机器人速度命令 |

## 安装与使用

### 前置要求

- ROS 2 Humble
- Python 3.10+
- InsightFace库
- OpenCV
- NumPy
- message_filters

### 安装依赖

```bash
cd /home/ubuntu/eyes
pip install insightface opencv-python numpy
```

### 编译包

```bash
cd /home/ubuntu/eyes
colcon build --packages-select eye_ros
source install/setup.bash
```

### 启动系统

#### 方式1：使用启动文件（推荐）

```bash
ros2 launch eye_ros face_tracking_obstacle_avoidance.launch.py
```

#### 方式2：分别启动节点

```bash
# 终端1：启动人脸识别节点
ros2 run eye_ros face_recognition_node.py

# 终端2：启动人脸跟随和避障节点
ros2 run eye_ros face_tracking_obstacle_avoidance_node.py
```

### 参数配置

#### 人脸识别节点参数

- `image_topic`：彩色相机话题（默认：/camera/camera/color/image_raw）
- `db_path`：人脸数据库路径（默认：/home/ubuntu/eyes/src/eye_ros/face_database/face_database.pkl）
- `model_name`：人脸识别模型名称（默认：buffalo_l）

#### 人脸跟随和避障节点参数

- `depth_topic`：深度相机话题（默认：/camera/camera/depth/image_rect_raw）
- `camera_info_topic`：相机内参话题（默认：/camera/camera/depth/camera_info）
- `face_name_topic`：人脸名称话题（默认：face_recognition/name）
- `face_position_topic`：人脸位置话题（默认：face_recognition/position）
- `body_vel_cmd_topic`：机器人速度命令话题（默认：/motion/body_vel_cmd）

## 控制算法

### 人脸跟随控制

1. **偏航控制**：根据人脸在图像中的X坐标偏差调整机器人朝向
   - 人脸在左侧：机器人左转
   - 人脸在右侧：机器人右转
   - 控制增益：0.005

2. **前进控制**：根据人脸在图像中的Y坐标调整前进速度
   - 人脸在上方（较远）：前进
   - 人脸在下方（较近）：后退
   - 控制增益：0.1

### 自动避障控制

1. **障碍物检测**：将深度图像分为左、中、右三个区域
   - 计算每个区域的最小距离
   - 障碍物阈值：0.8米

2. **避障策略**：
   - 检测到障碍物时停止前进
   - 比较左右两侧距离，选择空间更大的方向
   - 根据障碍物距离调整转向速度

## 性能参数

- 最大偏航速度：0.3 rad/s
- 最大线速度：0.3 m/s
- 障碍物检测阈值：0.8 m
- 控制频率：20 Hz

## 注意事项

1. 确保深度相机已正确启动并发布深度数据
2. 确保机器人已进入高级开发模式
3. 确保人脸数据库已正确配置
4. 首次使用时建议在安全环境下测试
5. 避障功能依赖于深度数据质量，请确保深度相机正常工作

## 故障排除

### 问题1：机器人不移动

- 检查是否检测到人脸
- 检查深度数据是否正常
- 检查机器人是否进入高级开发模式

### 问题2：避障不工作

- 检查深度相机话题是否正确
- 检查深度数据质量
- 调整障碍物检测阈值

### 问题3：人脸跟踪不准确

- 检查人脸识别模型是否正确加载
- 调整控制增益参数
- 确保光照条件良好

## 开发者

本系统基于ROS 2开发，遵循ROS 2最佳实践。

## 许可证

Apache-2.0
