自作ナビゲーションスタックでつくばチャレンジ2023に挑戦してみた話

# 概要
こんにちはBEIKEです。
クリスマス？こ、今年はなんとかなりそうです？？😇😇😇

ということで本編へ

私は今まで、つくばチャレンジ2021、2022というのに参加しました。
[つくばチャレンジ](https://tsukubachallenge.jp/2023/about/overview)は、移動ロボットがつくば市内のスタートからゴールまでの 
約 2 km のコースを自律走行する技術チャレンジです。

つくばチャレンジ2021、2022では、移動ロボットの自律移動のために
ROSの[**Navigation stack**](https://github.com/ros-planning/navigation)を使用していました。

[**Navigation stack**](https://github.com/ros-planning/navigation)は、パラメータの調整をするだけで、ある程度使えます。
例えば、つくばチャレンジ2022では、4回人の手が介入しましたが、スタートからゴールまで走り切ることができました。[**[動画]**](https://youtu.be/3gpjVhRIJDY?si=sLjXJs5qLE3dgy8Y)

しかし今年は、ROSのEOLも近いということで、ROS 2の[**Nav2**](https://github.com/ros-planning/navigation2)を使用することを考えていました。。。。。

が、ある時、「毎年どこかのパッケージを使ってるだけで、**俺って、自律移動何もわかってないんじゃ。。。**」 となりました。

そこで！
自作ナビゲーションスタックでつくばチャレンジに参加すれば
**自律移動を完全に理解した**になれるんじゃね！？
と考えました。

**自作ナビゲーションスタック(2次元)の作成の話**や**つくばチャレンジ2023での結果**について
共有できればと思い、記事を書いてみました！

# 自作ナビゲーションの作成の話

## 作成し始める！？
* 夏休みの始まりから作り始めました

https://github.com/uhobeike/ike_nav/commit/738c3838f7cfc0e1dfd3bc81443ada54628bd937

## 実装の順番を考える
* 自作ナビゲーションスタックを作成するにあたって、以下のような実装の順番を考えました

1. 地図の管理
1. コストマップの管理
1. 経路計画
1. 自己位置推定
1. 経路追従
1. ゴールまでの管理
1. 複数ゴールの管理

* 具体的には
1. pgmから占有格子地図に変換
1. 占有格子地図からコストマップを作成
1. コストマップを使って、経路計画をできるように
1. 自己位置推定しながら、経路計画をできるように
1. 自己位置推定をしながら、求めた経路を追従するように
1. ゴールに着いたら、停止するように
1. 複数のゴールを経由して、最後のゴールで停止できるように

## ということで実装していくぜ！

あれ、あなた何もわからないんじゃ。。。。
そうです！
アルゴリズムの名前は聞いたことがあるものの、実装方法は全く知らんのです。。。

## こんな世の中なので？！

自分の知識の限界は、あります。
当然わからないことばかりです。
しかし昨今、知らないことは何でも教えてくれる最強の相棒のChatGPTが登場しました。

ネットの情報やChatGPTのお力を借りて、全力実装です。

## さぁ、実装

* 7月22日　地図の管理の実装　完了

https://github.com/uhobeike/ike_nav/commit/5b133b458c1be49d227ebb597ce7120ec614ed39

* 7月28日　経路計画の実装（A*）　完了

https://github.com/uhobeike/ike_nav/commit/c95c712eb3dd248b3f859f24fe843a2fd6c06ba0

* 7月29日　自己位置推定の実装（Monte Carlo Localization ）　完了

https://github.com/uhobeike/ike_nav/commit/11f52973dbaaa20bdfb283c4c52028126a141fd2

* 8月4日　ゴールの管理の実装　完了

https://github.com/uhobeike/ike_nav/commit/980b830a5b18a9af77569f3a13b1ee78d8f2a603

* 8月12日　経路追従の実装（Model Predictive Control）　完了

https://github.com/uhobeike/ike_nav/commit/18d562d092ef14c7ad11c5345f006607e9ce9bcd

* 8月12日　コストマップの管理の実装　完了

https://github.com/uhobeike/ike_nav/commit/e691157fcab95526455d40942668f2cb62db07b6

* 8月24日　複数ゴールの管理の実装　完了

https://github.com/uhobeike/ike_nav/commit/3a7725fe82320af6d9b6ab6936848719130de445

予定していた実装の順番となんか違うけど、ヨシ！

## 実装完了？！
ということで、1ヶ月を使って、なんとか実装完了しました！　

よし、後は、つくばチャレンジで動かすだけだぜ！

になりたかったのですが、色々バグがあって、思ったように動かなかったりしていました。。。
なので、バグ取りや実機で動かすために改善をしていこうと思ったのですが、
大学の研究作業の方が優先で、しばらく放置していました。
（バグ取りがなかなかできなくて、開発のモチベーションがなかったのもある）

## つくばチャレンジ本走行前日？？？

えーと、気がついたら、つくばチャレンジ本走行前日になりました。。。
最初の実験走行（7/15-16）、本走行の前日（11/18）、本走行（11/19）以外は
後輩チームの様子を見たりしていました。

本走行前日の深夜から朝まで、地図からシミュレータ環境を作って
なんとか動くようにコードの修正を行いました。
なんとかなるか。。。。

なりますた！

実機で自作ナビゲーションスタックを動かすのは、ほぼ初めてたっだのですが
なんか動きました。。。
自分でもびっくりです。なんで上手く動いてるんだって思いました。（なんでやねん）

この調子で行けば、確認走行エリアの完走できるやろ！！

# つくばチャレンジ2023　本走行　結果発表！

最後の最後で、障害物回避ができなかっっった！！！
確認走行エリア完走ならず！

記録としては、自律走行距離440.9[m]でした。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">本走行は、障害物回避の実装が甘かったので途中でリタイアでした😅<a href="https://twitter.com/hashtag/%E3%81%A4%E3%81%8F%E3%81%B0%E3%83%81%E3%83%A3%E3%83%AC%E3%83%B3%E3%82%B82023?src=hash&amp;ref_src=twsrc%5Etfw">#つくばチャレンジ2023</a> <a href="https://t.co/33oKNyQkZ7">pic.twitter.com/33oKNyQkZ7</a></p>&mdash; BEIKE ٩(´･ω･`)و (@BEIKE_RE) <a href="https://twitter.com/BEIKE_RE/status/1726252685622751557?ref_src=twsrc%5Etfw">November 19, 2023</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

## 本走行の時にびっくりしたこと

点字ブロックに足を持ってかれて、自己位置の推定が縦方向にずれていたり
周りに人がいたのでここでリタイヤするのかーと思ったのですが
なんとかなりました。。。（なんで上手く動いてるんだ。。）

↓22:45〜24:45まで
<iframe width="560" height="315" src="https://www.youtube.com/embed/k9yxRKOCa14?si=c7ISmE1wH5W4BhgU&amp;start=1365" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>

# 自作したナビゲーションスタックのパッケージについて

最終的に以下のパッケージが出来上がりました👶

まだまだ改善すべきところが多くあります。
例えば、今は、全部一つのコンポーネントにまとめてシングルスレッドで
動かしているので、マルチスレッドでも動くようにしたいです。

https://github.com/uhobeike/ike_nav

↑この記事でまだ紹介できてないパッケージの詳細については
READMEに書いてたりします！

# おまけ

以下のコマンドを実行すると、自作した*IKE_NAV*を試すことができます！！

:::note warn
注意
ダウンロードする際のDockerのImageは、5GBほどあります。
:::

```
docker run -it \
           --rm \
           --net=host \
           --env="DISPLAY=$DISPLAY" \
           --user $(id -u):$(id -g) \
           --mount type=bind,source=/usr/share/zoneinfo/Asia/Tokyo,target=/etc/localtime \
           --name ike_nav \
           ghcr.io/uhobeike/ike_nav:humble
```

# まとめ

なんとか、自作のナビゲーションスタックを作って
つくばチャレンジ2023に挑戦することができました。

少しは勉強出来た気がします。

結局これ。。。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/280989/72cbca9a-59a3-2965-8c57-fd7346c8475c.png)

ここまで、読んでいただきありがとうございます🙏

# 謝辞
ここまで結果を出すことができたのは、千葉工業大学 上田研からの研究費をありがたいことに使わせていただけたことと、千葉工業大学 上田研、林原研のメンバー、および先生からのフィードバック等があったからです。ここで感謝を申し上げます。