DJI TelloでOMPLを用いた自律飛行ドローンのナビゲーション - 3. OMPL.appの入門

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

# OMPL.appの入門

# OMPLとは
OMPL (Open Motion Planning Library)は、サンプリングベース経路計画のアルゴリズムのパッケージです。衝突検知と可視化の機能が含まれていないので、簡単に既存システムに導入できます。ROSとMoveIt!等のシステムに既に導入されています。

# OMPL.appとは
OMPL.appは、OMPLライブラリのグラフィカルフロントエンドを提供します。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/9ea3e3bd-6a8e-1581-6748-32b37c56ec49.png)


# OMPL.appのインストール
- PyQtのインストール(for GUI)

```
pip3 install PyQt5
```

- PyOpenGLのインストール (for GUI)

```
pip3 install PyOpenGL PyOpenGL_accelerate
```

- Py++のインストール (needed to generate Python bindings)

```
sudo apt-get install castxml
pip3 install -vU pygccxml pyplusplus
```

- `OMPL installation script`をダウロード

[install-ompl-ubuntu.sh](http://ompl.kavrakilab.org/installation.html)

- install OMPL.app with Python bindings

```console
install-ompl-ubuntu.shを「ダウンロード」フォルダーから英語のみフォルダパスに移動
$ chmod u+x install-ompl-ubuntu.sh
$ ./install-ompl-ubuntu.sh --app
```

# OMPL.appの起動
いずれかのコマンドでompl_appを起動

```console
$ ompl_app
$ /usr/local/bin/ompl_app
$ python3 /usr/local/bin/ompl_app
```

# OMPL.appの使用手順
1. 環境とロボットのセットアップ
2. プランナーの選択
3. 環境のバウンド

## 環境とロボットのセットアップ

- Robot type : Rigid body planning (3D) を選択

- File→Open Environment→omplapp/resources/3D/Easy_env.dae を選択
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/520d51da-1227-c3e6-5008-edb9e0bf24bf.png)

- File→Open Robot→omplapp/resources/3D/Easy_robot.dae を選択
`L` shaped robot inside the opening of the wall
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/0aa7f20d-b4d8-e0b3-04fb-42bd224b73ba.png)

- ロボットのStart pose (270, 50, -200)を設定

- ロボットのGoal pose (270, 50, -400)を設定
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/8535c445-b2db-6154-b3cc-9b348b79d914.png)

- `Slove`をクリックしてデフォルト経路計画を作成
スタート位置からゴール位置までの経路を探索します。解決したパスをアニメションで表示します。
![ompl_app_easy_env_solve.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/9c01b03b-fec6-26c9-67d5-6fc575d07a53.gif)

- Showの「States」を選択してランダムサンプルStateを表示
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/9e3a20aa-9fba-7ac4-2955-f0f572459865.png)

## プランナーの選択

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/0f8fae7a-13c4-8ce9-7330-dec43367e47c.png)

## 環境のバウンディング
バウンディングボックスは環境周りの白いフレームです。ロボットがデフォルトでバウンディングボックス中に移動します。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/cd09ab64-2891-c5dd-67bb-d5f1d0e18157.png)

## 解決したパスの保存と再生

- File→Save Pathでパスを保存

（txtファイルで保存、環境とロボットのデータが保存されない）
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/b5a36b9d-f951-7581-7922-45a1f60e4af7.png)

- File→Open Pathで保存したパスを開く

Next: 4. [OMPLの使用](https://qiita.com/k-koh/items/3a8b372537aa29e17863)
prev: 2. [OMPLの基礎](https://qiita.com/k-koh/items/9933cd29599c859cac45)