Autoware x AWSIM シミュレーションで初期姿勢を設定する方法

はじめに

自動運転AIチャレンジ2024決勝 @ 11月初旬 に向けて開発を進めています。
決勝では、実車ならではの課題にチャレンジ させていただけるようです…　ヒィ…！

決勝では、コースで競技車両であるEVレーシングカートを使用した大会を行います。シミュレーションで得た知見を実車両に活かしつつ、AWSIMでは再現できない実車ならではの課題にもチャレンジします。
例えば参加者には実車両に適用するため、パラメータ調整にも挑戦してもらいます。また、シミュレーションでは再現できないノイズ処理、遅延対策のアルゴリズム開発も行います。

現在我々は、以下の 決勝向けアップデート の取り込み作業を進めています。

稀に 初期姿勢がおかしな方向になり走行シミュレーションできない場合がある ことがわかりましたので、その対策として 初期姿勢を設定する方法 をご紹介します。

初期姿勢の設定方法

いくつか方法がありそうですが、ここでは私が試した2つの方法を共有します。

1. 姿勢設定ノードを作成する（こちらまだ動きません、調整中）

2023年インテグレーション大会のリポジトリに以下ノードがありましたので、使ってみます。

これを2024年のsrc/aichallenge_submit下にコピーして、launchで起動します。

aichallenge_submit.launch.xml

…
 <!-- place a goal pose anywhere you like-->
  <node pkg="goal_pose_setter" exec="goal_pose_setter_node" name="goal_pose_setter" output="screen">
    <param from="$(find-pkg-share goal_pose_setter)/config/default_goal_pose.param.yaml" />
  </node>
  
  <!-- initialize pose -->
  <node pkg="initialpose_publisher" exec="initialpose_publisher" name="initialpose_publisher" output="screen">
    <param from="$(find-pkg-share initialpose_publisher)/config/initialpose_publisher.param.yaml" />
  </node>

</launch>

しかし、この方法だと初期姿勢を設定することができませんでした。
/api/localization/initialization_state の UNINITIALIZEDをトリガに、初期姿勢がpublishされるようなのですが、トリガとなっているトピックがpublishされていないようです。

2. bashにpublishコマンドを記載する

上記で実現できなかったため、以下を参考にコマンドで初期姿勢をpublishしてみます。

run_evaluation.bash

…

ros2 service call /debug/service/capture_screen std_srvs/srv/Trigger >/dev/null
sleep 1

# Publish initial pose
echo "Publish initial pose"
ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped '{
    header: {
        stamp: {
            sec: '"$(date +%s)"',
            nanosec: 0
        },
        frame_id: "map"
    },
    pose: {
        pose: {
            position: {
                x: 89633.3125,
                y: 43126.9296875,
                z: 0.0
            },
            orientation: {
                x: 0.0,
                y: 0.0,
                z: 0.8753332371013277,
                w: 0.4835201381801081
            }
        },
        covariance: [
           0.25, 0.0, 0.0, 0.0, 0.0, 0.0,
           0.0, 0.25, 0.0, 0.0 ,0.0, 0.0,
           0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
           0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
           0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
           0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891909122467
        ]
    }
}' --once >/dev/null

# Start driving and wait for the simulation to finish
echo "Waiting for the simulation"

…

こちらは所望の動作が実現できました。
以下のように走行開始前に指定した初期姿勢へと修正されています。

おわりに

以上、初期姿勢を設定する方法のご紹介でした。

今回使用したコマンドからpublishする方法はrun_evaluation.bashで起動されない限り有効にならないので、launchファイル等に書いたほうがいいかな…と本記事を執筆しながら思ってきました。
また、initialpose_publisherを動かすことができれば、初期姿勢パラメータ変更も容易でベターと思いますので、もう少し考えてみます。 (/api/localization/initialization_state の UNINITIALIZED を publishすればいいのかな…？ )

ということで、決勝も楽しんでいきましょう！！

この記事は、AI文章校正ツール「ちゅらいと」で校正されています。