DonkeyCarをREALSENSEで作る

**※注意**
作業している間に、RealSense-part ブランチが、めでたくDevブランチにマージされました。
文中はRealSense-partになっていますが、Devブランチから最新を取ってきてください。
#やりたいこと
ちょっとROSとかに気をひかれている間に、DonkeyCarがステレオカメラで動くようになったらしい。

+ D435i　距離測定用
+ Jetson-nano 制御基板は最近いつもこれ

T265も動きそうだけど、DonkeyCarではあまり意味がなさそうなので、今回はパスで。


#リポジトリの確認

[Realsense D435およびD435i深度カメラのサポート](https://github.com/autorope/donkeycar/pull/544)

ステレオカメラの対応をしているけど、要はDepthをRGBにした画像を使って学習をしているので、周辺に障害物が多いほど有利になるはず。
逆に移動するギャラリーが多いと不利なことこの上ないと思われます。

###ブランチRealSense-partをmasterと比較

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/328520/06a8fad4-da55-a71a-3fc9-4dcda7e67af7.png)

いろいろ追加されていますね。
便利なoled.pyの表示や、serial_controller.pyではUSBシリアルから入力を引っ張る機能が実装されています。
robohat.pyは使ったことがないですが、便利なのかな？
そしてrealsense435i.pyの実装が今回の核ですね。ユーザー側で画像サイズを設定できないので、リーズナブルなサイズのモードを選んで取り込んでいるそうです。

```
# NOTE: Jetson Nano users should clone the Jetson Hacks project
#       https://github.com/JetsonHacksNano/installLibrealsense
#       and _build librealsense from source_ in order to get the python bindings.
#       ./buildLibrealsense.sh
#
```
Jetson HacksからRealsenseのソースを持ってくるようにとのこと。
この辺り、JetPack4.3を使った上で、こちらを参考にしてRealSenseの動作ができるところまでやるとよさそうです。
[「Jetson Nano 超入門」を読んでSLAMやってみた](https://qiita.com/t_kumazawa/items/13bd1d851de65977c7d4)
ROSを使うわけではないので、JetPack4.2.2にしなくてもOK。
逆にOpenCVは4.1を前提にしているみたいです。


![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/328520/00174b38-48f7-b4af-7905-e32ffea48170.png)

カメラはD435i前提の実装に見えます。D435だと動くのだろうか？（うちは問題ないけど）
MPU6050でのimuの学習はあまり効果がなかったらしいので、有効にするかどうか。
しないかもね。

#DonkeyCarのセットアップ

devブランチらしいので、そっちでセットアップしないといかんですね。
基本的なJetson-nanoとRealSenseのドライバは入っている状態で開始します。

```bash:jetson-nano
sudo apt-get update
sudo apt-get upgrade
sudo apt-get install build-essential python3 python3-dev python3-pip libhdf5-serial-dev hdf5-tools nano ntp

pip3 install virtualenv
python3 -m virtualenv -p python3 env --system-site-packages
echo "source env/bin/activate" >> ~/.bashrc
source ~/.bashrc

pip install numpy==1.16.4
```

なんか、エラー出ますね。でも、CV2は動くと思う。
**ERROR: uff 0.6.5 has requirement protobuf>=3.3.0, but you'll have protobuf 3.0.0 which is incompatible.**

pythonでCV2の確認

```python:test
import cv2
print(cv2.__version__)
```

`4.1.1'が表示されましたので、大丈夫だと思います。

```
git clone -b RealSense-part https://github.com/autorope/donkeycar.git
cd donkeycar
pip install -e .[nano]

```

なんか、pandasで異常が出るので、pipをダウンしてやり直し。

```
pip install --upgrade pip==19.0
pip install cython
pip install pandas — no-use-pep517
pip install --extra-index-url https://developer.download.nvidia.com/compute/redist/jp/v42 tensorflow-gpu==1.13.1+nv19.3
pip install -e .[nano]
donkey createcar --path ~/mycar
```

pandasのインストールにめちゃくちゃ時間がかかりました。

#myconfig.pyの設定

この辺りを有効にします。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/328520/f3400663-6a38-06dc-b979-04b7a727d198.png)

しかし、
`REALSENSE_D435_IMU = True` にすると、エラーが出て止まります。

```
cfg.CAMERA_TYPE D435
Traceback (most recent call last):
  File "manage.py", line 613, in <module>
    meta=args['--meta'])
  File "manage.py", line 92, in drive
    device_id=cfg.REALSENSE_D435_ID)
  File "/home/kuma/projects/donkeycar/donkeycar/parts/realsense435i.py", line 67, in __init__
    self.imu_pipeline.wait_for_frames()
RuntimeError: Frame didn't arrived within 5000
```

とりあえず、IMUなしで設定すると、何となく動いてそうです。
テストはモータ接続無しで、カメラだけつないでやっています。

#仮に学習させてみる

Jetson-nanoで自己完結で学習も基板で行って実行ができました。
ただし、Webで接続してローカルパイロットに切り替えると、異常が発生します。

`ValueError: Error when checking input: expected img_in to have shape (120, 160, 3) but got array with shape (240, 424, 3)`

つまり、画像サイズと学習の設定が不一致で、D435iの入力サイズに合わせよ、ということになります。
しかし、安直にmyconfig.hを以下のように書き換えると、動かなくなります。
Jetson-nanoのdonkeycarは立ち上げで固まり、ctrl+c も聞かなくなります。
解決方法は **学習用のPCやクラウド側の設定のみ、240*424にする**

PC側もリポジトリはdonkeycarのRealSense-partを引っ張ってきて使いました。
使っているものを消去して、入れなおしています。

```bash:PCのmyconfig.py
# #CAMERA
CAMERA_TYPE = "D435"   # (PICAM|WEBCAM|CVCAM|CSIC|V4L|D435|MOCK)
IMAGE_W = 424
IMAGE_H = 240
IMAGE_DEPTH = 3         # default RGB=3, make 1 for mono

# # Intel Realsense D435 and D435i depth sensing camera
REALSENSE_D435_RGB = True       # True to capture RGB image
REALSENSE_D435_DEPTH = True     # True to capture depth as image array
REALSENSE_D435_IMU = False       # True to capture IMU data (D435i only)
REALSENSE_D435_ID = None        # serial number of camera or None if you only have one camera (it will autodetect)

```

```bash:jetson-nanoのmyconfig.py
# #CAMERA
CAMERA_TYPE = "D435"   # (PICAM|WEBCAM|CVCAM|CSIC|V4L|D435|MOCK)
#IMAGE_W = 424
#IMAGE_H = 240
#IMAGE_DEPTH = 3         # default RGB=3, make 1 for mono

# # Intel Realsense D435 and D435i depth sensing camera
REALSENSE_D435_RGB = True       # True to capture RGB image
REALSENSE_D435_DEPTH = True     # True to capture depth as image array
REALSENSE_D435_IMU = False       # True to capture IMU data (D435i only)
REALSENSE_D435_ID = None        # serial number of camera or None if you only have one camera (it will autodetect)

```


これで学習させると動くようになります。

#ご本人様から指摘

こうやったら動いた！と、書いたらブランチを作ったご本人から指摘が。

>イメージ値は、データを収集した車とデータでモデルをトレーニングするpcで同じでなければなりません。したがって、車はデフォルトの160x120を使用していますが、PCは424x240を使用しています。 160x120を使用するようにPCを変更してから、自動パイロットを再トレーニングする必要があります。

だけど、その通りに実行すると、

```bash
new mode: local_angle
Traceback (most recent call last):
  File "/home/kuma/projects/donkeycar/donkeycar/vehicle.py", line 151, in start
    self.update_parts()
  File "/home/kuma/projects/donkeycar/donkeycar/vehicle.py", line 199, in update_parts
    outputs = p.run(*inputs)
  File "/home/kuma/projects/donkeycar/donkeycar/parts/keras.py", line 166, in run
    outputs = self.model.predict(img_arr)
  File "/home/kuma/env/lib/python3.6/site-packages/tensorflow/python/keras/engine/training.py", line 1096, in predict
    x, check_steps=True, steps_name='steps', steps=steps)
  File "/home/kuma/env/lib/python3.6/site-packages/tensorflow/python/keras/engine/training.py", line 2382, in _standardize_user_data
    exception_prefix='input')
  File "/home/kuma/env/lib/python3.6/site-packages/tensorflow/python/keras/engine/training_utils.py", line 362, in standardize_input_data
    ' but got array with shape ' + str(data_shape))
ValueError: Error when checking input: expected img_in to have shape (120, 160, 3) but got array with shape (240, 424, 3)
Shutting down vehicle and its parts...
```

と、いろいろやり取りしていると、以下のことが判明。

+ Depthファイルは現在学習に使用していない。**Yolo**に使う予定
+ 学習に使うのは160*120のRGBファイルなので、424*240のファイルを生成するのはNG
+ manage.pyでサイズを指定すると動くはずだが、D435iの初期化の時に、サイズが指定されていない。

取り合えず、manage.pyだけ修正すると動きそうです。
Yoloの導入は期待大ですが、今のところD435iを使う意味があまりなさそうです。

#ブランチRealSense-partの修正

```bash:myconfig.py
# #CAMERA
CAMERA_TYPE = "D435"   # (PICAM|WEBCAM|CVCAM|CSIC|V4L|D435|MOCK)
IMAGE_W = 160
IMAGE_H = 120
IMAGE_DEPTH = 3         # default RGB=3, make 1 for mono

# # Intel Realsense D435 and D435i depth sensing camera
REALSENSE_D435_RGB = True       # True to capture RGB image
REALSENSE_D435_DEPTH = True     # True to capture depth as image array
REALSENSE_D435_IMU = False       # True to capture IMU data (D435i only)
REALSENSE_D435_ID = None        # serial number of camera or None if you only have one camera (it will autodetect)

```

manage.pyも修正。D435の設定を行っているところ。

```python:manage.py_86行目周辺
    elif cfg.CAMERA_TYPE == "D435":
        from donkeycar.parts.realsense435i import RealSense435i
        cam = RealSense435i(
            width = cfg.IMAGE_W, height = cfg.IMAGE_H,
            enable_rgb=cfg.REALSENSE_D435_RGB,
            enable_depth=cfg.REALSENSE_D435_DEPTH,
            enable_imu=cfg.REALSENSE_D435_IMU,
            device_id=cfg.REALSENSE_D435_ID)

```

これで、従来と同レベルの学習ができるはずです。

#Depthの画像を学習に使う改造
このままだと、カメラをUSBステレオに改造しただけで、何も新規性がありません。
Yoloの対応が終わるまで、深度カメラでの学習をさせてみます。
だだっ広い倉庫で学習できるアメリカと違って、日本の会場はものが多いので、深度カメラはそこそこ働いてくれるはず。
逆に屋外だと、従来のRGBのほうが有利になることは間違いないと思われます。
とりあえずやってみましょう。

realsense435i.pyの画像取り込み部分に数行挟み込んで、深度がRGBのJPEGファイルになるように細工。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/328520/e9bb3a7f-dea1-3336-86e9-cf7e8cc3b74b.png)

**※後に出てくるリサイズ前の画像が保存されるバグは治したソースコード**

```python:realsense435i.py改造
        #
        # convert camera frames to images
        #
        if self.enable_rgb or self.enable_depth:
            # Align the depth frame to color frame
            aligned_frames = self.align.process(frames) if self.enable_depth and self.enable_rgb else None
            depth_frame = aligned_frames.get_depth_frame() if aligned_frames is not None else frames.get_depth_frame()
            color_frame = aligned_frames.get_color_frame() if aligned_frames is not None else frames.get_color_frame()

            # Convert depth to 16bit array, RGB into 8bit planar array
            depth_image = np.asanyarray(depth_frame.get_data(), dtype=np.uint16) if self.enable_depth else None

            # 深度カラーのデータを作る
            import cv2
            depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
            color_image = depth_colormap

            if self.resize:
            #    import cv2
                if self.width != WIDTH or self.height != HEIGHT:
                    self.color_image = cv2.resize(color_image, (self.width, self.height), cv2.INTER_NEAREST) if self.enable_rgb else None
                    self.depth_image = cv2.resize(depth_image, (self.width, self.height), cv2.INTER_NEAREST) if self.enable_depth else None
                if self.channels != CHANNELS:
                    self.color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2GRAY) if self.enable_rgb else None

```

DonkeyCarを動かすと、こんな感じで画像ファイルが取得できました。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/328520/975b39d3-f35a-8252-bd10-2e5c1d496092.png)

##ちょっとしたバグ
画像が取得できましたが、ディレクトリをよく見るとサイズの違うファイルが混じっていることがあります。
変換ができていないようです。
60HzでREALSESEを回して画像を更新し、それをDonkeyCarが20Hzで拾い上げているようです。
メンバ変数に変換前後の画像を書き込んでるので、タイミング次第でサイズが違うファイルを保存しています。

```python:realsense435i.py
#修正前
            # Convert depth to 16bit array, RGB into 8bit planar array
            self.depth_image = np.asanyarray(depth_frame.get_data(), dtype=np.uint16) if self.enable_depth else None
            self.color_image = np.asanyarray(color_frame.get_data(), dtype=np.uint8) if self.enable_rgb else None

                    self.color_image = cv2.resize(self.color_image, (self.width, self.height), cv2.INTER_NEAREST) if self.enable_rgb else None
                    self.depth_image = cv2.resize(self.depth_image, (self.width, self.height), cv2.INTER_NEAREST) if self.enable_depth else None




#修正後
            # Convert depth to 16bit array, RGB into 8bit planar array
            depth_image = np.asanyarray(depth_frame.get_data(), dtype=np.uint16) if self.enable_depth else None
            color_image = np.asanyarray(color_frame.get_data(), dtype=np.uint8) if self.enable_rgb else None

                    self.color_image = cv2.resize(color_image, (self.width, self.height), cv2.INTER_NEAREST) if self.enable_rgb else None
                    self.depth_image = cv2.resize(depth_image, (self.width, self.height), cv2.INTER_NEAREST) if self.enable_depth else None

```


そのうち修正版が出るはず。
ソースコードは作業がある程度進んだら、githubに上げる予定です。

#動かして様子を見る
実施中。手ごろな走行会に参加して、実績を報告したい。