Jetson×ROS×RICOH Thetaで動かすお手軽Deeplearning

# JetsonとROSで動かすお手軽Deeplearning
こんにちは、ドコモの北出です。
普段は画像認識を使って「(物理的に)動くもの」を作っています。
人が暮らす空間上で画像認識を動かすには、処理にそれなりのリアルタイム性が求められます。

最近では超小型GPU搭載マシンとしてNVIDIA社製のJetsonが身近な存在になっています。この記事では、

* Jetson
* ROS
* darknet YOLO
* RICOH Theta

のようなキーワードで、JetsonとRICOH Thetaを組み合わせて物体認識をリアルタイム動作させるところまでを説明します。

## はじめに

### Jetsonとは
Deeplearningの各種エンジンはNVIDIA社製のGPU及びライブラリ(CUDAやCUDNN)を使って高速に動作させられるよう最適化されていることが多いですが、最近はその機能が手のひらサイズで動作させられるようになってきました。
その製品が、Jetsonシリーズです。

https://www.nvidia.com/ja-jp/autonomous-machines/embedded-systems-dev-kits-modules/

詳しい説明はNVIDIA社のページをご覧いただければと思いますが、これまでJetson TX1/TX2、最近ではJetson Xavierといった製品がリリースされています。

今日の記事は、Jetson TX1にJetpack3.3(CUDA/CUDNN/OpenCV4Tegra)をインストールした環境を前提に書かれております。

### ROSとは
Robot Operation System(ROS)はロボットシステムを構築するのに便利なソフトウェアプラットフォームです。OSという名前が付きつつも、実際にはUbuntuなどのコンピュータ用のOSの上で動作するミドルウェアです。
ここでは詳しい説明を省略しますが、

* 必要な機能をノードとして実装できる
* ノード間のメッセージはTopicとして表現され、独自のメッセージ形式も簡単に作れる
* 各ノードはPublisher/Subscriberとして動作させることができ、Subscriberでは目的のTopicが現れたときだけ動作させることができる

といった特徴があります。
以下の記事では、ROS kineticとcatkin_makeシステムを準備した環境で説明を進めます。

* [ROS Kinetic の Ubuntu へのインストール](http://wiki.ros.org/ja/kinetic/Installation/Ubuntu)
* [catkin の workspace を作る](http://wiki.ros.org/ja/catkin/Tutorials/create_a_workspace)

## リアルタイム画像認識を体験してみる！
高速動作が話題となり近年人気急上昇中の物体認識システム、[YOLO](https://pjreddie.com/darknet/yolo/)をROS上で動作させてみます。といっても、yoloのROS実装版として[darknet_ros](https://github.com/leggedrobotics/darknet_ros)が存在しますので、今回はそちらを活用します。

* カメラドライバノード([uvc_camera](https://github.com/ros-drivers/camera_umd))を使ってWebカメラからの画像をPublish
* darknet_rosノードを使ってカメラ画像をSubscribeし物体認識結果を表示

という感じです。

### カメラドライバノードのセットアップ
下記のようにして、カメラドライバノードをセットアップします。
「Old ROS Webcam Drivers」だよとアナウンスされてはいますが、目立った問題もなく安定して動作するため今回はこちらを使いました。

```
$ sudo apt-get install ros-kinetic-camera-info-manager ros-kinetic-image-view libv4l-dev
$ cd catkin_workspace/src
$ git clone https://github.com/ros-drivers/camera_umd.git
$ cd ../
$ catkin_make -DCMAKE_BUILD_TYPE=Release
```
インストールが終わったら、カメラの解像度を変更しておきます。
編集するファイルは「catkin_ws/src/camera_umd/uvc_camera/launch$ emacs camera_node.launch」です。
変更前：

```
<launch>
  <node pkg="uvc_camera" type="uvc_camera_node" name="uvc_camera" output="screen">
    <param name="width" type="int" value="320" />
    <param name="height" type="int" value="240" />
    <param name="fps" type="int" value="30" />
```

変更後：

```
<launch>
  <node pkg="uvc_camera" type="uvc_camera_node" name="uvc_camera" output="screen">
    <param name="width" type="int" value=“640” />
    <param name="height" type="int" value=“480” />
    <param name="fps" type="int" value="30" />
```

ちなみに、.launchファイルにはノード起動のための設定を記載することができ、「roslaunch hogehoge.launch」と実行するだけでノードを簡単に起動することができるので便利です。

### darknet_rosのセットアップ
こちらも、インストラクション通りインストールを進めます。

```
$ cd catkin_workspace/src
$ git clone --recursive $https://github.com/leggedrobotics/darknet_ros.git
$ cd ../
$ catkin_make -DCMAKE_BUILD_TYPE=Release
```
…どうも、Jetpack3.3との組み合わせで実行するとCUDAは有効なもののCUDNNが無効になるらしく、実行時に「exit code -11」でこけてしまいました。

そこで、catkin_makeでコンパイルする際にCUDNNも有効化させるために下記の通りCMakeLists.txtを編集しました。

```
nvidia@tegra-ubuntu:/media/nvidia/workspace/catkin_ws/src/darknet_ros/darknet_ros$ diff CMakeLists.txt.original CMakeLists.txt -u
--- CMakeLists.txt.original	2018-11-25 13:01:51.674430675 +0900
+++ CMakeLists.txt	2018-11-25 13:06:21.856462808 +0900
@@ -61,6 +61,10 @@
 # Enable OPENCV in darknet
 add_definitions(-DOPENCV)
 add_definitions(-O4 -g)
+add_definitions(-DCUDNN)
+include_directories(/usr/lib/aarch64-linux-gnu)
+
+

 catkin_package(
   INCLUDE_DIRS
@@ -145,6 +149,7 @@
     cudart
     cublas
     curand
+    cudnn
     ${Boost_LIBRARIES}
     ${OpenCV_LIBRARIES}
     ${catkin_LIBRARIES}
```
セットアップが終わったら、uvc_cameraノードからのトピックを受信できるよう、カメラトピック名を変更しておきます。
編集するファイルは「catkin_ws/src/darknet_ros/darknet_ros/config/ros.yaml」です。
変更前：

```
subscribers:

  camera_reading:
    topic: /camera/rgb/image_raw
    queue_size: 1
```
変更後：

```
subscribers:

  camera_reading:
    topic: /image_raw
    queue_size: 1
```
### 実際に動かしてみる
ターミナルを２つ起動して、それぞれ下記のように.launchファイルを起動します。

```
catkin_ws/src/camera_umd$ uvc_camera/launch/camera_node.launch
```
```
catkin_ws/src/darknet_ros/darknet_ros$ launch/darknet_ros.launch
```
ウィンドウが立ち上がって、推論結果が表示されたのではないでしょうか。

## Webカメラの代わりにRICOH Thetaを使ってみる
せっかくですので、今回は普通のWebカメラではなくRICOH THETA SからWi-Fi経由でライブストリーミングされた画像を使って推論できるようにしてみます。

上記で「uvc_camera」を使っていたところ、代わりにRICOH THETA Sからストリームした画像を使って代替します。

### Theta APIについて
RICOH THETAシリーズには、Wi-Fi経由で機器を操作したり、映像を転送したりする便利なAPIが公開されています。[(THETA Developers)](https://developers.theta360.com/ja/docs/)

今回はライブプレビュー画像を取得したいので、「_getLivePreview」というAPIを利用します。API利用時には、THETAをアクセスポイントとしてJetsonをWi-Fi接続させます。
処理の流れは、次の通りです。

* Thetaとの通信を開始する(startSession)
* ライブプレビューを取得する(_getLivePreview)
* Thetaとの通信を終了する(closeSession)

ライブプレビューについては、プログラムを終了させるまでストリームが流れ続けるため、ループ処理を行ないます。

ちなみに、THETA Sからストリームを取得する方法としては、

* USB UVC経由
* HDMI経由
* Wi-Fi経由

の３種類があります。
ただし、Wi-Fi経由以外の方法で取得したストリームはDual fisheye形式(魚眼レンズの映像を２つ並べたもの、スティッチされていない)になりますので、今回は唯一エクイレクタングラー形式での取得ができるWi-Fi経由のストリームを利用します。
※USB UVCドライバ経由でのストリーム取得についてもRICOH社が提供するドライバを利用するとエクイレクタングラー形式の画像が得られるが、Linux用のドライバがなく、Jetson上の動作は難しいと思われる。

THETA Sの場合、Wi-Fi経由のライブプレビューは640*360@15fpsです。（最新のTHETAだとここの性能が上がっているようなので、ぜひ入手したい。。）

### THETAからのライブストリームをROSのノードとしてPublishしてみる
いよいよ、rosで動作するTHETAライブストリームのPublisherを実装します。

まずは、OpenCVの他、ROSのノードとして動作させるために必要なライブラリをインポートします。

```python
import io
import json
import inspect
import time
import cv2
import numpy as np
import requests
import signal
import sys

import roslib
import rospy
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
```

続いて、THETAにアクセスするため、API URLやROSノードとしての機能に必要な設定を行ないます。「rospy.Publisher("image_raw", Image, queue_size=1)」の部分にて、Publishするトピックのアドレス(ここでは/image_raw)やトピックの種類(Image)を設定しています。
その他、キーボード入力による終了に対応できるよう、ハンドラを定義しています。

```python
_DEBUGMODE = True
cam = 'http://192.168.1.1:80'
url = cam+'/osc/'
exe = url+'commands/execute'

# ROS publisher
image_pub = rospy.Publisher("image_raw", Image, queue_size=1)
bridge = CvBridge()

def handler(signal, frame):
        print("Shutting down...")
        sys.exit(0)

# API ver2.0
```

THETAのAPIにアクセスするためのメソッドを定義します。
セッションの開始、終了です。
startSessionでは、セッションIDやステータスなどの情報がjsonとして返却されます。closeSessionでは、セッションIDを指定してセッションの終了を行ないます。

```python
def startSession():
    j = {'name': 'camera.{}'.format(inspect.currentframe().f_code.co_name), 'parameters': {}}

    r = requests.post(exe, data=json.dumps(j))
    if _DEBUGMODE:
        print("---startSession---")
        print(r.json())
    return r.json()


def closeSession(id):
    j = {'name': 'camera.{}'.format(inspect.currentframe().f_code.co_name), 'parameters': {'sessionId': '{}'.format(id)}}

    r = requests.post(exe, data=json.dumps(j))
    if _DEBUGMODE:
        print("---closeSession---")
        print(r.json())
    return r.json()

```
メインディッシュとなる、ライブプレビュー取得です。
requests.postの結果、JPEGデータがバイト文字列として返ってきます。
バイト文字列の切れ目を検索し、cv2.imdecodeを利用してOpenCVで取り扱い可能なデータへ変換しています。
ここで大切なのが、Wi-Fi経由でのストリームの場合稀にJPEGデータが復元できないことがあるので、エラー処理を入れて対処しています。

image_pub.publishの部分で、OpenCV形式のイメージをROS Image型のイメージに変換し、Publishを実行します。

```python
def _getLivePreview(id):
    global image_pub
    global bridge
    j={'name':'camera.{}'.format(inspect.currentframe().f_code.co_name),
    'parameters': {'sessionId': id}}
    print(j)
    r=requests.post(exe, data=json.dumps(j), stream=True)
    bytes = b''
    for byteData in r.iter_content(chunk_size=1024):
        bytes += byteData
        a = bytes.find(b'\xff\xd8')
        b = bytes.find(b'\xff\xd9')
        if a != -1 and b != -1:
            jpg = bytes[a:b+2]
            bytes = bytes[b+2:]
            try:
                i = cv2.imdecode(np.fromstring(jpg, dtype=np.uint8), cv2.IMREAD_COLOR)
                image_pub.publish(bridge.cv2_to_imgmsg(i, "bgr8"))
            except:
                print("OpenCV Error")
            '''
            cv2.imshow('live preview', i)
            if cv2.waitKey(1) == 27:
                cv2.destroyAllWindows()
                return
            '''
            #rospy.spin()
    return r.json()

```
これまで記載してきたメソッドを順に呼び出し、実行します。

```python
def main():
    signal.signal(signal.SIGINT, handler)
    # ROS settings
    rospy.init_node('theta_livePreview_publisher', anonymous=True)

    # session
    d = startSession()
    if d['state'] == 'error':
        return closeSession('SID_001')

    # api and options
    id = d['results']['sessionId']
    img = _getLivePreview(id)

    # session
    closeSession(id)


if __name__ == '__main__':
    main()

```

### 実行してみる！
THETA SとのWi-Fi接続が確立され、darknet_rosが実行された状態で、上で実装したTHETAストリームPublisherを実行します。
下記のように、エクイレクタングラー形式の画像と推論結果が表示されるのではないでしょうか。

![thetaApp_bre.png](https://qiita-image-store.s3.amazonaws.com/0/320418/aa33c8be-259d-5538-89ce-1b637491aa5e.png)


歪んだ画像ですが、思ったよりうまく推論されますね。
機会があれば、この辺りの補正方法を共有したいと思います。

### その他、便利なROSコマンド
ROSを使った開発をする際に便利なコマンドを記載しておきます。
上記プログラムを実行した状態で、下記のコマンドを試してみてください。

現在ROS上でやりとりされているトピック一覧

```
$ rostopic list
/darknet_ros/bounding_boxes
/darknet_ros/check_for_objects/cancel
/darknet_ros/check_for_objects/feedback
/darknet_ros/check_for_objects/goal
/darknet_ros/check_for_objects/result
/darknet_ros/check_for_objects/status
/darknet_ros/detection_image
/darknet_ros/found_object
/image_raw
/rosout
/rosout_agg
/statistics
```

特定のトピックの中身を確認

```
$ rostopic echo /darknet_ros/bounding_boxes
header:
  seq: 7828
  stamp:
    secs: 1543134096
    nsecs: 265472227
  frame_id: "detection"
image_header:
  seq: 475
  stamp:
    secs: 0
    nsecs:         0
  frame_id: ''
bounding_boxes:
  -
    Class: "person"
    probability: 0.66122251749
    xmin: 62
    ymin: 156
    xmax: 84
    ymax: 199
  -
    Class: "person"
    probability: 0.823418438435
    xmin: 132
    ymin: 109
    xmax: 313
    ymax: 303
  -
〜省略〜
```

ROSノードの接続状況を確認する

```
$ rosrun rqt_graph rqt_graph
```
<img width="750" alt="rosgraph.png" src="https://qiita-image-store.s3.amazonaws.com/0/320418/aa613b0f-4c54-fd13-fdc4-1647254249f6.png">
ROS上で起動しているノードと、その間でやり取りされているトピックが可視化されます。

## まとめ
いかがだったでしょうか。ROSと画像認識を組み合わせたロボット開発の参考になれば幸いです。
機会があれば、全天球カメラ画像に対する画像認識処理やロボット本体の実装についても共有できればと思います。