はじめに

AIチャレンジの予選が残すことあと２日にまで迫りました。みなさんも最終調整に入っているころでしょうか。私は、ランキング上位に圧倒されすぎて絶望しています。(´；ω；｀)
遅くなりましたが、今回はCostmapの作成をAutowareの既存のモジュールで行いたいと思います。
私のgithubのリンクをおいておきますが、この記事とは関係のない変更がある可能性もあります。
github

Costmap Generatorの導入、ビルド

Autoware node diagramをながめていると、costmap_generatorというNodeを発見しました。こちらを使えば、Costmapを作成できると考え、導入を試みました。

まずは、AIチャレンジのワークスペースにコピーします。
AIチャレンジで利用されているawsim-stableブランチのAutoware.universeを好きな場所にクローンします。このとき、ワークスペースにクローンしてしまうと、とんでもないビルドが走るので注意してください。

git clone -b awsim-stable https://github.com/autowarefoundation/autoware.universe.git

クローンができたら、AIチャレンジのワークスペースにcostmap_generatorをコピペしてください。
(universe/planning/constmap_generatorにあります)

次にビルドします。

./build_autoware.bash

※実はすでにAIチャレンジDockerに導入されているパッケージであり、/autoware/install　にあるのですが、ソースコードをカスタムするために再ビルドしています。

Costmap Generatorのカスタマイズ

さきほどのAutoware node diagramを確認すると、これはParkingのときに利用することを想定していることがわかります。ソースコードを確認すると、Parkingシナリオのときに起動するようにできています。

bool CostmapGenerator::isActive()
{
  if (!lanelet_map_) {
    return false;
  }

  if (activate_by_scenario_) {
    if (scenario_) {
      const auto & s = scenario_->activating_scenarios;
      ///ParkingのときにActivateしている
      if (
        std::find(std::begin(s), std::end(s), tier4_planning_msgs::msg::Scenario::PARKING) !=
        std::end(s)) {
        return true;
      }
    }
    return false;
  } else {
    const auto & current_pose_wrt_map = getCurrentPose(tf_buffer_, this->get_logger());
    return isInParkingLot(lanelet_map_, current_pose_wrt_map->pose);
  }
}

この条件をなくすと〜

コストマップの生成ができた！

このコストマップは、base_linkで座標変換がされるので車両の位置に合わせて動きます。もし、固定したい場合は、例えば、スタート地点のポーズをTFにして、launchに書きます。(static_baseと名前をつけています。)

<node pkg="tf2_ros" exec="static_transform_publisher" output="screen"
        args="89633.15625 43127.80078125 42.195587158203125 0.004984998786902679 -0.010415014249405455 0.8738473964122314 0.4860631698131876 map static_base"/>

そして、costmap_generator/launch/costmap_generator.launch.xmlを編集し、base_linkの場所をstatic_baseに書き換えます。

<param name="costmap_frame" value="map"/>
<!--param name="vehicle_frame" value="base_link"/-->
<param name="vehicle_frame" value="static_base"/>
<param name="map_frame" value="map"/>

固定されたコストマップの完成！

Costmapに障害物の情報を加える

AIチャレンジの初期環境では、障害物がAutowareのメッセージでPubされていません。そのため、自分で障害物情報をPubする必要があります。こちらの説明は省略します。詳しくは、私のgithubのパッケージを見てもらえると幸いです。

/aichallenge/objectsの情報をautoware_auto_perception_msgs::msg::PredictedObjectsの型でPubするNodeを私は作成しました。このTopicをCostmap GeneratorのTopicと一致させることで、コストマップに障害物情報が加えられます。

#さいごに
今回はコストマップをAutowareのパッケージで実装しました。ほかにもAIチャレンジのワークスペースに実装されていないパッケージが眠っているので、使えそうなパッケージを探してみたいとおもいます。