経路生成 (Informed RRT*) を実装してみた Part2

# はじめに
私がロボットに関して興味を持っており，ロボットの経路を作成したいと思ったところ，RRT (Rapidly-exploring Random Tree) と呼ばれる経路生成の手法に辿り着いた．
本記事では，経路生成の古典的手法である RRT (Rapidly-exploring Random Tree) を改良した Informed RRT* をPythonより，実装してみる．
前記事では，円形の干渉物が存在する2次元空間を探索する Informed RRT* のアニメーションを実装した．
前記事の内容を下記に記します．
・https://qiita.com/haruhiro1020/items/ad6d86c65adde3e2d1e6

前記事にて，Informed RRT* の処理時間が RRT* の処理時間よりも長くなり，想定外の結果となってしまった．
そのため本記事では，line_profiler を使用して，RRT* とInformed RRT* の処理時間のボトルネックを調査 + 改修する．
line_profiler のインストールおよび使用方法は，下記を参考にした．
・https://qiita.com/siruku6/items/046e2e7ae031309816b6

## 本記事で実装すること
・line_profiler による Informed RRT* の処理時間のボトルネックを調査 + 改修

## 本記事では実装できないこと(将来実装したい内容)
・球の干渉物が存在する3次元空間でのInformed RRT* による経路生成
・円形以外の干渉物が存在する2次元空間でのInformed RRT* による経路生成

## 動作環境
・macOS Sequoia (バージョン15.5)
・Python3 (3.10.9)
・Numpy (1.23.5) (数値計算用ライブラリ)
・Matplotlib (3.7.0) (アニメーション作成用ライブラリ)
・line_profiler (4.1.2) (関数内の各行の事項時間を計測するライブラリ)

## 経路生成手法のまとめ
経路生成手法をまとめた記事を展開いたします．本記事以外の手法も記載してますので，参照いただけますと幸いです．
(https://qiita.com/haruhiro1020/items/000b3287fce082ae4283)

## ソースコードに関して
投稿しているソースコードは綺麗でないため，随時更新しようと考えております．
また，処理時間を考慮した作りになっていないため，処理時間が高速となる作り方を勉強した後にソースコードを更新しようと考えております．

# 処理時間の計測 (line_profiler)
line_profiler を使用して，RRT* と Informed RRT* の各処理時間を計測する．
(参考にしたサイト　https://qiita.com/siruku6/items/046e2e7ae031309816b6)

## メイン処理に line_profiler の追加 (main.py)
メイン処理である main.py に処理時間を計測する line_profiler を追加する．
修正後のソースは下記の通りとなる．(<追加箇所> でタグ付けした箇所が修正した箇所となる)
main.pyで使用するRRTStarクラスなどは前回の記事(https://qiita.com/haruhiro1020/items/970a7f6a3388bd2e594f)に記載している．

```main.py
# ライブラリの読み込み
import numpy as np
import matplotlib.pyplot as plt
import time
from line_profiler import LineProfiler      # <追加箇所>

# 自作モジュールの読み込み
from rrt import RRT, RRTConnect, RRTStar, InformedRRTStar        # 経路生成アルゴリズム
from rrt_animation import RRTAnimation, RRTConnectAnimation, RRTStarAnimation, InformedRRTStarAnimation   # 経路生成のアニメーション
from interference import Circle, Ball   # 干渉物
from constant import *      # 定数


START_POS = 0                       # 始点位置
END_POS   = 2                       # 終点位置


def rrt_planning(dimention, plot_mode, start_pos, end_pos, interference_obj):
    """
    RRTによる経路生成
    
    パラメータ
        dimention(int): 次元数
        plot_mode(int): プロット状態
        start_pos(numpy.ndarray): 始点
        end_pos(numpy.ndarray): 終点
        interference_obj(list): 干渉物
    
    戻り値
        planning_result(bool): True / False = 経路生成に成功 / 失敗
        elapsed_time(float): 経路生成の処理時間 [sec]
        n_waypoint(int): 経由点数(始点と終点を含む)
    """
    # 戻り値の初期化
    planning_result = False
    elapsed_time = 0.0
    n_waypoint = 0

    # RRTクラスのインスタンスを作成
    rrt = RRT()
    # 始点から終点までの経路を生成
    start_time = time.time()
    planning_result = rrt.planning(start_pos, end_pos, interference_obj)
    if planning_result:
        end_time = time.time()
        # 処理時間を計算
        elapsed_time = end_time - start_time
        # 経由点数を計算
        n_waypoint = rrt.pathes.shape[0]
        # 生成した経路をファイルに保存
        rrt.save()

        # RRTアニメーションのインスタンスを作成
        rrtAni = RRTAnimation(rrt.strict_min_pos, rrt.strict_max_pos)
        # グラフ描画とアニメーション描画で同じ引数としたい
        if plot_mode == PLOT_GRAPH:         # グラフの描画
            result = rrtAni.plot(rrt.pathes, rrt.debug_pathes, interference_obj, dimention)
        elif plot_mode == PLOT_ANIMATION:   # アニメーションの映画
            result = rrtAni.plot_Animation(rrt.pathes, rrt.debug_pathes, rrt.debug_random_pathes, interference_obj, dimention)

    return planning_result, elapsed_time, n_waypoint


def rrt_connect_planning(dimention, plot_mode, start_pos, end_pos, interference_obj):
    """
    RRT-Connectによる経路生成
    
    パラメータ
        dimention(int): 次元数
        plot_mode(int): プロット状態
        start_pos(numpy.ndarray): 始点
        end_pos(numpy.ndarray): 終点
        interference_obj(list): 干渉物
    
    戻り値
        planning_result(bool): True / False = 経路生成に成功 / 失敗
        elapsed_time(float): 経路生成の処理時間 [sec]
        n_waypoint(int): 経由点数(始点と終点を含む)
    """
    # 戻り値の初期化
    planning_result = False
    elapsed_time = 0.0
    n_waypoint = 0

    # RRT-Connectクラスのインスタンスを作成
    rrt_connect = RRTConnect()
    # 始点から終点までの経路を生成
    start_time = time.time()
    planning_result = rrt_connect.planning(start_pos, end_pos, interference_obj)
    if planning_result:
        # 経路生成に成功
        end_time = time.time()
        # 処理時間を計算
        elapsed_time = end_time - start_time
        # 経由点数を計算
        n_waypoint = rrt_connect.pathes.shape[0]
        # 生成した経路をファイルに保存
        rrt_connect.save()

        # RRT-Connectアニメーションのインスタンスを作成
        rrt_connect_animation = RRTConnectAnimation(rrt_connect.strict_min_pos, rrt_connect.strict_max_pos)
        # グラフ描画とアニメーション描画で同じ引数としたい
        if plot_mode == PLOT_GRAPH:     # グラフの描画
            result = rrt_connect_animation.plot(rrt_connect.pathes, rrt_connect.start_tree, rrt_connect.end_tree, interference_obj, dimention)
        elif plot_mode == PLOT_ANIMATION:   # アニメーションの映画
            result = rrt_connect_animation.plot_Animation(rrt_connect.pathes, rrt_connect.start_tree, rrt_connect.end_tree, rrt_connect.debug_states, interference_obj, dimention)

    return planning_result, elapsed_time, n_waypoint


def rrt_star_planning(dimention, plot_mode, start_pos, end_pos, interference_obj):
    """
    RRT*による経路生成
    
    パラメータ
        dimention(int): 次元数
        plot_mode(int): プロット状態
        start_pos(numpy.ndarray): 始点
        end_pos(numpy.ndarray): 終点
        interference_obj(list): 干渉物
    
    戻り値
        planning_result(bool): True / False = 経路生成に成功 / 失敗
        elapsed_time(float): 経路生成の処理時間 [sec]
        n_waypoint(int): 経由点数(始点と終点を含む)
    """
    # 戻り値の初期化
    planning_result = False
    elapsed_time = 0.0
    n_waypoint = 0

    # RRT*クラスのインスタンスを作成
    rrt_star = RRTStar()
    rrt_star.max_iterate = 500
    # 始点から終点までの経路を生成
    start_time = time.time()
    
    # <追加箇所> line_profiler による処理時間の計測 ↓
    profile = LineProfiler()
    profile.add_function(rrt_star.planning)

    for i in range(1):
        np.random.seed(i)
        profile.runcall(rrt_star.planning, start_pos=start_pos, end_pos=end_pos, interference_obj=interference_obj)

    profile.print_stats()
    # <追加箇所> line_profiler による処理時間の計測 ↑

    # planning_result = rrt_star.planning(start_pos, end_pos, interference_obj)
    # if planning_result:
    #     # 経路生成に成功
    #     end_time = time.time()
    #     # 処理時間を計算
    #     elapsed_time = end_time - start_time
    #     # 経由点数を計算
    #     n_waypoint = rrt_star.pathes.shape[0]
    #     # 生成した経路をファイルに保存
    #     rrt_star.save()

    #     # RRT*アニメーションのインスタンスを作成
    #     rrtStarAni = RRTStarAnimation(rrt_star.strict_min_pos, rrt_star.strict_max_pos)
    #     # グラフ描画とアニメーション描画で同じ引数としたい
    #     if plot_mode == PLOT_GRAPH:         # グラフの描画
    #         result = rrtStarAni.plot(rrt_star.pathes, rrt_star.foler_name_current_tree, rrt_star.file_name_current_tree, rrt_star.count_success, "csv", interference_obj, dimention)
    #     elif plot_mode == PLOT_ANIMATION:   # アニメーションの描画
    #         result = rrtStarAni.plot_Animation(rrt_star.pathes, rrt_star.foler_name_current_tree, rrt_star.file_name_current_tree, rrt_star.count_success, "csv", interference_obj, dimention)

    return planning_result, elapsed_time, n_waypoint


def informed_rrt_star_planning(dimention, plot_mode, start_pos, end_pos, interference_obj):
    """
    Informed RRT*による経路生成
    
    パラメータ
        dimention(int): 次元数
        plot_mode(int): プロット状態
        start_pos(numpy.ndarray): 始点
        end_pos(numpy.ndarray): 終点
        interference_obj(list): 干渉物
    
    戻り値
        planning_result(bool): True / False = 経路生成に成功 / 失敗
        elapsed_time(float): 経路生成の処理時間 [sec]
        n_waypoint(int): 経由点数(始点と終点を含む)
    """
    # 戻り値の初期化
    planning_result = False
    elapsed_time = 0.0
    n_waypoint = 0

    # RRT*クラスのインスタンスを作成
    informed_rrt_star = InformedRRTStar()
    informed_rrt_star.max_iterate = 500
    # 始点から終点までの経路を生成
    start_time = time.time()
    
    # <追加箇所> line_profiler による処理時間の計測 ↓
    profile = LineProfiler()
    profile.add_function(informed_rrt_star.planning)

    for i in range(1):
        np.random.seed(i)
        profile.runcall(informed_rrt_star.planning, start_pos=start_pos, end_pos=end_pos, interference_obj=interference_obj)

    profile.print_stats()
    # <追加箇所> line_profiler による処理時間の計測 ↑

    # planning_result = informed_rrt_star.planning(start_pos, end_pos, interference_obj)
    # if planning_result:
    #     # 経路生成に成功
    #     end_time = time.time()
    #     # 処理時間を計算
    #     elapsed_time = end_time - start_time
    #     # 経由点数を計算
    #     n_waypoint = informed_rrt_star.pathes.shape[0]
    #     # 生成した経路をファイルに保存
    #     informed_rrt_star.save()

    #     # Informed RRT*アニメーションのインスタンスを作成
    #     informedRrtStarAni = InformedRRTStarAnimation(informed_rrt_star.strict_min_pos, informed_rrt_star.strict_max_pos)
    #     # グラフ描画とアニメーション描画で同じ引数としたい
    #     print(f"plot_mode = {plot_mode}")
    #     if plot_mode == PLOT_GRAPH:         # グラフの描画
    #         result = informedRrtStarAni.plot(informed_rrt_star.pathes, informed_rrt_star.foler_name_current_tree, informed_rrt_star.file_name_current_tree, informed_rrt_star.count_success, "csv", informed_rrt_star.ellipse_radius, interference_obj, dimention)
    #         print(f"result = {result}")
    #     elif plot_mode == PLOT_ANIMATION:   # アニメーションの描画
    #         result = informedRrtStarAni.plot_Animation(informed_rrt_star.pathes, informed_rrt_star.foler_name_current_tree, informed_rrt_star.file_name_current_tree, informed_rrt_star.count_success, "csv", informed_rrt_star.ellipse_radius, interference_obj, dimention)
    #         print(f"result = {result}")

    return planning_result, elapsed_time, n_waypoint


def path_planning(path_plan, dimention, plot_mode):
    """
    経路生成
        path_plan(int): 経路生成アルゴリズム番号
        dimention(int): 次元数
        plot_mode(int): プロット状態
    
    戻り値
        planning_result(bool): True / False = 経路生成に成功 / 失敗
        elapsed_time(float): 経路生成の処理時間 [sec]
        n_waypoint(int): 経由点数(始点と終点を含む)
    """
    # 戻り値の初期化
    planning_result = False
    elapsed_time = 0
    n_waypoint = 0

    if not (dimention == DIMENTION_2D or dimention == DIMENTION_3D):
        # 次元数が2 or 3以外は処理終了
        return planning_result, elapsed_time, n_waypoint

    # 始点と終点を作成
    start_pos = np.ones(dimention).reshape(1, -1) * START_POS
    end_pos   = np.ones(dimention).reshape(1, -1) * END_POS

    # 干渉物の半径を定義
    interference_radius = 0.3
    if dimention == DIMENTION_2D:
        # 円形の干渉物を作成
        interference_pos = [[0.8, 0.8], [1.2, 0.8], [1.2, 1.2], [0.8, 1.2]]
        interference_obj = [Circle(np.array(pos), interference_radius) for pos in interference_pos]
    elif dimention == DIMENTION_3D:
        # 球の干渉物を作成
        interference_pos = [[0.8, 0.8, 0.8], [1.2, 0.8, 0.8], [1.2, 1.2, 1.2], [0.8, 1.2, 1.2]]
        interference_obj = [Ball(np.array(pos), interference_radius) for pos in interference_pos]
    else:
        # 干渉物はなし
        interference_obj = []

    if path_plan == PATH_PLANNING_RRT:                      # RRTによる経路生成
        planning_result, elapsed_time, n_waypoint = rrt_planning(dimention, plot_mode, start_pos, end_pos, interference_obj)

    elif path_plan == PATH_PLANNING_RRT_CONNECT:            # RRT-Connectによる経路生成
        planning_result, elapsed_time, n_waypoint = rrt_connect_planning(dimention, plot_mode, start_pos, end_pos, interference_obj)

    elif path_plan == PATH_PLANNING_RRT_STAR:               # RRT*による経路生成
        planning_result, elapsed_time, n_waypoint = rrt_star_planning(dimention, plot_mode, start_pos, end_pos, interference_obj)
    
    elif path_plan == PATH_PLANNING_INFORMED_RRT_STAR:      # Informed RRT*による経路生成
        planning_result, elapsed_time, n_waypoint = informed_rrt_star_planning(dimention, plot_mode, start_pos, end_pos, interference_obj)

    print(f"planning_result = {planning_result}")
    return planning_result, elapsed_time, n_waypoint


def main():
    """
    メイン処理
    """
    # プロットできる状態
    # plot_modes = [PLOT_GRAPH, PLOT_ANIMATION, PLOT_NONE]
    plot_modes = [PLOT_ANIMATION, ]
    # 経路生成アルゴリズム
    # path_plans = [PATH_PLANNING_RRT, PATH_PLANNING_RRT_CONNECT, PATH_PLANNING_RRT_STAR, PATH_PLANNING_INFORMED_RRT_STAR]
    path_plans = [PATH_PLANNING_RRT_STAR, PATH_PLANNING_INFORMED_RRT_STAR]  # Informed RRT*だけを実施する場合
    # 次元数
    # dimentions = [DIMENTION_2D, DIMENTION_3D]
    dimentions = [DIMENTION_2D, ]

    # 全アルゴリズム，全次元，全プロット状態の全組み合わせを実施する
    for path_plan in path_plans:
        for dimention in dimentions:
            for plot_mode in plot_modes:
                if path_plan == PATH_PLANNING_RRT:
                    print("RRT")
                elif path_plan == PATH_PLANNING_RRT_CONNECT:
                    print("RRT-Connect")
                elif path_plan == PATH_PLANNING_RRT_STAR:
                    print("RRT*")
                elif path_plan == PATH_PLANNING_INFORMED_RRT_STAR:
                    print("Informed RRT*")

                if plot_mode == PLOT_GRAPH or plot_mode == PLOT_ANIMATION:
                    # グラフまたはアニメーションを1つ作成
                    path_planning(path_plan, dimention, plot_mode)
                else:
                    # 経路生成の成功割合や処理時間，経由点数の統計量を計算
                    planning_results = 0                # 経路生成の成功回数
                    elapsed_times = []                  # 処理時間
                    n_waypoints   = []                  # 経由点数

                    for i in range(PATH_PLANNING_NUM):
                        np.random.seed(i)               # RRT, RRT-Connect, RRT*で比較するためにシード値を設定する
                        result, elapsed_time, waypoint = path_planning(path_plan, dimention, plot_mode)
                        if result:
                            planning_results += 1
                            elapsed_times.append(elapsed_time)
                            n_waypoints.append(waypoint)

                    # 経路生成の成功割合
                    success_rate = planning_results / PATH_PLANNING_NUM
                    print(f"success_rate = {success_rate}")

                    if elapsed_times:
                        # 処理時間の平均，最大，最小，標準偏差を出力
                        print(f"time ave = {np.mean(elapsed_times)}")
                        print(f"time max = {np.max( elapsed_times)}")
                        print(f"time min = {np.min( elapsed_times)}")
                        print(f"time std = {np.std( elapsed_times)}")
                    if n_waypoints:
                        # 経由点の平均，最大，最小，標準偏差を出力
                        print(f"waypoint ave = {np.mean(n_waypoints)}")
                        print(f"waypoint max = {np.max( n_waypoints)}")
                        print(f"waypoint min = {np.min( n_waypoints)}")
                        print(f"waypoint std = {np.std( n_waypoints)}")

if __name__ == "__main__":
    # 本ファイルがメインで呼ばれた時の処理
    main()
```

## RRT* の処理時間を計測 (line_profiler 結果)
line_profiler による RRT* の処理時間を計測した．
計測結果は下図の通りである．単位は [usec] である．
下図の '% Time' 列が全処理時間に占める，実装行の処理時間の割合である．そのため， '% Time' が大きい行を処理時間のボトルネックということができる．

![RRT*_line_pro.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/ec28f421-9258-40b7-a643-645a7adfa591.png)

ボトルネックとなっている箇所を赤枠で囲んだ．
上図から，ボトルネックの箇所を下表にまとめた．

|No |処理の概要  |全体の処理時間に対する処理の割合 [%] |関数名 |
|---|---       |---|---|
|1  |近傍ノードからコストが最小となるノードを取得  |45.8 |_get_min_cost_node |
|2  |近傍ノード内でコストが小さくなれば，最短ノード(親ノード)を更新  |45.7 |_update_near_node |
|3  |現在のツリーをファイル化する  |6.6 |_save_current_tree |

上表のNo.3はアニメーションを作成する時に使用したい，デバッグ処理のためコメントアウトやデバッグ時だけ処理が通るように修正すれば良い．そのため，No.3の処理時間を削減することは可能である．

次に，No.1，No.2を再度 line_profiler により，処理時間を計測する．

### _get_min_cost_node の処理時間を計測 (line_profiler)
上記にて，_get_min_cost_node 関数が処理時間のボトルネックであることを説明した．
そのため，再度 line_profiler を使用して，_get_min_cost_node の処理時間を計測し，ボトルネックを調査する．
line_profiler の結果を下図に記す．ボトルネックの箇所を赤枠で記載した．

![get_near_node.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/277c6dc8-9fe2-4d83-91f2-dd96c496fa3e.png)

ボトルネックとなっている箇所は，2点間の干渉判定の処理であることが判明した．
後ほど，2点間の干渉判定の処理を修正する案を説明する．

### _update_near_node の処理時間を計測 (line_profiler)
上記にて，_get_min_cost_node 関数が処理時間のボトルネックであることを説明した．
そのため，再度 line_profiler を使用して，_get_min_cost_node の処理時間を計測し，ボトルネックを調査する．
line_profiler の結果を下図に記す．ボトルネックの箇所を赤枠で記載した．

![図1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/42139564-3357-4a2a-8ee3-31791dc67a5a.png)

ボトルネックとなっている箇所は，_get_min_cost_node 関数と同様で，2点間の干渉判定の処理であることが判明した．
2点間の干渉判定の処理を修正する必要がある．

## 2点間の干渉判定の処理を修正
処理時間のボトルネックを調査したところ，2点間の干渉判定の処理が要因であると判明した．
そのため，2点間の干渉判定の処理を修正する．

はじめに，現状の2点間の干渉判定の処理を説明する．
現状の処理を下図に記す．
点1と点2の距離を一定距離で分割し，点1に近い分割点から順番に干渉判定を実施している．

![干渉判定_従来版.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/5b3f093f-d41b-43d0-920c-bbd507c7a566.png)

次に，修正後の2点間の干渉判定の処理を説明する．
修正後の処理を下図に記す．
干渉物は円形と固定しているため，円と直線の最短距離を算出することで干渉判定を実施するように修正する．そうすることで，干渉判定の回数を減らすことができ，処理時間の短縮につながる．
また，従来処理では分割点を採用することで干渉判定の抜け漏れが発生するけど，修正後では干渉判定の抜けもれは発生しなくなる．(分割点の間では干渉判定を実施しないため，干渉判定に漏れがある)
円と直線の最短距離の算出方法は下記を参考とした．
・https://manabitimes.jp/math/857

![円と直線の公式.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/1071c6c3-e0ae-4933-9ff5-82480a2ad707.png)

干渉判定の修正後を使用した RRT* の処理時間を下図に記す．
下図の赤枠は干渉判定を修正した際の処理時間である．

![図1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/9a7eba8a-a920-407d-bc4b-9ac52f545f07.png)

上図の赤枠より，干渉判定を修正することで処理時間の短縮がわかる．

## Informed RRT* の処理時間を計測 (line_profiler 結果)
line_profiler による Informed RRT* の処理時間を計測した．
計測結果は下図の通りである．単位は [usec] である．
下図の '% Time' 列が全処理時間に占める，実装行の処理時間の割合である．そのため， '% Time' が大きい行を処理時間のボトルネックということができる．

![informed_RRT*_修正前.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/2150512f-8702-40b4-a0e1-9f1d5623cd91.png)

ボトルネックとなっている箇所を赤枠で囲んだ．
上図から，ボトルネックの箇所を下表にまとめた．

|No |処理の概要  |全体の処理時間に対する処理の割合 [%] |関数名 |
|---|---       |---|---|
|1  |近傍ノードからコストが最小となるノードを取得  |45.8 |_get_min_cost_node |
|2  |近傍ノード内でコストが小さくなれば，最短ノード(親ノード)を更新  |45.7 |_update_near_node |
|3  |現在のツリーをファイル化する  |6.6 |_save_current_tree |

上表のボトルネックは RRT* と同じである．そのため，対処方法も RRT* と同様となる．

干渉判定の処理を修正後の処理時間を下図に示す．

![修正後.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/65e61658-5f89-43ea-bb09-07b2f947c44d.png)

干渉判定の処理を修正することにって，処理時間が短縮することを確認できる．
今回は干渉物が円形しか定義していないため，本修正案で問題ない．
しかし，円形以外の干渉物も採用したい．そのために，干渉判定用ライブラリである python-fcl を使用できるようになりたい．

# おわりに
本記事では，Pythonを使用して，下記内容を実装しました．
・RRT* と Informed RRT* の処理時間のボトルネックの調査 + 改修
・干渉判定の処理を修正

次記事では，下記内容を実装していきます．
・干渉判定用ライブラリである python-fcl の使い方を説明