Donkey car + Jetson nano + Jetson inferenceで信号認識をしてみた

# 初めに
はじめまして、ココネ株式会社でサーバー開発をしています、shiroです。
RC carにDonkeycarを活用して自動運転車両を製作しましたが、今回はその流れの一つである
DonkeycarライブラリにJetson nano + Jetson inferenceを適用して物体認識した話をしようと思います。

:::note info
本稿は信号認識内容中心になっております。
Donkeycar、車両のアーキテクチャや制御に関してはまたの機会にお話しできればと思います。
また、全てのコードはJetson nanoの**Ubuntu18.04**環境で実行されたものです。
:::

## Donkeycarについて
Donkeycar (https://github.com/autorope/donkeycar) はPythonのモジュラー型の自動運転ライブラリです。
RC Car基盤の車体にシングルボードコンピュータを利用してカメラで認識した画像をAIが学習してモデルを生成します。
そのモデルをベースとして車は車線を認識して自動で走ります。
自動運転は関連の研究所や企業などで多くの費用を投資して行い、一般的に接するのは難しいと感じられますが、Donkeycarは周りで簡単に手に入れられる素材で自動運転を体験できるようにしてくれるのが長所です。
Donkeycarはキットで一括購入してすぐに組み立てを始めることもできますし、ある程度組み立てに慣れていれば、自分だけのDonkeycarを作るのももう一つの楽しみです。

## Jetson nano、Jetson inferenceについて
Jetson nanoはnvidiaのAI、Deep learningなどに特化したシングルボードコンピュータです。

https://developer.nvidia.com/embedded/jetson-nano-developer-kit

実際のスペックをRaspberry Pi 4と比較しました。
||Raspberry Pi 4|Jetson nano|
|------|---|---|
|CPU|Quad-core ARM A72 @ 1.5Ghz|Quad-core ARM A57|
|GPU|Broadcom VideoCore VI|128-core Maxwell|
|Memory|2, 4, 8GB LPDDR4|4GB 64-bit LPDDR4 25.6 GB/s|
|Storage|microSD (not included)|microSD (not included)|
|Video Encode|H264 1080p 30|4K @ 60 / 4x 1080p @ 30 / 9x 720p @ 30 [H.264/H.265]|
|Video Decode|H.265(4Kp60), H.264(1080p60)|4K @ 60 / 2x 4K @ 30 / 8x 1080p @ 30 / 18x 720p @ 30[H.264/H.265]|
|Power|5V 3A|5V 3A / 5W / 10W|
|Camera|2-lane MIPI CSI camera port|2x MIPI CSI-2 DPHY lanes|
|Connectivity|Gigabit Ethernet / Wifi 802.11ac|Gigabit Ethernet, M.2 Key E|
|Display|2x micro-HDMI (up to 4Kp60)|HDMI 2.0 or DP1.2 / eDP 1.4|
|USB|2x USB 3.0, 2x USB 2.0|4x USB 3.0, USB 2.0 Micro-B|
|I/O|I2C, SPI, UART, I2S, GPIOs|I2C, SPI, UART, I2S, GPIOs|
|Price|$35 USD ~ |$99 USD|

大体のスペックはほぼ似ていますが、
CPUはRaspberry Pi 4の方が、GPUはJetson nanoの方が優れていますね。
動画処理においてはGPU性能の高いJetson nanoの方が適切だと思い、Jetson nanoを選びました。

### Jetson Inference
>inference and realtime DNN vision library for NVIDIA Jetson Nano/TX1/TX2/Xavier NX/AGX Xavier/AGX Orin.
https://github.com/dusty-nv/jetson-inference


Jetson inferenceはPython, C++に対応するreal-time DNN vision ライブラリです。
このライブラリを活用して信号認識をします。

## カメラの動作確認
自分が使ったカメラは **Raspberry Pi 4 camera(CSI Camera)** です。
カメラの種類によってpipeline設定が異なるため以下のコードで対応できない可能性があります。
カメラの動作確認のためにテストコードを作成します。
```python:SimpleCamera.py
import cv2

def gstreamer_pipeline(
    capture_width=640,
    capture_height=480,
    display_width=640,
    display_height=480,
    framerate=60,
    flip_method=0,
):
    return (
        "nvarguscamerasrc ! "
        "video/x-raw(memory:NVMM), "
        "width=(int)%d, height=(int)%d, "
        "format=(string)NV12, framerate=(fraction)%d/1 ! "
        "nvvidconv flip-method=%d ! "
        "video/x-raw, width=(int)%d, height=(int)%d, format=(string)BGRx ! "
        "videoconvert ! "
        "video/x-raw, format=(string)BGR ! appsink"
        % (
            capture_width,
            capture_height,
            framerate,
            flip_method,
            display_width,
            display_height,
        )
    )

def cameraTest():
    cap = cv2.VideoCapture(gstreamer_pipeline(), cv2.CAP_GSTREAMER)
    if cap.isOpened():
        window_handle = cv2.namedWindow("CSI Camera", cv2.WINDOW_AUTOSIZE)
        # Window
        while True:
            success, img = cap.read()
            cv2.imshow("CSI Camera", img)
            cv2.waitKey(1)
            # This also acts as
            keyCode = cv2.waitKey(30) & 0xFF
            # Stop the program on the ESC key
            if keyCode == 27:
                break
        cap.release()
        cv2.destroyAllWindows()

if __main__ == "__main__":
    cameraTest()
```

テスト結果は以下のようになります。
![11.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2988862/958d11bd-790e-5145-c3e3-0748fbe46c64.png)
夜間赤外線カメラモジュール（[NOIR Camera Board /w CS mount Lens [B0036]](https://www.arducam.com/product/arducam-5mp-cs-mount-noir-camera-raspberry-pi-b0036/)）を使ったので画像全体的に赤い感じがしますが、後で画像を白黒に変える予定なので大丈夫です。
`cv2.VideoCapture(gstreamer_pipeline(), cv2.CAP_GSTREAMER)`でgstreamerのpipelineを設定します。
`cv2.namedWindow("CSI Camera", cv2.WINDOW_AUTOSIZE)`でウィンドウを生成して
`cv2.imshow("CSI Camera", img)`でカメラで撮影したイメージを画面上に結果を出力します。

続いて物体認識処理を適用します。

## 物体認識テスト
modelとJetson inferenceを使うためにJetson inferenceをインストールします。
以下のURLから詳細をご覧いただけます。
https://github.com/dusty-nv/jetson-inference/blob/master/docs/building-repo-2.md
```shell_session
$ sudo apt-get update
$ sudo apt-get install git cmake libpython3-dev python3-numpy
$ git clone --recursive https://github.com/dusty-nv/jetson-inference
$ cd jetson-inference
$ mkdir build
$ cd build
$ cmake ../
$ make -j$(nproc)
$ sudo make install
$ sudo ldconfig
```
SimpleCamera.pyのcameraTest()に以下のコードを追加します。
```diff_python:SimpleCamera.py
+ import jetson.inference
+ import jetson.utils

...

def cameraTest():
+   net = jetson.inference.detectNet("ssd-mobilenet-v2", threshold=0.5)
    cap = cv2.VideoCapture(gstreamer_pipeline(), cv2.CAP_GSTREAMER)
    if cap.isOpened():
        window_handle = cv2.namedWindow("CSI Camera", cv2.WINDOW_AUTOSIZE)
        # Window
        while True:
            success, img = cap.read()
+           imgCuda = jetson.utils.cudaFromNumpy(img)
+           detections = net.Detect(imgCuda)
+           if len(detections) > 0 :    
+               for d in detections:
+                   x1, y1, x2, y2 = int(d.Left), int(d.Top), int(d.Right), int(d.Bottom)
+                   className = net.GetClassDesc(d.ClassID)
+                   if className:
+                       print("detected {:d} objects in image".format(len(detections)))
+                       print(d)
+                       cv2.rectangle(img, (x1, y1), (x2, y2), (255, 0, 255), 2)
+                       cv2.putText(img, className, (x1+5, y1-15), cv2.FONT_HERSHEY_DUPLEX, 0.75, (255, 0, 255), 2)
+               else:
+                   break

            cv2.imshow("CSI Camera", img)
            cv2.waitKey(1)
            # This also acts as
            keyCode = cv2.waitKey(30) & 0xFF
            # Stop the program on the ESC key
            if keyCode == 27:
                break
        cap.release()
        cv2.destroyAllWindows()
```
`jetson.inference.detectNet()`で物体認識に使うmodelを設定します。
modelとしては基本的に使える**SSDMobilenetV2**を使います。
**SSDMobilenetV2**はOne-stageのオブジェクト検出modelで、スリムなネットワークと新しい深さに分離可能なコンボリューションで人気を集めています。
`jetson.utils.cudaFromNumpy(img)`でNumpy arrayをcuda形式に変換します。
`net.Detect(imgCuda)`でcuda形式の画像から物体認識します。
その結果から座標を得てcv2で確認できるように印をつけます。
実行結果は以下のようになります。
![68747470733a2f2f71696974612d696d6167652d73746f72652e73332e61702d6e6f727468656173742d312e616d617a6f6e6177732e636f6d2f302f323938383836322f34653135346665662d363132622d303036382d643238372d6162306638336137373762352e706e67.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2988862/328f3559-0070-5db6-0550-b54d1e061a5b.png)
物体認識されるのを確認できました。
これで信号認識するための準備が整えました。

## 信号認識の流れ
**SSDMobilenetV2**は信号灯自体は認識しますが、何の信号かまでは把握できないので、直接信号を把握する必要があります。
信号認識のアルゴリズムは以下のようにしました。
1. 信号灯認識後ROI指定
1. ROI内の画像をgrayscale化
1. 各信号ごとヒストグラム化して明るい信号を把握
1. 結果をもとに車両を制御

必要のない演算を避けるために信号灯を認識した座標からROIを取得します。
そして`cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)`でGrayscale化してその画像をヒストグラム化して信号の明るい範囲を比較します。

テストすると以下のようになりました。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2988862/064ce2ab-b99b-d8f8-bb83-869c00497617.png)
Donkeycarのmotor制御ファイルに信号処理を追加します。
motor種類によって処理が入るクラスが異なります。
Donkeycarのソースでpartsフォルダの中にmotorを制御する`actuator.py`がありますが、使用するmotorによって信号処理が入るクラスが異なります。
自分が使った車両はPWM制御だったので信号が赤だった時にはpulseとして０を投げる必要がありました。
```python:parts/actuator.py
class PWMThrottle:
    ...
    def run_threaded(self, throttle, traffic, run_pilot, pause):
        ...
        # 信号が赤だった時
        if self.traffic:
            self.pulse = self.zero_pulse # 0 pulseを投げて車両を止める
```

### 自動運転モードでの信号認識テスト結果
![ezgif.com-gif-maker.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2988862/86958cf1-0612-8166-e628-7606d8c873b6.gif)
赤信号の時に止まってから走るのを確認できました。

# 最後に
実機でモニタリングした時に動画のfpsは15~20でした。
それで信号認識のタイミングを合わせるために車のスピードを下げる必要がありました。
これらに対しては何らかの改善が必要そうですね。
自動運転に興味のある方はDonkeycarで試してみてはいかがでしょうか。