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

# 概要
「初心者がAI Challengeやってみた」シリーズの第8弾です。
第7弾では障害物回避用の外部モジュールの導入を試みましたが、autowareやROS2の知識不足で実装はできませんでした。
今回は横着せず、数回の投稿にわたって障害物回避モジュールの実装の記録をしていく予定です。

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

前の記事はこちら：
* 第1弾：[初心者がAIチャレンジやってみた(1)](https://qiita.com/h_bog/items/86fba5b94b2148c4d9da)：Autowareを動かしてみる
* 第2弾：[初心者がAIチャレンジやってみた(2)](https://qiita.com/h_bog/items/a36df3042a342bd2a0c5)：1つ目の障害物の回避成功
* 第3弾：[初心者がAIチャレンジやってみた(3)](https://qiita.com/h_bog/items/849a4241cdef257f3b3a)：2つ目の障害物の回避成功
* 第4弾：[初心者がAIチャレンジやってみた(4)](https://qiita.com/h_bog/items/8bce776fef0a39676c46)：全障害物回避達成！（全パラメータ公開）
* 第5弾：[初心者がAIチャレンジやってみた(5)](https://qiita.com/h_bog/items/4ed94ef6256ac5ae9c03)：開発環境の再構築（番外編）
* 第6弾：[初心者がAIチャレンジやってみた(6)](https://qiita.com/h_bog/items/075543f30f9b3432fa1d)：Autoware-Miniを使ってみる
* 第7弾：[初心者がAIチャレンジやってみた(7)](https://qiita.com/h_bog/items/bcfd5648afa8b6dee5b3)：外部モジュール導入（失敗）


:::note warn
筆者はautoware初心者です。
説明等が正確でない可能性があるので本記事だけではなく他の記事や[autowareのドキュメント](https://autowarefoundation.github.io/autoware-documentation/main/)も確認するようにしてください。
:::

# 目標
Autowareですでに実装されているObstacleAvoidancePlanner等の`MotionPlanner`を、自作のパッケージで置き換えて障害物回避機能を実装する。

# 必要な前知識
## 2つのPlannerの違い
障害物回避には回避経路を生成する必要があります。

Autowareは主に`BehaviorPathPlanner`と`MotionPlanner`の2つのモジュールが組み合わさって経路が生成されています。それぞれの違いは以下の通りです。
- `BehaviorPathPlanner`： 交通ルール（信号、一時停止線、走行可能エリア等）を加味した`Path`（＝経路？）を生成します。`Path`についての詳細は[こちら](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/components/planning/#from-behavior-planning-to-motion-planning)に書いてあります。
- `MissionPlanner`：交通ルールは考慮しないが、障害物の回避や速度の調整などをした`Trajectory`（＝軌道？）を生成します。詳細は[こちら](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/components/planning/#from-scenario-planning-to-validation)に書いてあります。

`Path`と`Trajectory`は持っている情報が違います。どちらも[autoware_auto_planning_msgs](https://github.com/tier4/autoware_auto_msgs/tree/tier4/main/autoware_auto_planning_msgs/msg)でmsg型が定義されているので、より詳細を知りたい方はそちらをご参照ください。

流れとしては
`BehaviorPathPlanner`が現在地から目的地までの、交通ルールを遵守した経路を生成する。
→生成された経路をもとに`MotionPlanner`が障害物回避軌道を生成する。
→`Controller`が軌道をもとに車両を制御する。
といった感じでしょうか。

交通ルール関連はいじるのが大変そうなので今回は`BehaviorPathPlanner`はノータッチで行きます。
`MotionPlanner`部分を新しく自作して障害物回避を目指します。

## Potential Field Algorithm
具体的な経路生成手法ですが、Potential Fieldアルゴリズムを使おうと考えています。理由は仕組みが直感的に理解しやすいからです。イメージは以下の通りです：
- 目的地は車両を「引き寄せる力」を発している
- 障害物は車両を「遠ざける力」を発している

これらの力を足し合わせると車両が障害物を避けながら目的地に向かっていくような動作を生成することができます。


Potential FieldアルゴリズムのPythonの実装がGitHub上に載っていました。今回はC++で実装をしたいのでこちらを参考にしつつ、Autowareの他のモジュールと一生に動くように色々と変えながら実装を進めていきます。

https://github.com/AtsushiSakai/PythonRobotics#potential-field-algorithm

# Autowareをカスタマイズ
## パッケージの作成（練習）
Autowareにパッケージを追加する方法ですが、[公式サイト](https://automotiveaichallenge.github.io/aichallenge2023-sim/customize/index.html#how-to-customize-and-use-autoware-packages)にちょっとだけ説明が載っています。

まずは難しいことはせずに、簡単なコードを作ってビルド・実行ができるかを試してみます。
ROSチュートリアルの[Writing a simple publisher and subscriber](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html)を見ながら、簡単なパブリッシャーとサブスクライバーを作ります。

まず、[公式サイト](https://automotiveaichallenge.github.io/aichallenge2023-sim/customize/index.html#how-to-customize-and-use-autoware-packages)にあるように`aichallenge_submit`のフォルダにパッケージを作成します。
```sh
$ cd ~/aichallenge2023-sim/docker/aichallenge/aichallenge_ws/src/aichallenge_submit  # aichallenge_submitに移動
$ ros2 pkg create --build-type ament_cmake cpp_pubsub  # cpp_pubsubというパッケージを作成
```
あとは[ROSチュートリアル](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html)に従ってコードをダウンロードして、`package.xml`と`CMakeLists.txt`を変更します。

こちらのコードではパブリッシャーが500ms毎にString型のトピックを配信し、サブスクライバーがそのトピックを受信したら"I heard XX"と画面にプリントするという非常にシンプルなコードです。

次に、Dockerを起動してコードをビルドします。
```sh
# Rockerコンテナ内で
cd /aichallenge
bash build.sh
```

以下のように`cpp_pubsub`が他のパッケージと一緒にビルドされれば成功です。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3483805/b8fb17d3-f9ea-a6cf-dd7c-8a395a3425ee.png)


今回はC++でのコードを使いましたが、Pythonでも同じ様にROSチュートリアルに従えばできると思います。
内容は違いますが、以下の記事に自作のPythonパッケージを動かす方法が記述されていました。Pythonで書きたい方は参考になるかもしれません。

https://qiita.com/yhisaki/items/01963edd696590c3e3d2

## パッケージの実行（練習）
実行方法は色々とありますが、今回はせっかくなので他のモジュールと同じようにlaunchファイルから実行してみます。Autoware-miniを使う場合、`autoware_mini_awsim.launch.xml`を少しだけ編集すればできます。

Launchファイルの作り方はROSチュートリアルの[Creating a launch file](https://docs.ros.org/en/humble/Tutorials/Intermediate/Launch/Creating-Launch-Files.html)を参照します。今回はトピックのremapやパラメータの設定は必要ないので非常に簡単な編集でできます。今回は`autoware_mini_awsim.launch.xml`の`lane_driving` group内に以下の"Simple Pub Sub"部分を追加しました。

```xml
<!-- lane_driving -->
<group>
  <push-ros-namespace namespace="lane_driving"/>
  
  <!-- 割愛 -->

  <!-- Simple Pub Sub -->
  <group>
    <node pkg="cpp_pubsub" exec="talker"/>
    <node pkg="cpp_pubsub" exec="listener"/>
  </group> <!-- Simple Pub Sub -->
</group> <!-- lane_driving -->
```

いよいよ実行です。
Dockerコンテナを起動し、以下のコマンドを実行します。
```sh
# Rockerコンテナ内で
cd /aichallenge
bash run.sh
```

Autowareの他のパッケージと同じ様に実行することができました！
少し分かりにくいですが、以下の画像で`talker-27`や`listener-28`の部分から、追加したパブリッシャーとサブスクライバーが正常に動作していることが確認できます。
![Screenshot from 2023-08-07 12-09-11.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3483805/bcadbb26-227d-efc8-7fe3-09ee1c22f040.png)


## autoware_auto_msgsを用いたパッケージ
ひとまずシンプルなパッケージの作成、ビルド、実行に成功しました。
次のステップは`autoware_auto_msgs`のメッセージを用いたパッケージを作ることです。

前述したように`Path`や`Trajectory`はAutoware独自のmsg型です。
そのため、今度は自作パッケージでこれらのmsg型が使えることを確認します。

結論から言うと[前述したパッケージの作成方法](https://qiita.com/h_bog/items/368543a4b10b356db0e5#%E3%83%91%E3%83%83%E3%82%B1%E3%83%BC%E3%82%B8%E3%81%AE%E4%BD%9C%E6%88%90%E7%B7%B4%E7%BF%92)だと`autoware_auto_msgs`を使う際にエラーが発生してしまいました。おそらく`package.xml`や`CMakeLists.txt`関連のエラーだと思うのですが原因が分からず、既存のautowareパッケージをコピペしてパッケージ名、ファイル名、中身のコード等を編集することで解決しました。

作ったコードはシンプルで、`/planning/scenario_planning/lane_driving/behavior_planning/path`トピックにサブスクライブし、受信をしたら"I heard"と画面にプリントするというものです。

### コード
今回は`initialpose_publisher`フォルダをコピペしました。
フォルダ名を`path_subscriber`に変更、中の`config`と`launch`フォルダを削除、そしてファイルを以下のように作成・編集しました。

```src/path_subscriber.hpp
#ifndef PATH_SUBSCRIBER_HPP_
#define PATH_SUBSCRIBER_HPP_

#include <rclcpp/rclcpp.hpp>
#include <autoware_auto_planning_msgs/msg/path.hpp>

using Path = autoware_auto_planning_msgs::msg::Path;

class PathSubscriber : public rclcpp::Node {
    public:
        PathSubscriber();

        void topicCallback(const Path::ConstSharedPtr msg);
    
    private:
        rclcpp::Subscription<Path>::SharedPtr sub_path_;
        Path::ConstSharedPtr path_;
};
#endif  // PATH_SUBSCRIBER_HPP_
```

```src/path_subscriber.cpp
#include "path_subscriber.hpp"
#include <memory>

using Path = autoware_auto_planning_msgs::msg::Path;
using std::placeholders::_1;

PathSubscriber::PathSubscriber() : Node("path_subscriber")
{
  sub_path_ = create_subscription<Path>("behavior_planning/path", 10, std::bind(&PathSubscriber::topicCallback, this, _1));
}

void PathSubscriber::topicCallback(const Path::ConstSharedPtr msg)
{
  path_ = msg;
  RCLCPP_INFO(this->get_logger(), "I heard");
}

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<PathSubscriber>());
  rclcpp::shutdown();
  return 0;
}
```

```CMakeLists.txt
cmake_minimum_required(VERSION 3.14)
project(path_subscriber)

find_package(ament_cmake REQUIRED)
find_package(autoware_cmake REQUIRED)
autoware_package()

ament_auto_add_executable(path_subscriber
  src/path_subscriber.cpp
)

ament_auto_package(INSTALL_TO_SHARE)
```

```package.xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>path_subscriber</name>
  <version>0.1.0</version>
  <description>The path_subscriber package</description>
  <maintainer email="TODO">TODO</maintainer>
  <license>Apache License 2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <build_depend>autoware_cmake</build_depend>

  <depend>autoware_auto_planning_msgs</depend>
  <depend>rclcpp</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>autoware_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>
```

### 実行
`autoware_mini_awsim.launch.xml`の`lane_driving` group内にパッケージを追加します。
（`lane_driving` group内に置くことでnamespaceが共有されます。外においた場合はトピックのremapなどの対応が必要です。）

```xml
<!-- lane_driving -->
<group>
  <push-ros-namespace namespace="lane_driving"/>
  
  <!-- 割愛 -->

  <!-- Path Subscriber -->
  <group>
    <node pkg="path_subscriber" exec="path_subscriber"/>
  </group> <!-- Path Subscriber -->
</group> <!-- lane_driving -->
```

実行する。
```sh
# Rockerコンテナ内で
cd /aichallenge
bash run.sh
```

今回はPathが生成される必要があるのでAWSIMも起動する。
```sh
# Rockerコンテナ内で
sudo ip link set multicast on lo
source /autoware/install/setup.bash
/aichallenge/AWSIM/AWSIM.x86_64
```

結果：`BehaviorPathPlanner`からの`Path`を受信して画面にメッセージをプリントできた！
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3483805/488092b1-323f-8803-2db6-bd3cc82ebf5a.png)

# 今後の予定
`autoware_auto_msgs`を使うことができたので、次回以降はいよいよそれらのmsgを使って経路生成の実装を進めていきます。
構想としては`BehaviorPathPlanner`からのpathと`OccupancyGridMap`を入力として受け取りTrajectoryを生成するイメージです。