Autoware Universe の Localization 機能と Navigation2 を使用してつくばチャレンジに参加した話

## はじめに

こんにちは、nacky823 です👋

この記事は [ROS 2 Advent Calendar 2023](https://qiita.com/advent-calendar/2023/ros2) 21日目の投稿です。

私たちは今年、3D LiDAR を搭載した自律移動ロボットを用い、つくばチャレンジに初参加してきました。[つくばチャレンジ](https://tsukubachallenge.jp/2023/about/overview)は、移動ロボットが遊歩道等の市街地を自律走行する、技術チャレンジです。参加している方々の技術力は凄まじく、圧倒されながらも楽しい経験が出来ました。

https://tsukubachallenge.jp/2023/about/overview

3D LiDAR を用いた移動ロボットの自律走行は初めての経験であり、ゼロから自律走行を行う環境を構築する必要があったため、つくばチャレンジへの参加期間中に完成するかどうかギリギリの状態でした😅

一時期は行き詰まった状態からなかなか抜け出せず、某C●C●壱でやけ食いしたこともありましたが、同じくつくばチャレンジに参加している他大学の方や、先輩方の助けもあり、なんとか[本走行](https://tsukubachallenge.jp/2023/about/schedule)では約 150 メートルの自律走行を行うことができました 🎉


本番前日まで動くかどうか怪しいラインだったので、当日は動いてくれて本当に嬉しかったですね。その際の我らがロボットの勇姿は、下記の動画から見ることができるので、良ければご覧ください。

https://youtu.be/N0qvrBZ9X80?si=0Nqb_8xXOqRr6zot

それでは、そろそろ本題に入らなければお叱りを受けそうなので、気を引き締めまして。

本記事では、[autoware.universe](https://github.com/autowarefoundation/autoware.universe.git) の Localization 機能と [navigation2](https://navigation.ros.org/index.html) を使用し、移動ロボットを自律走行させる方法を紹介します。

主要な内容としましては、「自己位置推定から自律走行」を行うための作業手順となっており、最終的にはシミュレーション上で自律走行を行うことを目標とします。

### 実施した自律走行の大まかな流れ

まず、自律走行を行うための環境地図を事前に作成します。
その際、以下の２つのステップを踏みます。
1. **３次元の点群地図を作成**
    + 地図のファイル形式: **`.pcd`**
1. **３次元の点群地図に対応した、２次元の占有格子地図を作成**
    + 地図のファイル形式: **`.pgm`**

:::note
今回は地図の作成方法については触れないので、[ここのリンク](https://drive.google.com/drive/folders/1BlO1ROFuVQugXf-dDo2YeupFj47nrUnt?usp=sharing)に作成済みの地図を載せています。任意の場所に保存し、シミュレーションを行う際に使用してください。
:::

作成した地図を使用して、以下のような処理で自律走行を行います。
1. **３次元点群地図と、センサデータ（３次元点群、オドメトリ）をもとに、自己位置推定**
1. **推定した自己位置をもとに、２次元占有格子地図上で経路計画を行い、自律走行**

<font color="GoldenRod">ここからは、上記の **自己位置推定 〜 自律走行** の動作方法について説明していきます :writing_hand:</font>

## 動作環境

今回使用するソフトウェアはすべて ROS 2 を使用し、以下の環境で動作させることを想定しています。
+ OS: Ubuntu 22.04.3 LTS
+ ROS 2 Distribution: Humble Hawksbill

以下は、必ず使用する ROS 2 パッケージ一覧です。
+ [autoware.universe](https://github.com/autowarefoundation/autoware.universe.git)
    + [sensing/pointcloud_preprocessor](https://github.com/autowarefoundation/autoware.universe/tree/main/sensing/pointcloud_preprocessor)
    + [map/map_loader](https://github.com/autowarefoundation/autoware.universe/tree/main/map/map_loader)
    + [localization/ekf_localizer](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ekf_localizer)
    + [localization/ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher)
+ [navigation2](https://navigation.ros.org/index.html)
+ [ros2_odometry_twist_converter](https://github.com/nacky823/ros2_odometry_twist_converter/tree/feat/sim-true)

:::note info
実機で動作させる場合は、以下のセンサデータを送信するノードが必要です。
+ ３次元点群: `/points_raw [sensor_msgs/msg/PointCloud2]`
+ オドメトリ: `/odom [nav_msgs/msg/Odometry]`
:::

以下は、シミュレーションを行う場合に使用する ROS 2 パッケージ一覧です。
シミュレーション環境には、[RT Corporation](https://github.com/rt-net) 様のリポジトリを使用しています。
+ [raspicat_description](https://github.com/nacky823/raspicat_description/tree/feat/ros2-pc2-sensor)
+ [raspicat_ros](https://github.com/rt-net/raspicat_ros/tree/ros2)
+ [raspicat_sim](https://github.com/rt-net/raspicat_sim/tree/ros2)
+ [raspicat_slam_navigation](https://github.com/nacky823/raspicat_slam_navigation/tree/feat/ros2-pc2-sensor)

### autoware.universe のインストール

autoware.universe の環境構築を行う方法については、公式ドキュメントまたは @porizou1 さんが素晴らしい記事を書かれているのでそちらをご覧ください。

公式ドキュメント: Autoware Documentation Source installation

https://autowarefoundation.github.io/autoware-documentation/main/installation/autoware/source-installation/

@porizou1 さんの記事: 最新のAutowareをGPUなしで環境構築できるDockerfileを作った

https://qiita.com/porizou1/items/95dacd4e8842e19c3369


::: note info
この記事では、autoware.universe の自己位置推定に関する機能のみを使用するため、上記４つのパッケージのビルドに成功していれば問題ありません。
```
colcon build --symlink-install --packages-up-to pointcloud_preprocessor map_loader ekf_localizer ndt_scan_matcher --cmake-args -DCMAKE_BUILD_TYPE=Release
```
:::

### navigation2 のインストール

```
sudo apt install ros-humble-navigation2
sudo apt install ros-humble-nav2-bringup
```

### ros2_odometry_twist_converter のインストール

+ 適切な ROS 2 ワークスペースの `src` に移動し、以下を実行（ex. `~/ros2_ws/src`）
```
git clone https://github.com/nacky823/ros2_odometry_twist_converter.git
cd ..
colcon build --symlink-install --packages-up-to ros2_odometry_twist_converter
```

### シミュレーション環境のインストール

+ 適切な ROS 2 ワークスペースの `src` に移動し、以下を実行（ex. `~/ros2_ws/src`）
```
git clone -b feat/ros2-pc2-sensor https://github.com/nacky823/raspicat_description.git
git clone -b ros2 https://github.com/rt-net/raspicat_ros.git
git clone -b ros2 https://github.com/rt-net/raspicat_sim.git
git clone -b feat/ros2-pc2-sensor https://github.com/nacky823/raspicat_slam_navigation.git
./raspicat_sim/raspicat_gazebo/scripts/download_gazebo_models.sh
cd ..
rosdep update
rosdep install -r -y --from-paths src --ignore-src
colcon build --symlink-install --packages-up-to raspicat_description raspicat_navigation raspicat_slam raspicat_bringup raspicat raspicat_gazebo
```

## autoware.universe の Launch & Param ファイルの編集

下記のリポジトリは、autoware.universe 様のオリジナルリポジトリをフォークしたものです。今回はこのリポジトリにあらかじめ編集済みのファイルを用意していますので、それを使用してシミュレーションを行います。

https://github.com/nacky823/autoware.universe/tree/demo/ekf-sim

### 編集の手順

環境構築を行った `autoware.universe` 内のファイルを、[あらかじめ用意したリポジトリ](https://github.com/nacky823/autoware.universe/tree/demo/ekf-sim)のファイルに書き換えていきます。

#### 1. 以下の ekf_localizer の Launch ファイルを[用意ファイル](https://github.com/nacky823/autoware.universe/blob/demo/ekf-sim/localization/ekf_localizer/launch/ekf_localizer.launch.xml)に書き換える

+ File path: `localization/ekf_localizer/launch/ekf_localizer.launch.xml`

#### 2. 以下の ndt_scan_matcher の Param ファイルを[用意ファイル](https://github.com/nacky823/autoware.universe/blob/demo/ekf-sim/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml)に書き換える

+ File path: `localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml`

#### 3. 以下の ndt_scan_matcher の Launch ファイルを[用意ファイル](https://github.com/nacky823/autoware.universe/blob/demo/ekf-sim/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml)に書き換える

+ File path: `localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml`

#### 4. 以下の map_loader の Param ファイルを[用意ファイル](https://github.com/nacky823/autoware.universe/blob/demo/ekf-sim/map/map_loader/config/pointcloud_map_loader.param.yaml)に書き換える

+ File path: `map/map_loader/config/pointcloud_map_loader.param.yaml`

#### 5. 以下の map_loader の Launch ファイルを[用意ファイル](https://github.com/nacky823/autoware.universe/blob/demo/ekf-sim/map/map_loader/launch/pointcloud_map_loader.launch.xml)に書き換える

+ File path: `map/map_loader/launch/pointcloud_map_loader.launch.xml`
+ 書き換えた [map_loader の Launch ファイル](https://github.com/nacky823/autoware.universe/blob/demo/ekf-sim/map/map_loader/launch/pointcloud_map_loader.launch.xml)の **2, 3 行目**のパスを、
先ほど保存した３次元点群地図（**`.pcd`**）の絶対パスに書き換える

#### 7. 用意された [pointcloud_preprocessor の Launch ファイル](https://github.com/nacky823/autoware.universe/blob/demo/ekf-sim/sensing/pointcloud_preprocessor/launch/basic_preprocessor.launch.py)を以下に保存

+ Directory path: `sensing/pointcloud_preprocessor/launch`

+ pointcloud_preprocessor をビルド

    ```
    colcon build --symlink-install --packages-up-to pointcloud_preprocessor --cmake-args -DCMAKE_BUILD_TYPE=Release
    ```

## シミュレーションの実行

### 1. 各ターミナルで Launch ファイルを起動

```
ros2 launch raspicat_gazebo raspicat_with_iscas_museum.launch.py rviz:=false
```
```
ros2 launch ros2_odometry_twist_converter odom_to_twist_cov_stamp.launch.py
```
```
ros2 launch map_loader pointcloud_map_loader.launch.xml
```
```
ros2 launch pointcloud_preprocessor basic_preprocessor.launch.py 
```
```
ros2 launch ndt_scan_matcher ndt_scan_matcher.launch.xml 
```
```
ros2 launch ekf_localizer ekf_localizer.launch.xml
```
```
ros2 launch raspicat_navigation raspicat_nav2.launch.py map:=map_path.yaml
```

+ `map:=map_path.yaml` は、先ほど保存した `.yaml` ファイルの絶対パスに変更して実行

:::note info
２次元占有格子地図は、以下のようなディレクトリ構造で保存する必要がある。
```
cost
 ├── iscas_museum_2d.pgm
 └── iscas_museum_2d.yaml
```
:::



### 2. RViz 上で初期位置を指定

+ GUI から `2D Pose Estimate` でロボットがいる位置を示す :inbox_tray:

### 3. 自己位置推定を開始する

```
ros2 service call /trigger_node std_srvs/srv/SetBool "data: true"
```
```
ros2 service call /motor_power std_srvs/SetBool "{data: true}"
```

### 4. RViz 上でゴールを指定して自律走行

+ GUI から `2D Goal Pose` でロボットが向かう位置を示す :inbox_tray:

https://youtu.be/6Wp3zqb1lLU

**問題なく動作すれば、上の動画の様に自律走行ができます。**
RViz の設定の差により、見た目が異なる場合がありますが、挙動に差はないため問題はありません。お好みのレイアウトに編集してください。

## おわりに

以上、Autoware Universe の Localization 機能と Navigation2 を使用してつくばチャレンジに参加した話でした。



最後までお読みいただき、ありがとうございました :santa::blush: