DJI TelloでOMPLを用いた自律飛行ドローンのナビゲーション - 6.1. Turtlebot3 NavigationをGazebo Simulationで動かしてみた

DJI Tello等ドローンの自律飛行を実現するには、3D環境でナビゲーション経路を計画としてOMPL（Open Motion Planning Library）を試してみます。


# 目次
1. [OMPLのインストール](https://qiita.com/k-koh/items/4f7e441d1225e6580f82)
2. [OMPLの基礎](https://qiita.com/k-koh/items/9933cd29599c859cac45)
3. [OMPL.appの入門](https://qiita.com/k-koh/items/fe1f05313045ed729c8b)
4. [OMPLの使用](https://qiita.com/k-koh/items/3a8b372537aa29e17863)
5. DJI Telloの使用
    1. [DJI Telloのフロントカメラのcamera_calibration](https://qiita.com/k-koh/items/094631646931ea5bc2bb)
    2. [Visual SLAM ORB_SLAM2 用のカメラキャリブレーションYamlファイルの作成](https://qiita.com/k-koh/items/b524fc175198e701bdff)
    3. [DJI Telloのカメラ でVisual-SLAMのORB-SLAM2 を動かしてみた](https://qiita.com/k-koh/items/98b5f079f784b22dcd01)
6. navigationスタックの使い方
    1. [Turtlebot3 NavigationをGazebo Simulationで動かしてみた](https://qiita.com/k-koh/items/1a4fbc175ca37be6206b)　← いまココ
    2. [Turtlebot3 Navigationでmap_fileの代わりにSLAMを使用](https://qiita.com/k-koh/items/1ec9097e4afedd0b43ca)
    3. [ARdroneのシミュレーターをGazeboで動かす](https://qiita.com/k-koh/items/feccbaee7c11c43c3ab4)
    4. [AirSimのシミュレーターを動かす](https://qiita.com/k-koh/items/52122fb5e96336ac8d33)
    5. [octomapのインストール](https://qiita.com/k-koh/items/c9a3a87e043934456842)
    6. [単眼カメラでORB_SLAM2から3D point clouodの生成](https://qiita.com/k-koh/items/1e069c9dda43b85e2fa0)
    7. [point_cloud2から3D octomapの生成](https://qiita.com/k-koh/items/01e34f353487eb8a6e26)
7. OMPLで3D 経路計画
    1. [状態空間環境のセットアップ](https://qiita.com/k-koh/items/e19ac08bb0a16975081e)

## 動作環境
- Ubuntu 18.04
- ROS Melodic
- Tello EDU

# インストール
## 依存パッケージをインストール
```console
sudo apt-get install ros-melodic-joy ros-melodic-teleop-twist-joy ros-melodic-teleop-twist-keyboard ros-melodic-laser-proc ros-melodic-rgbd-launch ros-melodic-depthimage-to-laserscan ros-melodic-rosserial-arduino ros-melodic-rosserial-python ros-melodic-rosserial-server ros-melodic-rosserial-client ros-melodic-rosserial-msgs ros-melodic-amcl ros-melodic-map-server ros-melodic-move-base ros-melodic-urdf ros-melodic-xacro ros-melodic-compressed-image-transport ros-melodic-rqt-image-view ros-melodic-navigation
```

## Turtlebot3のインストール
```console
$ cd ~/catkin_ws/src
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3.git
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
$ sudo apt-get install ros-melodic-turtlebot3-gazebo
$ cd ~/catkin_ws && catkin_make
```

## SlamとNavigationのインストール
```console
$ cd ~/catkin_ws/src
$ git clone https://github.com/ros-perception/slam_gmapping.git
$ git clone https://github.com/ros-perception/openslam_gmapping.git
$ git clone https://github.com/ros-planning/navigation_msgs.git
$ git clone https://github.com/ros-planning/navigation.git
$ git clone https://github.com/ros/geometry2.git
$ cd ~/catkin_ws && catkin_make
```

環境変数`TURTLEBOT3_MODEL`を~/.bashrcに追加
`burger` `waffle` `waffle_pi`の何れかを指定

```console
export TURTLEBOT3_MODEL=burger
$ source ~/.bashrc
```

## RVizでTurtleBot3の確認
```console
$ roslaunch turtlebot3_fake turtlebot3_fake.launch
```

```console
$ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

Control Your TurtleBot3!
---------------------------
Moving around:
        w
   a    s    d
        x

w/x : increase/decrease linear velocity (Burger : ~ 0.22, Waffle and Waffle Pi : ~ 0.26)
a/d : increase/decrease angular velocity (Burger : ~ 2.84, Waffle and Waffle Pi : ~ 1.82)

space key, s : force stop

CTRL-C to quit
```
![turtlebot3_fake.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/0b4a1a72-4683-23fd-5c0e-8d80a5cbbeac.gif)

# TurtleBot3 worldで起動
```console
$ roslaunch turtlebot3_gazebo turtlebot3_world.launch
```

- TurtleBot3を自律移動

```console
$ roslaunch turtlebot3_gazebo turtlebot3_simulation.launch
```

- Rvizで表示

```console
roslaunch turtlebot3_gazebo turtlebot3_gazebo_rviz.launch
```

![turtlebot3_simulation-2019-12-29_16.14.13.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/e98d669c-590e-ddc7-9b37-e4ab935502fc.gif)

![turtlebot3_simulation-2019-12-29_16.15.22.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/5e56b70c-8aca-fbf3-2a4d-aada34a32f5e.gif)

# Turtlebot3でSLAM Mapを生成
```console
$ roslaunch turtlebot3_gazebo turtlebot3_world.launch
$ roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:=gmapping
$ roslaunch turtlebot3_gazebo turtlebot3_simulation.launch

自律で環境探索が終わったときにMapを保存
$ rosrun map_server map_saver -f ~/map
```

map.yaml

```yaml
image: /home/k-koh/map.pgm
resolution: 0.050000
origin: [-10.000000, -10.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
```

map.pgm
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/6007c90a-cbff-7f46-bcc1-81bf8bbdeed8.png)

# NavigationでTurtlebot3でを動かす
```console
$ roslaunch turtlebot3_gazebo turtlebot3_world.launch
$ roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:='/home/k-koh/map.yaml'
```

![turtlebot3_navigation-2019-12-29_20.56.46.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/ce3b8c64-4dde-69be-29ee-a818976d9a87.gif)


Next: 6.2. [Turtlebot3 Navigationでmap_fileの代わりにSLAMを使用](https://qiita.com/k-koh/items/1ec9097e4afedd0b43ca)

Prev: 5.3. [DJI Telloのカメラ でVisual-SLAMのORB-SLAM2 を動かしてみた](https://qiita.com/k-koh/items/98b5f079f784b22dcd01)