AIチャレンジ2024 Autowareのパッケージでcostmapを作る

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

# Costmap Generatorの導入、ビルド
[Autoware node diagram](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/node-diagram/)をながめていると、costmap_generatorというNodeを発見しました。こちらを使えば、Costmapを作成できると考え、導入を試みました。

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

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

次にビルドします。
```bash
./build_autoware.bash
```


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

# Costmap Generatorのカスタマイズ

さきほどの[Autoware node diagram](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/node-diagram/)を確認すると、これはParkingのときに利用することを想定していることがわかります。ソースコードを確認すると、Parkingシナリオのときに起動するようにできています。
```c++
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);
  }
}
```
この条件をなくすと〜
![Videotogif.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3673708/ba3f80a2-1170-ae96-4584-66c4b59206fc.gif)
コストマップの生成ができた！

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

```

![Videotogif (1).gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3673708/d8e45140-c51f-fa53-4efb-42cfa2e24e08.gif)
固定されたコストマップの完成！

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

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

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3673708/4407313d-b62f-b0f9-2070-52c21eb21496.png" alt="Example Image" width="400" height="400">

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