初心者がAIチャレンジやってみた（9）：障害物回避モジュールを自作する①

概要

「初心者がAI Challengeやってみた」シリーズの第9弾です。
第8弾ではautowareにシンプルな自作パッケージを導入してみました。
今回は現在地と目的地を取得してポテンシャル場を計算し経路を探索するプログラムを作ってみます。

本シリーズではJapan Automotive AI Challenge 2023にautoware初心者の筆者が試行錯誤しながら挑戦する記録を公開しています。自動運転に興味があるけどプログラミングに自信が無い方などの参考になれば幸いです。

前の記事はこちら：

• 第1弾：初心者がAIチャレンジやってみた(1)：Autowareを動かしてみる
• 第2弾：初心者がAIチャレンジやってみた(2)：1つ目の障害物の回避成功
• 第3弾：初心者がAIチャレンジやってみた(3)：2つ目の障害物の回避成功
• 第4弾：初心者がAIチャレンジやってみた(4)：全障害物回避達成！（全パラメータ公開）
• 第5弾：初心者がAIチャレンジやってみた(5)：開発環境の再構築（番外編）
• 第6弾：初心者がAIチャレンジやってみた(6)：Autoware-Miniを使ってみる
• 第7弾：初心者がAIチャレンジやってみた(7)：外部モジュール導入（失敗）
• 第8弾：初心者がAIチャレンジやってみた(8)：自作パッケージの導入

筆者はautoware初心者です。
説明等が正確でない可能性があるので本記事だけではなく他の記事やautowareのドキュメントも確認するようにしてください。

目標

最終的な目標は障害物回避軌道を出力するローカルプランナーを作ることです。
これに向けて課題を以下のように細分化して、1ステップずつ進めていく予定です。

• 目的地の設定
• 走行可能エリアの設定
• 障害物の位置の設定
• その他交通ルールの設定

（暫定的なものなので実装を進めるにつれて変わるかもしれません。）

開始地点と目標地点から経路を計算

まずはとてもシンプルに、車両の開始地点と最終目的地を取得して、そこまで辿り着くための経路の計算を目指します。

開始地点の取得

Autoware Miniのアーキテクチャを見てみる。
→ pose_initializerから/initialpose3dというトピックが出てきており、それによって車両の初期位置が設定されている
→ これにサブスクライブして車両の開始地点として使用する

目的地の取得

AutowareのPlanningのアーキテクチャを見てみる。

• Mission Planning：目的地やマップ情報からルート生成を行う（カーナビのようなもの？）
• Scenario Planning：ルートをもとに障害物の存在等を加味した軌道を生成する
  • Behavior Path Planning：障害物等と交通ルールを加味したパスを生成する（横断歩道に歩行者がいるので横断歩道手前で停止する的な？）
  • Motion Planning：車両の特性等を加味したパスの最適化を行う（大型トラックと軽自動車では加速度等が違うのでブレーキをかけるタイミングが変わる的な？）

これらを見ると、今回の目標はMission Planningの内容に似てそう。
調べたら目的地のトピックがいっぱいあった（/planning/mission_planning/goal, /planning/scenario_planning/modified_goal, /planning/mission_planning/echo_back_goal_poseなど）
→いまいち違いが分からなかったのでとりあえず/planning/mission_planning/goalを使ってみる

実装

第8弾はC++で書きましたが、なんだかプログラミングが難しそうだったのでPythonで書くことにしました。コードが汚いですがご容赦ください。
Python Robotics のコードをかなり参考にしました。また、PythonでのROS実装はこちらの Qiita記事を参考にしました。
（Pythonパッケージの作り方はROSチュートリアルをご参照ください。）

コードでやっていることは以下のようなことです：

• マップをグリッドのように分割する
• グリッドの各セルで、その地点から目的地までの距離を計算（これをポテンシャルとしてセルに保存）
• 現在地の周囲のセルから一番ポテンシャルが低いものを選択して移動する。これを目的地につくまで繰り替えす

import numpy as np
import matplotlib.pyplot as plt
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped


class PotentialFieldPlanner(Node):
    def __init__(self) -> None:
        super().__init__("potential_field_planner")
        self.is_start_received = False
        self.is_goal_received = False

        self.start_sub = self.create_subscription(
            PoseWithCovarianceStamped,
            "/initialpose3d",
            self.start_callback,
            10,
        )

        self.goal_sub = self.create_subscription(
            PoseStamped,
            "/planning/mission_planning/goal",
            self.goal_callback,
            10,
        )

    def start_callback(self, msg: PoseWithCovarianceStamped):
        self.is_start_received = True
        self.start_x = msg.pose.pose.position.x
        self.start_y = msg.pose.pose.position.y
        self.get_logger().info(f"\n\n\nStart position: x: {self.start_x:.2f}, y: {self.start_y:.2f}\n\n\n")
    
    def goal_callback(self, msg: PoseStamped):
        self.is_goal_received = True
        self.goal_x = msg.pose.position.x
        self.goal_y = msg.pose.position.y
        self.get_logger().info(f"\n\n\nGoal position: x: {self.goal_x:.2f}, y: {self.goal_y:.2f}\n\n\n")

        if self.is_start_received:
            self.compute_potential_field(1.0)
            self.get_logger().info(f"\n\n\n{self.potential_map.shape}\n\n\n")
    
    def compute_potential_field(self, resolution):
        self.resolution = resolution
        self.min_x = min(self.start_x, self.goal_x)
        self.min_y = min(self.start_y, self.goal_y)
        max_x = max(self.start_x, self.goal_x)
        max_y = max(self.start_y, self.goal_y)
        step_x = int(round((max_x - self.min_x) / resolution))
        step_y = int(round((max_y - self.min_y) / resolution))

        # compute potential at each cell of grid map
        self.potential_map = np.zeros(shape=(step_x, step_y))

        for idx_x in range(step_x):
            x = self.min_x + idx_x * resolution

            for idx_y in range(step_y):
                y = self.min_y + idx_y * resolution
                u_attract = self.compute_attractive_potential(x, y, self.goal_x, self.goal_y)
                u_repulse = 0.0
                u_total = u_attract + u_repulse
                self.potential_map[idx_x][idx_y] = u_total

        # compute route by looking for cells with smallest potential
        init_x = self.start_x
        init_y = self.start_y

        rx, ry = [init_x], [init_y]
        ix = (init_x - self.min_x) / self.resolution
        iy = (init_y - self.min_y) / self.resolution
        dist_to_goal = np.hypot(init_x - self.goal_x, init_y - self.goal_y)
        motion = self.get_motion_model()

        while dist_to_goal >= self.resolution:
            minp = float("inf")
            minix, miniy = -1, -1
            for i, _ in enumerate(motion):
                inx = int(ix + motion[i][0])
                iny = int(iy + motion[i][1])
                if inx >= self.potential_map.shape[0] or iny >= self.potential_map.shape[1] or inx < 0 or iny < 0:
                    p = float("inf")  # outside area
                    print("outside potential!")
                else:
                    p = self.potential_map[inx][iny]
                if minp > p:
                    minp = p
                    minix = inx
                    miniy = iny
            ix = minix
            iy = miniy
            xp = ix * self.resolution + self.min_x
            yp = iy * self.resolution + self.min_y
            dist_to_goal = np.hypot(self.goal_x - xp, self.goal_y - yp)
            #self.get_logger().info(f"Map: {self.potential_map}")
            self.get_logger().info(f"Steps: {step_x}, {step_y}")
            self.get_logger().info(f"Position: {xp:.2f}, {yp:.2f}")
            self.get_logger().info(f"Start: {self.start_x:.2f}, {self.start_y:.2f}")
            self.get_logger().info(f"Goal: {self.goal_x:.2f}, {self.goal_y:.2f}")
            self.get_logger().info(f"Distance to goal:{dist_to_goal:.2f}\n\n\n")
            rx.append(xp)
            ry.append(yp)

            plt.plot(xp, yp, ".r")

        plt.axis('equal')
        plt.show()

        return rx, ry
    
    def compute_attractive_potential(self, x, y, gx, gy):
        return np.hypot(x - gx, y - gy)

    def get_motion_model(self):
        # dx, dy
        motion = [[1, 0],
                [0, 1],
                [-1, 0],
                [0, -1],
                [-1, -1],
                [-1, 1],
                [1, -1],
                [1, 1]]

        return motion

def main(args=None):
    rclpy.init(args=args)
    potential_field_planner = PotentialFieldPlanner()

    rclpy.spin(potential_field_planner)

    potential_field_planner.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

ビルド時のエラー

上記のPythonコードをビルドをする時に以下のようなDepreciationWarningが出て失敗してしまいました。

調べたらsetuptoolsのバージョンが58.2.0以降のものだとROS2でビルド時にエラーが出てしまうらしいです。これを参考にダウングレードしてビルドしたら成功しました。

# Rockerコンテナ内で
pip install setuptools==58.2.0

cd /aichallenge
bash build.sh

結果

生成された経路を可視化してみました。

これを実際のマップに重ねてみるとこんな感じです。
開始地点と目的地しか考慮していないため、障害物やコースを完全に無視した形になっています。
もちろんこれでは使えませんが、経路生成っぽいことはできました。

走行可能エリアも加味してみる

さて、上記の実装ではコースなどを完全に無視しているので実際の大会で使えば即失格となってしまいます。
そこで、どうすれば走行可能エリアも加味できるのかを考えました。
パッと思いついたのは以下の2種類のアプローチです：

• コースの両端（レーン）を障害物としてポテンシャルを計算
• BehaviorPathPlannerによって生成される経路のポイントを仮の目的地として設定、逐一目的地を更新する

理想的には前者の方法が良いと思います。実際にPathのmsg型にはleft_boundとright_boundというものがあり、どうやらこれらがコースの左端と右端を表しているようです（AWSIMのLaneletVisualizerというものに似たようなものが載っています）。
ただし中身を確認すると左右のレーン合わせて31個のポイントしか登録されておらず、そのような粒度ではレーンを障害物として表現するのは難しいと思い後者の方法を取ることとしました。

後者の方法のアイデアとしては、BehaviorPathPlannerがすでに交通ルールに沿った経路を生成しているのだから、それにできるだけ従うような形で進めばレーンを外れることはないだろうという、楽観的な考えです。
生成されるPathの長さや粒度はパラメータ調整の際に散々いじったbehavior_path_planner.param.yamlファイルで設定できます（第3弾や第4弾を参照）。例えばPathの長さを10mに設定し、その10m先の地点を仮の目的地として設定して軌道を計算するみたいなことをイメージしています。

また、マップをグリッド状に分割する方法では計算される経路が微妙です。
実際に上記の結果では開始地点と目的地を直線で結ぶ経路がベストにも関わらずそのような経路は出力されませんでした。
ということでこちらの資料を参考にしてポテンシャル場の勾配を計算して車両の進むべき方向を計算するというものを作ってみます。

車両の現在地の取得

AutowareのLocalizationアーキテクチャを見てみる。
→ /localization/pose_with_covarianceで車両の現在地が取得できそうなのでこれを使う

目的地（仮）の取得

前述のようにBehaviorPathPlannerからのPathが使えそう
→/planning/scenario_planning/lane_driving/behavior_planning/pathにサブスクライブして、pointsの最後を目的地として使う

実装

コードがまだ途中で進むべき方向の計算までしかできてませんが、共有します。

import numpy as np
import matplotlib.pyplot as plt
import rclpy
from rclpy.node import Node
from autoware_auto_planning_msgs.msg import Path
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped


class PotentialFieldPlanner(Node):
    def __init__(self) -> None:
        super().__init__("potential_field_planner")
        self.potential_calculated = False

        self.position_sub = self.create_subscription(
            PoseWithCovarianceStamped,
            "/localization/pose_with_covariance",
            self.position_callback,
            10,
        )

        self.path_sub = self.create_subscription(
            Path,
            "/planning/scenario_planning/lane_driving/behavior_planning/path",
            self.path_callback,
            10,
        )

    def position_callback(self, msg: PoseWithCovarianceStamped):
        self.current_x = msg.pose.pose.position.x
        self.current_y = msg.pose.pose.position.y
    
    def path_callback(self, msg: Path):
        tmp_goals = msg.points
        last_goal = tmp_goals[-1].pose.position
        self.goal_x = last_goal.x
        self.goal_y = last_goal.y

        ox, oy = [], []

        direction = self.compute_grad(self.current_x, self. current_y, self.goal_x, self.goal_y, ox, oy)
        
        self.get_logger().info(f"\n\nCurrent position: x: {self.current_x:.2f}, y: {self.current_y:.2f}\n\n\n")
        self.get_logger().info(f"\n\nGoal position: x: {self.goal_x:.2f}, y: {self.goal_y:.2f}\n\n\n")
        self.get_logger().info(f"\n\nDirection: x: {direction[0]:.2f}, y: {direction[1]:.2f}\n\n\n")

    def compute_grad(self,  x, y, gx, gy, ox, oy):
        dx, dy = 0.01, 0.01
        curr_pot = self.compute_potential(x, y, gx, gy, ox, oy)
        dp_dx = (self.compute_potential(x + dx, y, gx, gy, ox, oy) - curr_pot) / dx
        dp_dy = (self.compute_potential(x, y + dy, gx, gy, ox, oy) - curr_pot) / dy

        grad = np.array([dp_dx, dp_dy])
        norm_grad = grad / np.linalg.norm(grad)
        return norm_grad

    def compute_potential(self, x, y, gx, gy, ox, oy):
        w_attract, w_repulse = 1.0, 2.0
        attract = self.compute_attractive_potential(x, y, gx, gy)
        repulse = 0.0 #self.compute_repulsive_potential(x, y, ox, oy)
        return w_attract * attract + w_repulse * repulse
    
    def compute_attractive_potential(self, x, y, gx, gy):
        return 1.0 / np.hypot(x - gx, y - gy)
    
    def compute_repulsive_potential(self, x, y, ox, oy):
        return -1.0 / np.hypot(x - ox, y - oy)

def main(args=None):
    rclpy.init(args=args)
    potential_field_planner = PotentialFieldPlanner()

    rclpy.spin(potential_field_planner)

    potential_field_planner.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

結果

ひとまず以下のように車両が進むべき方向が計算できました。

今後の予定

今回は目的地の設定ができて、走行可能エリアの設定もなんとなくできた（？）と思います。
次回は障害物のポテンシャルも計算できるようじっそうを進めます！

• 目的地の設定
• 走行可能エリアの設定
• 障害物の位置の設定
• その他交通ルールの設定