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

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の使用

## デザイン考察
- OMPLは柔軟性と適用性を維持ため環境の幾何学とロボットを明確に表現していません。
- ユーザがロボットの表現、stateの有効性、衝突チェックを提供する必要があります。

## OMPL 基礎
- StateSampler クラス：
  一律とガウスサンプリングの方法を提供する。
- NearestNeighbors  クラス：
  state区間中のサンプの最近傍検索のインタフェースを提供する。
- StateValidityChecker  クラス：
  環境の障害物とロボットの制約の衝突構成を評価する。
- MotionValidator  クラス：
  ２つstate間のロボットの行動の有効性をチェックする。
- OptimizationObjective  クラス：
  コスト付き関連操作のインタフェースを提供する。
- ProblemDefinition  クラス：
  スタートStateとゴール構成を定義する。

## クエリを解決

OMPLのコンポーネント

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/1c11e849-827c-023e-434a-84add78e76b1.png)

### 幾何学計画
ユーザがロボットのstate区間を定義、スタートとゴール構成を提供する必要があります。

### 計画とコントロール
幾何学計画と同様にロボットのstate区間を定義、スタートとゴール構成を提供する必要があります。
その上、ユーザが`ControlSpace`オブジェクトで有効なコントロール入力を定義、`StatePropagator`インスタンスでコントロールを適用した場合システムStateがどのように変化するのかを計算する方法を提供する必要があります。

### サンプルセットアップ
幾何学とコントロールを解決する複数オブジェクトをカプセル化します。サンプルセットアップを使用には、state space, start state, goal stateの情報を設定します。

## OMPLを用いたソースのコンパイル

- base libraryを変更した場合

（例：ompl/geometric/plannersに新しいplannerを追加した場合）

```
cmakeを再度実行
```

- OMPLをリンクした場合

```
-lompl のコンパイルオプションでリンク
```

## ベンチマーク
OMPLがパスクエリの回数と複数plannerを試すベンチマーククラスを提供します。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/443adf6a-107b-7572-2673-e53070465aee.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/764d22ae-f1a6-aa23-bdec-0808bf726775.png)

## 高度な機能

### 状態空間構造(State Space)
共通状態空間（Rn for Euclideanspaces,  SO(2) and SO(3) for the space of rotations in 2D and 3D）を提供します。

### プランナーカスタマイズ
OMPLのプランナーをカスタマイズことができます。
例として、PRMとRRTプランナーも最近傍探索をユーザコードに置換できるフックを提供します。

### Python バインディング
C++言語以外も、ユーザがPython スクリプトでPython バインディングのAPIを呼び出して行動計画クエリを解決することができます。（Py++ と Boostが必要）

```
from ompl import base
from ompl import geometric
```

- An STL vector of int’s is of type vectorInt in Python (analogously for other types)
- The C++ class State has been renamed AbstractState, while the C++ class ScopedState<> iscalled State in Python.
- The C++ class ScopedState<RealVectorStateSpace> is called RealVectorState. The Scoped-State’s for the other pre-defined state spaces have been renamed analogously.
- The C++ class RealVectorStateSpace::StateType has been renamed to RealVectorStateInternal(analogously for the other state space types), to emphasize that a user should really be usingRealVectorState.
- Use the “()” operator on a ScopedState in Python to retrieve a reference to a C++ State.
- The print method (for classes that have one) is mapped to the special python method __str__,so a C++ call likefoo.print(std::cout)becomesprint fooin python. Similarly,a C++ call likefoo.printSettings(std::cout)becomesprint foo.settings()in python.
- The signature of the state validity checker is changed. In python the state validity checker hasthe following form:

```
def isStateValid(spaceInformation, state):
    returnspaceInformation.satiesfiesBounds(state)
```

Next: 5.1. [DJI Telloのフロントカメラのcamera_calibration](https://qiita.com/k-koh/items/094631646931ea5bc2bb)
prev: 3. [OMPL.appの入門](https://qiita.com/k-koh/items/fe1f05313045ed729c8b)