「Jetson Nano 超入門」を読んでSLAMやってみた

#はじめに
いつもJotson関連の情報でお世話になっているからあげさんがJetsonNanoの本を書いたので、いつものように真似してみるよ。
（注意！JetPackは4.2.2を使ってね）

[Jetson Nano本のサポートページ](https://karaage.hatenadiary.jp/jetson-nano-book)

#教材のレベルも高い
MIDIや３Dカメラをつかうページでは、D435やポケミクが必要だったりで、さすが超入門。
どこのご家庭にも必ずあるJetsonNanoと違って、デバイス側の在庫がある人は少ないはず・・・だけど、Twitter見てるとみんな持っているから不思議。
ちなみに私のはD435じゃなくてD435iだからセーフ（謎）
![IMG_3749.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/328520/b83c3d72-bd4f-ad11-7915-d4a17ce09bf0.jpeg)


#JetPack 4.3
本が届く前にJetPack4.3がリリースされて、OpenCV4.1がプリインストールされた。
これで長々とビルドする必要がなくなった！
（注意！JetPack4.3だとSLAMが動かせなかったので、現状ではJetPack4.2.xを使って続行してください）

#Part7 ROSを使ってロボットの眼を作ってみよう をやってみる
いきなりすべての章を吹っ飛ばして第7章から行きます。
JetPackを64GのSDに焼いてスタート。
（第３章を参考にパフォーマンスやスワップメモリが設定してないとビルドで詰まります。）
親切なスクリプトを起動して、全自動でインストールしてもらいます。
はい、亀が出ます。
![IMG_3750.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/328520/ce4b577f-41d3-e0d2-5af6-6bae29b17d71.jpeg)


#RealSenseがバージョンアップされてパッチ不要に！
D435はパフォーマンスが上がらず、D435iは全く動かないデフォルトのJetsonNano。ひと晩かけて超めんどくさいパッチを当てていたのですが、JetPack4.3にバージョンアップされたのと時同じくして、インテルがRealSenseのドライバを改良してくれました。
サメのおじさんも大感激ですね。

[Install RealSense Camera in 5 minutes – Jetson Nano](https://www.jetsonhacks.com/2019/12/22/install-realsense-camera-in-5-minutes-jetson-nano/)

５分でインストールできるとサメのおじさんが豪語するとおり、あっという間に終わります。
以前では信じられないほど楽なうえ、パフォーマンスも上がっているように見えます。
7-3のカーネルにパッチを当てるところはパスできるので、超入門を読んでいる皆さんにはおススメ。

#？realsense_rosがビルドできない
これで楽にセットアップできると思いきや、catkin buildでエラー発生。
エラーメッセージから、realsense_rosはOpenCV3.xじゃないと動かないよ。とのこと( ﾉД`)ｼｸｼｸ…。

#JetPack4.3 → JetPack4.2.2 ひとつ戻る・・・
OpenCVの再ビルトとダウングレードなんぞやってられないので、別のSDカードにJetPack4.2.2を焼いて、再度サメのおじさんの手順でRealSenseをインストール。
ありがとう、JetsonHacks

```
git clone https://github.com/JetsonHacksNano/installLibrealsense
cd installLibrealsense
./installLibrealsense.sh
realsense-viewer
```

Viewerは順調に起動。
D435iはあっさり動きます。

```
./buildLibrealsense.sh
```

realsenseのSDKのビルドには時間が小一時間ほどかかりました。
スクリプトにお任せなので放置でOK。

#realsense-rosをビルド
```
cd ~/catkin_ws/src
git clone https://github.com/IntelRealSense/realsense-ros
cd realsense-ros
catkin build
```
最新のrealsense-rosのバージョンでビルドします。
JetPack4.3だとここで詰まりましたが、今回はあっさり通りました。

```
roslaunch realsense2_camera demo_pointcloud.launch
```

ポイントクラウドのデモも動きますが、やはり重いです。
realsense-viewerに見慣れていると、この遅さはいかがなものかと。
思っていたらアドバイスをいただきました。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">あとはlibrealsense2使うときにDecimation filter（間引き処理）使ってるかどうかですかね。realsense-viewerはデフォルトで使用してます。 <a href="https://t.co/kxKuo3AJbA">https://t.co/kxKuo3AJbA</a></p>&mdash; dandelion (@dandelion1124) <a href="https://twitter.com/dandelion1124/status/1211132711571611652?ref_src=twsrc%5Etfw">December 29, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script> 

確かにdecimation属性で間引きができそうです。
ROSの書式が難しかったのであとでやるとして、車体を用意します。

#車体を用意
仮想空間で動いているだけでは机上の空論、リアルワールドのボディが必要です。
![IMG_3774.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/328520/6c8d7549-8455-2c5d-b9b4-5febfbb18154.jpeg)

DokneyCarに改造していたTAMIYAのTT02Bを引っ張り出し、カメラマウントを単眼レンズからD435i+T265用に交換。
PWMドライバのPCA9685を載せてあるので、これを使ってモーターを動かします。
すでにDonkeyCarで動いた実績があるのでハードウェアは安心です。

#「D435iを使用したSLAM」
githubのrealsense-rosのwikiにこんな記事がありました。
[D435iを使用したSLAM](https://github.com/IntelRealSense/realsense-ros/wiki/SLAM-with-D435i)
これはまさに私の必要とするものではないか？

あまり深く考えず、この通りに実行すると、それっぽく動きました。
速度も遅くない、壁も作れる。
しかし、少し早く動かすとIMUがドリフトを起こすようです。
自己位置の推定がうまくいっていないということでしょう。

それでは、自己位置推定に長けたT265を使えば改善しそうです。

![IMG_3775.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/328520/17a740ac-6935-bdfc-df69-2435c96cea18.jpeg)

#チュートリアルにもどって基本的なところの確認
そもそも、ROSが何かよくわからないまま進めてきたので、チュートリアルレベルに戻る。
書籍「ROS2で始めよう次世代ロボットプログラミング」のROS1の章にD435iの使い方が記載されている。

```
roslaunch realsense2_camera rs_rgbd.launch
```

を実行すると、異常が出て止まってしまう。多分ドライバやlaunchファイルがインストールされていないのだろう。
（注意！あとで `sudo apt install ros-melodic-rgbd-launch`で不足分をインストールして実行できました）

realsense2_cameraについているデモを片っ端から実行してみる。

opensource_tracking.launch はそこそこいい速さで動く。（設定の変更が必要）
rs_rtabmap.launch はイメージも表示されるが、効能が分からない。pointcloudとの差は？

rs_camera.launch はopensource_tracking.launch から呼び出される。
rs_d435_camera_with_model.launch D435のイメージも表示される。かなりそれっぽい感じ。

・・・いろいろやってみましたが、SLAMがきっちりできている感じではありません。

#githubのissueを詳細に読んでみる
githubのlibRealseseのissueでは活発な意見が交わされていました。
こんな話が出てきました。

[D435でraspbery pi 3bでopensouce_tracking.launchを使用中の問題 ＃1044](https://github.com/IntelRealSense/realsense-ros/issues/1044)
>IMU-less D435を使用したSLAMマッピングの場合、SLAMソフトウェア「ORB_SLAM2」が適切な選択である可能性があります。以下のリンクには、ROSで使用するためのガイドがあります。
>

ORB_SLAM2とは？
[ORB_SLAM2](http://wiki.ros.org/orb_slam2_ros)

とりあえず、ソースをダウンロードして動かしてみます。
https://github.com/appliedAI-Initiative/orb_slam_2_ros
クローンしたらcatkin buildで再構築。
Jetson-nanoでもかなり時間がかかります。
（うちでは15分程度。シャドウミストレスが変身してからモモがダウンするまで）

#ORB_SLAM2を動かしてみる
>roslaunch realsense2_camera rs_rgbd.launch

で失敗するので、rgbd-launchパッケージを入れる。
>sudo apt install ros-melodic-rgbd-launch

ORB_SLAM2を動かしてみる
>roslaunch orb_slam2_ros orb_slam2_d435_rgbd.launch

これで表示させてみよう。

#表示してみる
やり方とか考え方が適当なので、詳しい方の突っ込み待ちです。

```
roslaunch realsense2_camera demo_pointcloud.launch
roslaunch orb_slam2_ros orb_slam2_d435_rgbd.launch
roslaunch realsense2_camera rs_rgbd.launch
```
この順番で別々のターミナルで実行する。
ポイントクラウドで画面が出てくることを確認してから、順番に立ち上げると、それっぽい画面で表示されました。

![IMG_3804.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/328520/400d26c8-7d31-d5c0-6ce7-393eb2a640d0.jpeg)

毎秒30fpsで動作しているようですが、CPUの負荷をほぼ使い切って動いています。
ゆっくり走らせるなら、この点群に当たらないように、動かせばよさそうです。

#次回の予定
点群を取ることがROSでできそうなので、マップを生成したり、衝突回避のようなものを実装していきます。

編集中の次の記事はこちら
[ROSでRealSenseを真面目に動かしてみる](https://qiita.com/t_kumazawa/items/1c25fb2e243b0c61baec)