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

はじめに

こんにちは、nacky823 です👋

この記事は ROS 2 Advent Calendar 2023 21日目の投稿です。

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

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

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

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

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

本記事では、autoware.universe の Localization 機能と navigation2 を使用し、移動ロボットを自律走行させる方法を紹介します。

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

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

まず、自律走行を行うための環境地図を事前に作成します。
その際、以下の２つのステップを踏みます。

1. ３次元の点群地図を作成
   • 地図のファイル形式: .pcd
2. ３次元の点群地図に対応した、２次元の占有格子地図を作成
   • 地図のファイル形式: .pgm

今回は地図の作成方法については触れないので、ここのリンクに作成済みの地図を載せています。任意の場所に保存し、シミュレーションを行う際に使用してください。

作成した地図を使用して、以下のような処理で自律走行を行います。

1. ３次元点群地図と、センサデータ（３次元点群、オドメトリ）をもとに、自己位置推定
2. 推定した自己位置をもとに、２次元占有格子地図上で経路計画を行い、自律走行

ここからは、上記の 自己位置推定 〜 自律走行 の動作方法について説明していきます

動作環境

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

• OS: Ubuntu 22.04.3 LTS
• ROS 2 Distribution: Humble Hawksbill

以下は、必ず使用する ROS 2 パッケージ一覧です。

• autoware.universe
  • sensing/pointcloud_preprocessor
  • map/map_loader
  • localization/ekf_localizer
  • localization/ndt_scan_matcher
• navigation2
• ros2_odometry_twist_converter

実機で動作させる場合は、以下のセンサデータを送信するノードが必要です。

• ３次元点群: /points_raw [sensor_msgs/msg/PointCloud2]
• オドメトリ: /odom [nav_msgs/msg/Odometry]

以下は、シミュレーションを行う場合に使用する ROS 2 パッケージ一覧です。
シミュレーション環境には、RT Corporation 様のリポジトリを使用しています。

• raspicat_description
• raspicat_ros
• raspicat_sim
• raspicat_slam_navigation

autoware.universe のインストール

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

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

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

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

編集の手順

環境構築を行った autoware.universe 内のファイルを、あらかじめ用意したリポジトリのファイルに書き換えていきます。

1. 以下の ekf_localizer の Launch ファイルを用意ファイルに書き換える

• File path: localization/ekf_localizer/launch/ekf_localizer.launch.xml

2. 以下の ndt_scan_matcher の Param ファイルを用意ファイルに書き換える

• File path: localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

3. 以下の ndt_scan_matcher の Launch ファイルを用意ファイルに書き換える

• File path: localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml

4. 以下の map_loader の Param ファイルを用意ファイルに書き換える

• File path: map/map_loader/config/pointcloud_map_loader.param.yaml

5. 以下の map_loader の Launch ファイルを用意ファイルに書き換える

• File path: map/map_loader/launch/pointcloud_map_loader.launch.xml
• 書き換えた map_loader の Launch ファイルの 2, 3 行目のパスを、
  先ほど保存した３次元点群地図（.pcd）の絶対パスに書き換える

7. 用意された pointcloud_preprocessor の Launch ファイルを以下に保存

• 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 ファイルの絶対パスに変更して実行

２次元占有格子地図は、以下のようなディレクトリ構造で保存する必要がある。

cost
 ├── iscas_museum_2d.pgm
 └── iscas_museum_2d.yaml

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

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

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 でロボットが向かう位置を示す

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

おわりに

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

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