はじめに
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チャレンジのワークスペースに実装されていないパッケージが眠っているので、使えそうなパッケージを探してみたいとおもいます。