LoginSignup
3
1
@h_bog(H Bog)

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

Last updated at Posted at 2023-08-07

概要

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

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

前の記事はこちら：

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

目標

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

必要な前知識

2つのPlannerの違い

障害物回避には回避経路を生成する必要があります。

Autowareは主にBehaviorPathPlannerとMotionPlannerの2つのモジュールが組み合わさって経路が生成されています。それぞれの違いは以下の通りです。

  • BehaviorPathPlanner： 交通ルール（信号、一時停止線、走行可能エリア等）を加味したPath（＝経路？）を生成します。Pathについての詳細はこちらに書いてあります。
  • MissionPlanner：交通ルールは考慮しないが、障害物の回避や速度の調整などをしたTrajectory（＝軌道？）を生成します。詳細はこちらに書いてあります。

PathとTrajectoryは持っている情報が違います。どちらもautoware_auto_planning_msgsでmsg型が定義されているので、より詳細を知りたい方はそちらをご参照ください。

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

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

Potential Field Algorithm

具体的な経路生成手法ですが、Potential Fieldアルゴリズムを使おうと考えています。理由は仕組みが直感的に理解しやすいからです。イメージは以下の通りです：

  • 目的地は車両を「引き寄せる力」を発している
  • 障害物は車両を「遠ざける力」を発している

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

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

Autowareをカスタマイズ

パッケージの作成（練習）

Autowareにパッケージを追加する方法ですが、公式サイトにちょっとだけ説明が載っています。

まずは難しいことはせずに、簡単なコードを作ってビルド・実行ができるかを試してみます。
ROSチュートリアルのWriting a simple publisher and subscriberを見ながら、簡単なパブリッシャーとサブスクライバーを作ります。

まず、公式サイトにあるようにaichallenge_submitのフォルダにパッケージを作成します。

$ cd ~/aichallenge2023-sim/docker/aichallenge/aichallenge_ws/src/aichallenge_submit  # aichallenge_submitに移動
$ ros2 pkg create --build-type ament_cmake cpp_pubsub  # cpp_pubsubというパッケージを作成

あとはROSチュートリアルに従ってコードをダウンロードして、package.xmlとCMakeLists.txtを変更します。

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

次に、Dockerを起動してコードをビルドします。

# Rockerコンテナ内で
cd /aichallenge
bash build.sh

以下のようにcpp_pubsubが他のパッケージと一緒にビルドされれば成功です。
image.png

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

パッケージの実行（練習）

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

Launchファイルの作り方はROSチュートリアルのCreating a launch fileを参照します。今回はトピックのremapやパラメータの設定は必要ないので非常に簡単な編集でできます。今回はautoware_mini_awsim.launch.xmlのlane_driving group内に以下の"Simple Pub Sub"部分を追加しました。

<!-- 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コンテナを起動し、以下のコマンドを実行します。

# Rockerコンテナ内で
cd /aichallenge
bash run.sh

Autowareの他のパッケージと同じ様に実行することができました！
少し分かりにくいですが、以下の画像でtalker-27やlistener-28の部分から、追加したパブリッシャーとサブスクライバーが正常に動作していることが確認できます。
Screenshot from 2023-08-07 12-09-11.png

autoware_auto_msgsを用いたパッケージ

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

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

結論から言うと前述したパッケージの作成方法だと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などの対応が必要です。）

<!-- 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 -->

実行する。

# Rockerコンテナ内で
cd /aichallenge
bash run.sh

今回はPathが生成される必要があるのでAWSIMも起動する。

# Rockerコンテナ内で
sudo ip link set multicast on lo
source /autoware/install/setup.bash
/aichallenge/AWSIM/AWSIM.x86_64

結果：BehaviorPathPlannerからのPathを受信して画面にメッセージをプリントできた！
image.png

今後の予定

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

3
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
3
1