micro-ROSとros2_controlで構成したDiff Botでナビゲーションしてみた

# 概要
こんにちはBEIKEです。
クリスマス？そんなものは知りません。
ボッチで開発作業たのしいなぁ🙃

ということで本編へ

ある時、**micro-ROS**と**ros2_control**について全くわからねぇぇ。。。
いつ使うんだ？ってなりました。

そこで！
実際にロボット(*Diff Bot*)作って、**micro-ROS**と**ros2_control**使ってみれば
なんとなく分かるんじゃね！？
と考えました。

その中で得た知見や進捗について共有できればと思い、記事を書いてみました！

:::note warn
当方、知識が浅いため、もしかしたら適当なことを書いてることがあるかもしれません。
コメントで指摘等いただけると大変助かります🙏
:::

# 準備編？

## micro-ROSってなんぞや？？？

以下の**micro-ROS**公式サイトがあります。
https://micro.ros.org/

[概要](https://micro.ros.org/docs/overview/features/)を読んでみると
> **Micro-ROS** は、ノード、パブリッシュ/サブスクライブ、クライアント/サービス、ノード グラフ、ライフサイクルなどの
すべての主要なコア概念をマイクロコントローラー (MCU) にもたらします。

と書かれていました。

つまり、マイコン上でROS2の実装ができるってわけですね🙃（本当か？）

更に読んでみると
どうやら、**micro-ROS**には[**Agent**](https://github.com/micro-ROS/micro-ROS-Agent)と**Client**というものが存在するそうです。

**Agent**さんは、何をしているかというと
通常のROS2ノードと**micro-ROS**ノードとの橋渡し（ブリッジ）を行っているそうです。

**Client**さんは、マイコン上で動作している**micro-ROS**ノードらしいです。

多分こんな感じ？
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/280989/1974f0f5-b38c-3bcc-a450-0c8ab59abf30.png)


そして、ROS2ノードと**micro-ROS**間は
UDP、TCPあるいはシリアルポートを使用した2通りの通信方法が可能です。

### ここで疑問

**マイコン側のメモリ（RAM）の使用**とかって、どうなってるんだ？

Agent側で上手いことやってくれてるのかな？（PC側で負担）と思い調べてみました。

**micro-ROS**はデフォルトで**eProsima Micro XRCE-DDS**というミドルウェアを使用しているそうです。（[参考元](https://github.com/micro-ROS/micro_ros_stm32cubemx_utils/blob/58b4c8fe26c5cd695b7a33120c754a8a78d71300/README.md?plain=1#L23)）
このミドルウェアについても同様に公式サイトがあります。
https://micro-xrce-dds.docs.eprosima.com/en/latest/

で、サイトに書かれている
**eProsima Micro XRCE-DDS**の主な機能を読むと

> ライブラリは、 クライアント スタックの増加によるメモリ フットプリントのみであるため、動的および静的メモリ フリーです。 2kB未満のRAMで単純なパブリッシャー/サブスクライバーを管理できます。 さらに、 クライアント の概念に従って構築されて はプロファイル おり、バイナリ サイズを変更すると同時にライブラリに機能を追加またはライブラリから削除できます。

と書かれていました。

なので、マイコン側のRAMで負担してそうだなーとなりました。

**以上、micro-ROSってなんぞや？？？
です。色々勘違いをしてそうですが、こんなもんだろ？という感じです。**

## ros2_controlってなんぞや？？？

そもそも、ROS(1)にもros_controlというのが存在していて
何がなんだか分からず使ったことがありませんでした。

これも**ros2_control**の公式サイトがあるので読んでみます。
https://control.ros.org/master/index.html

> ros2_controlは、(ROS 2)を用いてロボットを(リアルタイムに)制御するためのフレームワークです。ROS (Robot Operating System) で使用されている ros_control パッケージを書き直したパッケージです．


また、**ros_control**や**ros2_control**については、多くの先人方が記事で説明をしてくださっています。

例えば、以下の記事が分かりやすかったです。
* [ros2_controlを使ってみた](https://rt-net.jp/humanoid/archives/3571)

> ros2_controlは、世の中にある制御理論をロボットに適用しやすくするものです。（※私の解釈です）
> 例えば、2輪走行ロボットで自動走行したい場合、アプリケーション（自動走行）とロボットとの間に差動2輪走行制御が必要です。
この走行制御はros2_control特有のものではなく、すでに世の中にある制御理論です。
> アプリケーションやロボットのハードウェアをできるだけ意識せずに、制御理論を適用できる仕組みがros2_controlです。
 
なるほど。。。

**ということで？？？**
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/280989/52b1ea46-5d61-980b-bd49-42bc8a330175.png)


# *Diff Bot*を作る？？

## *Diff Bot*について

ここからは、**micro-ROS**と**ros2_control**実装のために
作製した*Diff Bot*について説明します。

まず、名前の由来ですが、
*Diff Bot*は**Differential Robot**の略です。

あとは、[ros2_control_demos](https://github.com/ros-controls/ros2_control_demos)にある*Diff Bot*から来てます。

作成したロボットがこちら!

<div align="center">
  <img src="https://user-images.githubusercontent.com/40545422/209434577-0b613f76-7072-439d-89f4-00957e8f6136.jpg" width="500">
</div>

ロボットの作成に関しては、
同期の**高見俊介**さんがハードウェアと速度制御周り、
**micro-ROS**、ROS2周りを私が担当しました。
 
後ほど、説明がありますが、このロボット（*Diff Bot*）の
部品やコードはすべてパッケージとして公開しています。

## *Diff Bot*のハードウェア構成

ざっと、ハードウェア構成はこんな感じです。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/280989/50f1fed0-2b48-3def-7347-2f8651d4538a.png)

**micro-ROS Agent**用、LiDAR（RPLiDAR S1）用にRaspberry Pi 4 DRAM 4GBを搭載してます。
**micro-ROS Client**用にNUCLEO-F446REを搭載してます。

NUCLEO-F446REについては、手元にあったものでmicro-ROSが動くボードということで採用しました。

## *Diff Bot*のソフトウェア構成

ソフトウェア構成についてはこんな感じです。（SLAMとナビゲーションをまとめました）

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/280989/e9c178a7-2502-dea3-cf08-565c9d17440c.png)


本当は、Raspberry Piのみでナビゲーションをしたかったのですが、
やってみた感じCPU処理負荷に耐えられず、処理落ちしてしまうことがありました。（調査不足）

そのため、PCのCPUリソースをお借りして、SLAMとNavigationを行いました。

## *Diff Bot*のパッケージについて

#### [takagon-dynamics/diff_bot](https://github.com/takagon-dynamics/diff_bot)
* *Diff Bot*関連のパッケージを[repos](https://github.com/takagon-dynamics/diff_bot/blob/main/diff_bot.repos)でまとめた
* **micro-ROS Agent**用の[Docker file](https://github.com/takagon-dynamics/diff_bot/blob/main/Dockerfile.humble)がある

#### [takagon-dynamics/diff_bot_control](https://github.com/takagon-dynamics/diff_bot_control.git)
* *Diff Bot*の各リンクの公開
* /cmd_velからodomリンクの発行

#### [takagon-dynamics/diff_bot_documentation](https://github.com/takagon-dynamics/diff_bot_documentation.git)
* *Diff Bot*用の[ドキュメント](https://takagon-dynamics.github.io/diff_bot_documentation/document/diff_bot_documentaion/)
* インストール、実行方法について記載（現在進んでない）

#### [takagon-dynamics/diff_bot_firmware](https://github.com/takagon-dynamics/diff_bot_firmware.git)
* [/cmd_velをサブスクライブ](https://github.com/takagon-dynamics/diff_bot_firmware/blob/03636bf930595903c0ed198a05752a102fe4ccdc/Core/Src/main.c#L117-L121)し、[PID制御によるDCモータの速度制御](https://github.com/takagon-dynamics/diff_bot_firmware/blob/03636bf930595903c0ed198a05752a102fe4ccdc/Core/Src/main.c#L180-L205)を行う

#### [takagon-dynamics/diff_bot_launch](https://github.com/takagon-dynamics/diff_bot_launch.git)
* Slam-toolbox、Navigation2の立ち上げに必要なパラメータやlaunchファイルがある

#### [takagon-dynamics/diff_bot_slam](https://github.com/takagon-dynamics/diff_bot_slam.git)
* SLAM用のlaunchファイルがある

#### [takagon-dynamics/diff_bot_navigation](https://github.com/takagon-dynamics/diff_bot_navigation.git)
* Navigation2用のlaunchファイルがある

#### [takagon-dynamics/Diff_Bot_Hardware](https://github.com/takagon-dynamics/Diff_Bot_Hardware.git)
* *Diff Bot*を作製するために必要なものをまとめた

# *Diff Bot*を動かしてみる！！！

ということで、作製したパッケージを使って*Diff Bot*を動かします。

## *Diff Bot*でSLAM（slam_toolbox）！？
いい感じそうです！
あまり歪みのない綺麗なマップが出来ました！

<iframe width="560" height="315" src="https://www.youtube.com/embed/xyJLHq5zHBg" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

## *Diff Bot*でナビゲーション（navigation2）！？

なんかうまくいってなさそうですね。
tf周りが怪しいです。
パラメータ調整については、ほとんど何もしてないです。

でも、なんか動いたのでヨシ！（ひどい）

<iframe width="560" height="315" src="https://www.youtube.com/embed/cwRRLqlM9TA" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

# まとめ
なんとか、**micro-ROS**と**ros2_control**を使って
ナビゲーションまでできました。

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

結局これ。。。
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">micro-ROS何もわかってない🥲 <a href="https://t.co/VY7wprPPQR">pic.twitter.com/VY7wprPPQR</a></p>&mdash; BEIKE (@BEIKE_RE) <a href="https://twitter.com/BEIKE_RE/status/1604526549117460480?ref_src=twsrc%5Etfw">December 18, 2022</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script> 

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

# 余談
本当は倒立振子を作る予定でした。

IMUのデータ（[imu_filter_madgwick後のデータ](http://wiki.ros.org/imu_filter_madgwick)）を**micro-ROS**経由で（Raspberry Pi 4から）受け取ると、レイテンシがあるのか、倒立の制御（NUCLEO-F446RE側）が間に合いませんでした。
そのため、今年はナビゲーションできるロボットをとりあえず作るかという感じになりました。

来年は**micro-ROS**使って、*Diff Bot*を倒立させたいです🔥