ROSでIntel Realsenseカメラから深度情報を取得する(ROS+Python)

UbuntuにてPython+ROSを使ってIntel Realsenseカメラから深度情報を取得する方法。

ROS, Intel Realsenseカメラのセットアップ方法は前回の記事参照。
[UbuntuでIntel Realsense D415を使えるようにするまで(ROSあり・なし両方)](https://qiita.com/kei_mo/items/c0387b7d277948451881)

今回はRealsenseカメラからRGB画像情報、深度情報をROS+Pythonを使って取得していく。
Jupyter notebookからROSを操作できるJupyter ROSを使用したが、つまるポイントがいくつかあったため、記す。
また次回はJupyterを使わない方法を試していく。

目次
1. セットアップ 
2. ROS Publisher, Subscriber, topicについて
3. PythonでSubscriberを書く

# 環境
Ubuntu 18.04
ROS Melodic
Python 3.6 (Anaconda)
Realsense D415

# 1. セットアップ
## 1.1 rospy
PythonでROSを扱えるようにするために[rospy](http://wiki.ros.org/rospy)を用いる。
インストールはpipにて行う。  
```pip install rospy```

インストール後以下のコードを実行するとエラーメッセージがでた。エラーメッセージが出る原因はいくつかあるみたいだが、自分の場合は```pip install rospkg```によりrospkgインストールすることで解決できた。

``` python
import rospy
```

## 1.2 jupyter-rosのセットアップ
今回はROSの結果をすぐに確認するためにjupyterからROSを操作する方法を試した。jupyterからの操作は標準的なものではなく開発途中であるため動作が安定しない。 
https://github.com/RoboStack/jupyter-ros

インストール方法はgithubに載っている。
https://github.com/RoboStack/jupyter-ros#installation-and-dependencies

``pip install jupyter-ros``
``jupyter nbextension enable --py --sys-prefix jupyros``

# 2. ROS Publisher, Subscriber, topicについて
ROSはデータをtopicとしてやりとりする。Topicを作り発信するnodeをpublisher, 受け取るnodeをsubscriberと呼ぶ。 
今回はrealsenseが画像のtopicをpublishし、Pythonで定義したnodeがそのtopicをsubscribeする。
## realsense立ち上げ
まずはrealsenseのnodeを立ち上げる。
ターミナルにて 
``roslaunch realsense2_camera rs_camera.launch``

## Topicを確認する
次にrealsenseからどのようなtopicがpublishされているか確認する。これにはrostopicコマンドを使用する。 
別のターミナルにて 
``rostopic list``
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/190972/ad453e7f-c1f8-92aa-0d4a-c97bd20725ac.png)

relasense-rosでも一部を確認できる
https://github.com/IntelRealSense/realsense-ros#usage-instructions

それぞれのtopicは型が決まっており、後にtopicをsubscribeするときに型を指定する必要がある前、確認する。 
これには`rostopic type`コマンドを使用する。
例えば/camera/color/image_raw/compressedの型を確認するには以下のようにする。
``rostopic type /camera/color/image_raw/compressed ``

sensor_msgs/CompressedImageという型であることがわかる。


| topic name | type |
|---|---|
|/camera/color/image_raw | Image|  
|/camera/color/image_raw/compressed | CompressedImage|  
|/camera/depth/image_rect_raw | Image|  
|/camera/depth/image_rect_raw/compressed | CompressedImage|  


# 3. PythonでSubscriberを書く
今回はJuypterでsubscriberを書く方法について記す。いくつかつまずいたので次回はJupyterを使わない方法を試す。  
ROS+PythonのチュートリアルにはJupyterを使わない方法について記されていて参考になる。 
http://wiki.ros.org/ja/ROS/Tutorials/WritingPublisherSubscriber%28python%29

**rospyとjupyrosのインポート**

```python

import rospy as rp
import jupyros as jr
```

**topicの型のインポート**  

```python 
from sensor_msgs.msg import CompressedImage, Image
from std_msgs.msg import String
```

**Nodeの立ち上げ**

```python
rp.init_node('test')
```

**Subscriberの準備**
callback関数(受けた入力を処理する関数)を定義する
受け取ったデータをnumpy arrayに変換する
用途によってはここから更に処理することが可能 

```python
def call_back(ros_data):
    np_arr = np.fromstring(ros_data.data, np.uint8)
    print(np_arr)
```

**Subscribeする** 
``` jr.subscribe(topic name, topic type, call back function)```


```python 
jr.subscribe('camera/depth/image_rect_raw/compressed',CompressedImage,call_back)
```

これによりnumpy arrayがプリントされるようになれば成功。

**jupyter rosの不具合修正**
しかし自分の場合はエラー以下のようなエラーが出た。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/190972/7cb42e39-b62b-c0a2-5877-fb4d1a698c5f.png)

どうやらjupyros pubsub.pyの25行目が悪さをしているらしい。
たぶんdictに入れるkeyの問題？
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/190972/0064cd6f-fbe9-994c-43a5-d7201e3ecef6.png)
試行錯誤の結果、パッケージの24-25行目を以下のように直接編集したらうまく動くようになった。
根本的な解決にはなっていないような気がするが、今の所これでうまく行っている。

```python
        if thread_name != 'MainThread':
            # output_registry[threading.currentThread().getName()].append_stdout(msg)
            self.original.write(msg)
```


また本題とはずれるが、pipでインストールできるバージョンでは49-50行目が以下のようになっているが、

```python
if subscriber_registry.get(topic): 
    raise Error("Already registerd...") 
```

これは正しくは以下のとおりである。githubでは修正されている。  

```python
if subscriber_registry.get(topic): 
    raise RuntimeError("Already registerd...") 
```


自分の場合はこれによりrealsenseからめでたく画像、深度データを取得できるようになった。

# 次回
今回はJupyterでROSを扱うことを試みたため苦戦した。次回はJupyterを使わずにデータを受け取ることを試す。