# PNGNav
这是我们在 TurtleBot 导航中使用的 NIRRT*-PNG(带有基于点云状态表示的神经启发RRT*)的 ROS 实现,这是我们在 ICRA 2024 论文中介绍的方法。
### Neural Informed RRT*: 基于点云状态表示的学习路径规划在允许的椭圆约束下
##### [Zhe Huang](Zhe Huang), [Hongyu Chen](https://www.linkedin.com/in/hongyu-chen-91996b22b), [John Pohovey](https://www.linkedin.com/in/johnp14/), [Katherine Driggs-Campbell](KRDC – Katie Driggs-Campbell)
[Paper] [[arXiv](https://arxiv.org/abs/2309.14595)] [[GitHub for train/eval](GitHub - tedhuang96/nirrt_star: [ICRA24] Neural Informed RRT*)] [[Project](https://sites.google.com/view/nirrt-star)]
所有代码均在 Ubuntu 20.04 上使用 CUDA 12.0、ROS Noetic、conda 23.11.0、Python 3.9.0 和 PyTorch 2.0.1 进行开发和测试。该存储库提供了 ROS 包 `png_navigation`,其中包含了 RRT*、Informed RRT*、Neural RRT* 和我们的 NIRRT*-PNG 的 rospy 实现,用于 TurtleBot 导航。我们提供了如何在 Gazebo 仿真中使用 `png_navigation` 的说明,并且 `png_navigation` 可以轻松应用于实际场景中。
PointNet++ 和 PointNet 模型用于 PNG(基于点云网络引导)的训练脚本将很快发布。
### 引用
如果您发现这个存储库有用,请引用
```
@article{huang2023neural,
title={Neural Informed RRT*: Learning-based Path Planning with Point Cloud State Representations under Admissible Ellipsoidal Constraints},
author={Huang, Zhe and Chen, Hongyu and Pohovey, John and Driggs-Campbell, Katherine},
journal={arXiv preprint arXiv:2309.14595},
year={2023}
}
```
## 设置
1. 运行
```
cd ~/PNGNav
catkin_make
```
2. 运行
```
conda env create -f environment.yml
```
3. 将脚本中的 shebang 行中的 `/home/zhe/miniconda3/` 替换为您自己的系统路径。例如,如果您使用的是 ubuntu 和 miniconda3,并且您的账户名是 `abc`,则将 `/home/zhe/miniconda3/` 替换为 `/home/abc/miniconda3`。
4. 下载 [PointNet++ 模型权重](https://drive.google.com/file/d/1YfocGh1pcr_Eg8XhEAxmwaAZQsosjRhM/view?usp=sharing) 用于 PNG,创建文件夹 `model_weights/` 在 `PNGNav/src/png_navigation/src/png_navigation/wrapper/pointnet_pointnet2/` 中,并将下载的 `pointnet2_sem_seg_msg_pathplan.pth` 移动到 `model_weights`。
```
cd ~/PNGNav/src/png_navigation/src/png_navigation/wrapper/pointnet_pointnet2/
mkdir model_weights
cd model_weights
mv ~/Downloads/pointnet2_sem_seg_msg_pathplan.pth .
```
5. 如果您有 `map_realworld.pgm` 和 `map_realworld.yaml`,请将它们移动到 `PNGNav/src/png_navigation/src/png_navigation/maps`。
## 如何运行 TurtleBot3 Gazebo 仿真
### 仿真设置
按照 [TurtleBot3 官方网站](TurtleBot3) 上的教程安装依赖项,并测试 TurtleBot3 gazebo 仿真。
```
sudo apt-get install ros-noetic-joy ros-noetic-teleop-twist-joy \
ros-noetic-teleop-twist-keyboard ros-noetic-laser-proc \
ros-noetic-rgbd-launch ros-noetic-rosserial-arduino \
ros-noetic-rosserial-python ros-noetic-rosserial-client \
ros-noetic-rosserial-msgs ros-noetic-amcl ros-noetic-map-server \
ros-noetic-move-base ros-noetic-urdf ros-noetic-xacro \
ros-noetic-compressed-image-transport ros-noetic-rqt* ros-noetic-rviz \
ros-noetic-gmapping ros-noetic-navigation ros-noetic-interactive-markers
```
```
sudo apt install ros-noetic-dynamixel-sdk
sudo apt install ros-noetic-turtlebot3-msgs
sudo apt install ros-noetic-turtlebot3
```
```
cd ~/PNGNav/src
git clone -b noetic-devel https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
cd ~/PNGNav && catkin_make
```
### 说明
0. 在 `~/.bashrc` 中添加以下行。
```
export TURTLEBOT3_MODEL=waffle_pi
```
1. 启动 Gazebo 仿真。
```
cd ~/PNGNav
conda deactivate
source devel/setup.bash
roslaunch turtlebot3_gazebo turtlebot3_world.launch
```
2. 为 Turtlebot3 启动 `map_server` 和 `amcl`。注意,这是我们 `png_navigation` 包中的启动文件,其中不包括 `move_base` 和 `rviz` 的启动。
```
cd ~/PNGNav
conda deactivate
source devel/setup.bash
roslaunch png_navigation turtlebot3_navigation.launch
```
3. 启动 rviz。
```
cd ~/PNGNav
conda deactivate
source devel/setup.bash
roslaunch png_navigation rviz_navigation_static.launch
```
4. 通过遥控测量估计 Turtlebot3 的姿态。在姿态估计后,请记得终止 `turtlebot3_teleop_key.launch`。
```
conda deactivate
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
```
5. 启动规划算法。通过在 rviz 中选择导航目标来开始规划。
```
cd ~/PNGNav
conda deactivate
source devel/setup.bash
roslaunch png_navigation nirrt_star_c.launch
```
或者任何这些行
```
roslaunch png_navigation nirrt_star.launch
roslaunch png_navigation nrrt_star.launch
roslaunch png_navigation irrt_star.launch
roslaunch png_navigation rrt_star.launch
```
## 如何运行动态障碍物实现
以下是在仿真中运行包含动态障碍物的实现的说明。我们将术语动态障碍物和移动人类互换使用。
0. 在 `~/.bashrc` 中添加以下行。
```
export TURTLEBOT3_MODEL=waffle_pi
```
1. 启动 Gazebo 仿真。
```
cd ~/PNGNav
conda deactivate
source devel/setup.bash
roslaunch turtlebot3_gazebo turtlebot3_world.launch
```
2. 为 Turtlebot3 启动 `map_server` 和 `amcl`。注意,这是我们 `png_navigation` 包中的启动文件,其中不包括 `move_base` 和 `rviz` 的启动。
```
cd ~/PNGNav
conda deactivate
source devel/setup.bash
roslaunch png_navigation turtlebot3_navigation.launch
```
3. 启动 rviz。
```
cd ~/PNGNav
conda deactivate
source devel/setup.bash
roslaunch png_navigation rviz_navigation_static.launch
```
4. 通过遥控测量估计 Turtlebot3 的姿态。在姿态估计后,请记得终止 `turtlebot3_teleop_key.launch`。
```
conda deactivate
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
```
5. 创建移动人类。
```
cd ~/PNGNav
conda deactivate
source devel/setup.bash
rosrun png_navigation moving_humans_with_noisy_measurements.py
```
将 `/dr_spaam_detections/PoseArray` 和 `/gt_human_positions/PoseArray` 添加到 rviz 中。
6. 启动人类检测器。
```
cd ~/PNGNav
conda deactivate
source devel/setup.bash
rosrun png_navigation human_checker_gazebo.py
```
7. 开始运行具有动态障碍物感知的规划算法。
```
cd ~/PNGNav
conda deactivate
source devel/setup.bash
roslaunch png_navigation nrrt_star_dynamic_obstacles.launch
```
或者
```
cd ~/PNGNav
conda deactivate
source devel/setup.bash
roslaunch png_navigation nirrt_star_c_dynamic_obstacles.launch
```
注意:
- 通过更改 `PNGNav/src/png_navigation/scripts_dynamic_obstacles/moving_humans_with_noisy_measurements.py` 中的 `vx, vy` 来更改移动人类的速度。
- 通过更改 `PNGNav/src/png_navigation/scripts_dynamic_obstacles/human_checker_gazebo.py` 中的 `human_detection_radius` 来更改机器人的人类检测半径。
## 如何创建您自己的地图 YAML 文件
1. 在完成 SLAM 并将地图保存为 `.pgm` 后,您还将获得一个 yaml 文件。编辑类似于以下内容的文件。
```
image: /home/png/map_gazebo.pgm
resolution: 0.010000
origin: [-10.000000, -10.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
setup: 'world'
free_range: [-2, -2, 2, 2]
circle_obstacles: [[1.1, 1.1, 0.15],
[1.1, 0, 0.15],
[1.1, -1.1, 0.15],
[0, 1.1, 0.15],
[0, 0, 0.15],
[0, -1.1, 0.15],
[-1.1, 1.1, 0.15],
[-1.1, 0, 0.15],
[-1.1, -1.1, 0.15]]
rectangle_obstacles: []
```
字段的格式如下所示。
```
free_range_pixel: [xmin, ymin, xmax, ymax]
circle_obstacles: [[x_center_1, y_center_1, r_1],
[x_center_2, y_center_2, r_2],
[x_center_3, y_center_3, r_3],
...,
[x_center_n, y_center_n, r_n]]
rectangle_obstacles: [[xmin_1, ymin_1, xmax_1, ymax_1],
[xmin_2, ymin_2, xmax_2, ymax_2],
[xmin_3, ymin_3, xmax_3, ymax_3],
...,
[xmin_n, ymin_n, xmax_n, ymax_n]]
```
2. 将 `.pgm` 文件和编辑后的 `.yaml` 文件移动到 `PNGNav/src/png_navigation/src/png_navigation/maps` 中。保持它们的名称相同,例如 `abc.pgm` 和 `abc.yaml`。在运行启动文件时,运行
```
roslaunch png_navigation nirrt_star_c.launch map:=abc
```
3. 您可以保持其他字段不变,并将它们保留在那里。这里是这些字段的参考。如果您将要从像素地图转换以获取几何配置,则需要这些信息,并且您需要 `PNGNav/src/png_navigation/src/png_navigation/maps/map_utils.py`。
```
Required fields:
image : 包含占用数据的图像文件的路径;可以是绝对路径,也可以是相对于 YAML 文件位置的路径
resolution : 地图的分辨率,米/像素
origin : 地图中左下角像素的二维姿态,为 (x, y, yaw),其中 yaw 为逆时针旋转的角度(yaw=0 表示无旋转)。目前系统的许多部分忽略 yaw。
occupied_thresh : 具有大于此阈值的占用概率的像素被认为是完全占用的。
free_thresh : 具有小于此阈值的占用概率的像素被认为是完全空闲的。
negate : 是否应该反转白色/黑色空闲/占用语义(阈值的解释不受影响)
```
## 如何转移到您的移动机器人
1. 修改 `PNGNav/src/png_navigation/src/png_navigation/configs/rrt_star
_config.py` 中的 `robot_config`。目前,只有 `robot_config.clearance_radius` 对全局规划有影响。
2. 修改 `src/png_navigation/scripts/local_planner_node.py`。特别是,请确保 `class LocalPlanner` 中的 `self.cmd_vel`、`self.odom_frame` 和 `self.base_frame` 与您的机器人设置相匹配。根据需要调整参数。