# nav2_humanoid 

**Repository Path**: unity-robotics/nav2humanoid

## Basic Information

- **Project Name**: nav2_humanoid 
- **Description**: nav2 适配人形机器人
- **Primary Language**: C++
- **License**: Apache-2.0
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 1
- **Forks**: 0
- **Created**: 2025-12-06
- **Last Updated**: 2026-01-16

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# nav2_humanoid

[English](README.en.md) | 简体中文

## 📖 项目简介

nav2_humanoid 是一个将 Navigation2 (Nav2) 适配到人形机器人的 ROS2 功能包。本项目将 ROS1 项目 [ros_navigation_humanoid](https://github.com/deepglint/ros_navigation_humanoid) 的人形机器人导航功能迁移到 ROS2 Humble 环境。

**核心设计原则**：
- ✅ **不修改 Navigation2 源码** - 通过插件和配置实现适配
- ✅ **清晰的架构分离** - 自定义功能与 Nav2 核心分离
- ✅ **开源维护** - 在 Gitee 上持续维护和更新

**项目状态**：🟢 核心功能可用（RViz仿真、步态仿真），持续开发中（真机集成待测试）

---

## 🏗️ 项目架构

```
nav2humanoid/
├── navigation2/                          # Nav2 源码 (v1.1.18, 不修改)
│   ├── nav2_bt_navigator/
│   ├── nav2_controller/
│   ├── nav2_planner/
│   └── ... (42个功能包)
│
└── src/                                  # 人形机器人适配代码
    ├── nav2_humanoid_bringup/            # 启动和配置整合包
    │   ├── launch/                       # Python launch 文件
    │   │   ├── humanoid_rviz_sim.launch.py
    │   │   └── humanoid_nav.launch.py
    │   ├── config/                       # Nav2 参数配置
    │   │   └── nav2_humanoid_params.yaml
    │   ├── urdf/                         # 机器人模型
    │   │   ├── humanoid_robot.urdf (-> unitree_g1/...)
    │   │   └── unitree_g1/               # Unitree G1 示例模型
    │   ├── rviz/                         # RViz 配置
    │   │   └── humanoid_nav.rviz
    │   └── maps/                         # 地图文件
    │
    ├── nav2_humanoid_rviz_sim/           # RViz 仿真节点
    │   └── src/rviz_sim_node.cpp         # 订阅 /cmd_vel 发布里程计
    │
    └── nav2_humanoid_footstep_sim/       # 步态仿真节点
        └── src/footstep_sim_node.cpp     # 订阅步态规划器输出，模拟双足运动
```

---

## 🔑 关键特性

### 人形机器人适配特点

1. **特殊的 TF 树结构**
   ```
   map → pelvis (腰关节) → base_link → 各关节
   ```
   - 使用 `pelvis` 作为机器人中心坐标系
   - 可配置的腰部高度参数 `pelvis_to_foot_height` (默认 0.8m)

2. **保守的速度参数**
   - 最大线速度：0.5 m/s
   - 最大角速度：1.0 rad/s
   - 适度的加速度限制以保证稳定性

3. **DWB 局部规划器**
   - 使用 Nav2 的 DWB (Dynamic Window Approach) 控制器
   - 针对人形机器人优化的代价函数权重
   - 支持无损仿真模式（通过里程计积分）

4. **灵活的插件架构**
   - 未来可扩展：自定义控制器、步态规划器等
   - 通过 pluginlib 机制集成

---

## 📦 依赖项

### ROS2 依赖
```bash
sudo apt-get update
sudo apt-get install -y \
  ros-humble-navigation2 \
  ros-humble-nav2-bringup \
  ros-humble-tf2-ros \
  ros-humble-robot-state-publisher \
  ros-humble-joint-state-publisher
```

### 构建工具
```bash
sudo apt-get install -y \
  python3-colcon-common-extensions \
  python3-rosdep
```

---

## 🚀 编译与安装

### 1. 创建工作空间（如果还没有）
```bash
mkdir -p ~/humanoid_ws/src
cd ~/humanoid_ws/src
```

### 2. 克隆本仓库
```bash
# 从 Gitee 克隆
git clone https://gitee.com/your-username/nav2humanoid.git
cd nav2humanoid
```

### 3. 安装依赖
```bash
cd ~/humanoid_ws
rosdep install -y --from-paths src --ignore-src --rosdistro humble
```

### 4. 编译
```bash
cd ~/humanoid_ws
colcon build --symlink-install --packages-select \
  nav2_humanoid_rviz_sim \
  nav2_humanoid_footstep_sim \
  nav2_humanoid_bringup
```

### 5. 配置环境
```bash
source ~/humanoid_ws/install/setup.bash
```

---

## 📚 使用说明

### 场景 1：RViz 仿真模式

在 RViz 中进行导航仿真，适合调试和参数调优。

```bash
# 启动仿真
ros2 launch nav2_humanoid_bringup humanoid_rviz_sim.launch.py
```

**功能**：
- 加载地图服务器
- 启动 Nav2 导航栈
- 运行 RViz 仿真节点（订阅 `/cmd_vel`，通过里程计积分发布 TF）
- 打开 RViz2 可视化界面

**使用步骤**：
1. 在 RViz 中使用 "2D Pose Estimate" 设置初始位置
2. 使用 "2D Goal Pose" 发布目标点
3. 观察机器人在地图中的运动

### 场景 2：真机导航模式

连接真实的人形机器人进行导航。

```bash
# 启动真机导航
ros2 launch nav2_humanoid_bringup humanoid_nav.launch.py
```

**前提条件**：
- 机器人提供实时定位话题（如 `/localization_3d`）
- 激光雷达或点云数据发布到 `/scan` 话题
- 机器人能够接收 `/cmd_vel` 速度指令

### 场景 3：步态仿真模式（实验性）

使用步态规划器进行双足步态仿真。

```bash
# 启动步态仿真
ros2 launch nav2_humanoid_bringup humanoid_footstep_sim.launch.py
```

**功能**：
- 订阅步态规划器输出（`/footstep_planner/footsteps_array`）
- 模拟双足行走步态
- 动态更新髋关节角度
- 发布 `map → pelvis` TF 变换

**前提条件**：
- 需要额外启动步态规划器节点（如 `footstep_planner`）
- 依赖 `humanoid_msgs` 和相关功能包
- 适用于需要精细步态控制的人形机器人

**注意**：此功能需要额外的依赖包，参考 ROS1 原始项目的说明进行配置。

---

## ⚙️ 配置说明

### 1. 机器人参数

编辑 [config/nav2_humanoid_params.yaml](src/nav2_humanoid_bringup/config/nav2_humanoid_params.yaml)：

```yaml
# 机器人基础参数
robot_base_frame: pelvis           # 人形机器人使用 pelvis
robot_radius: 0.22                 # 机器人半径 (m)

# 速度限制
max_vel_x: 0.5                     # 最大前进速度
min_vel_x: 0.1                     # 最小前进速度
max_vel_theta: 1.0                 # 最大旋转速度

# 加速度限制
acc_lim_x: 1.0                     # 线性加速度
acc_lim_theta: 0.6                 # 角加速度
```

### 2. 腰部高度

在 launch 文件中修改：

```bash
ros2 launch nav2_humanoid_bringup humanoid_rviz_sim.launch.py \
  pelvis_to_foot_height:=0.85
```

### 3. 更换机器人模型

1. 将您的 URDF 文件放置到 `src/nav2_humanoid_bringup/urdf/`
2. 修改符号链接：
   ```bash
   cd src/nav2_humanoid_bringup/urdf
   ln -sf your_robot.urdf humanoid_robot.urdf
   ```

### 4. 添加地图

将地图文件放置到 `src/nav2_humanoid_bringup/maps/`：
- `map.pgm` - 占据栅格地图图像
- `map.yaml` - 地图元数据

---

## 🔧 开发与扩展

### 添加自定义控制器插件

1. 创建新包继承 `nav2_core::Controller`
2. 在 `nav2_humanoid_params.yaml` 中配置：
   ```yaml
   controller_plugins: ["YourCustomController"]
   YourCustomController:
     plugin: "your_package::YourController"
   ```

---

## 📊 与 ROS1 版本的对比

| 特性 | ROS1 (ros_navigation_humanoid) | ROS2 (nav2_humanoid) |
|------|--------------------------------|----------------------|
| **导航框架** | move_base | Navigation2 |
| **节点通信** | ROS topics/services | rclcpp (DDS) |
| **Launch 文件** | XML | Python API |
| **TF 库** | tf | tf2 |
| **生命周期管理** | ❌ | ✅ Lifecycle Nodes |
| **行为树** | ❌ 硬编码状态机 | ✅ BehaviorTree XML |

---

## 🛠️ 后续工作计划

### 已完成 ✅
- [x] 迁移 `rviz_sim.cpp` 到 ROS2（RViz 仿真）
- [x] 迁移 `footstep_sim.cpp` 到 ROS2（步态仿真）
- [x] 线程安全修复（v0.3.0）
- [x] API兼容性更新（rclcpp::WallRate）
- [x] Launch文件健壮性改进（v0.2.0）
- [x] 完善中英文文档

### 计划中 📋
- [ ] 添加示例地图文件
- [ ] 开发自定义人形机器人控制器插件
- [ ] 集成 IMU 数据用于平衡控制
- [ ] 实现离散步态规划器（ROS2版本）
- [ ] 添加单元测试和集成测试
- [ ] 真机测试和参数调优

---

## 📝 参考资源

- **原始 ROS1 项目**: [ros_navigation_humanoid](https://github.com/deepglint/ros_navigation_humanoid)
- **Navigation2 官方文档**: https://navigation.ros.org/
- **ROS2 Humble 文档**: https://docs.ros.org/en/humble/

---

## 🤝 参与贡献

欢迎提交 Issue 和 Pull Request！

1. Fork 本仓库
2. 创建特性分支 (`git checkout -b feature/AmazingFeature`)
3. 提交更改 (`git commit -m 'Add some AmazingFeature'`)
4. 推送到分支 (`git push origin feature/AmazingFeature`)
5. 提交 Pull Request

---

## 📄 许可证

本项目采用 Apache-2.0 许可证 - 详见 [LICENSE](LICENSE) 文件。

---

## 👥 维护者

- **gcl** - 项目维护  ROS2 迁移
  - 单位：Unity Embodied Intelligence Robotics Technology Co., Ltd
  - 微信公众号：同舟具身智能机器人

  <img src="docs/images/wechat_qrcode.png" alt="微信公众号二维码" width="200"/>

---

## 🙏 致谢

- [Navigation2](https://github.com/ros-planning/navigation2) 团队
- [Unitree Robotics](https://www.unitree.com/) - G1 机器人模型
- 所有贡献者

---

**⭐ 如果这个项目对您有帮助，请给一个 Star！**