D435からの画像をrvizで表示させる

#目的
今回はRealSenseのD435を使って、ROSのrvizやウインドウ上でカメラ画像を表示させるということをしたいと思います。

# Intel RealSense Depth Camera D435
Intel RealSense Depth Camera D435はカラー画像に加えてdepth画像も取得できる三次元カメラです。
ROSにも対応しているのでロボットのカメラセンサとしても使用することができます。
[Depth Camera D435の公式ページ](https://www.intelrealsense.com/depth-camera-d435/)

#私の環境
| モノ | 環境 |
|:-----------:|:------------:|
| OS | Ubuntu 18.04.4 |
| ROS | melodic |
| Depth Camera | RealSense D435 |
| realsense-viewer |v2.32.1|


# 参考
基本的にはkaraage先生の記事を参考にしています。
[Jetson NanoでIntel Realsense D435を使う方法（ROS対応）](https://qiita.com/karaage0703/items/cb9bbe1b4f2504f8ad0d#intel-realsense%E3%81%AEros%E5%AF%BE%E5%BF%9C)

## Librealsenseのインストール
D435を使えるようにするためLibrealsenseのインストールを行います。
以下はJetson Nano用のインストール方法です。
<details><summary>インストール方法</summary><div>

```shell
# githubよりclone
cd & git clone https://github.com/jetsonhacksnano/installLibrealsense
cd ~/installLibrealsense

# 実行してインストール
./installLibrealsense.sh
```

</div></details>

##ビューアでカメラ画像が表示されるか確認する
ビューアを開いて画像を表示させてみます。

```shell
# realsenseのビューアを開く
realsense-viewer
```

![view.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/232369/378eb2b1-117d-9877-b2d0-de8740bf049a.png)

正常なら左側のStereo ModuleとRGB Cameraの項目をonにしてカメラ画像を表示させることができると思います。

確認ができたらビューアを閉じておきます。

#ROS対応
ここからはRealSenseがROS対応しているのでROS対応させてrvizを表示させます。
※（ROSのインストールについては省略）

## 作業用ファイルの作成
ROS対応したIntelRealSenseの作業用ファイルはROS導入時に作ってると思いますので省略しています。

```shell
# githubからcloneしてくる
cd ~/catkin_ws/src
git clone https://github.com/IntelRealSense/realsense-ros
cd realsense-ros
git checkout -b 2.2.3 2.2.3
```

## 実行する準備
ROSの実行準備をしていきます。

```shell
# catkin_makeする
cd ~/catkin_ws
catkin_make
source devel/setup.bash
source ~/.bashrc
```
次にlaunchファイルをroslaunchします。
はじめからフォルダにあるlaunchファイルはデモ用にlaunchファイルになります。
使用するlaunchファイルは用途によって選択したらいいと思います。

## demo_pointcloud.launchでlaunchする
pointcloudのデモを実行できる。

```shell
# roslaunchで実行する
roslaunch realsense2_camera demo_pointcloud.launch
```

実行すると勝手にrvizが起動します。
起動するとpointcloud2が動いていて以下のような3D画像がでていると思います。
![Screenshot from 2020-04-27 20-37-51.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/232369/6872e92a-b1d4-67ab-97b8-33b3761c2bb6.png)

今回は画像(2D画像)を確認するので以下の方法で2D画像を確認していきます。
### rivzで2D画像の確認の仕方
<details><summary>確認の方法</summary><div>
1. 右下のAddを押してimageを選択すると項目が追加される。
2. Image Topicの項目には
> /camera/color/image_raw
> /camera/depth/image_rect_raw
の2種類があると思うので確認してください。
</details></div>

## rs_rgbd.launchでlaunchする
RGB、Depth、infra画像などが取得できます。
rvizは勝手に表示されないので任意で実行してください。

```shell
# roslaunchで実行する
roslaunch realsense2_camera rs_rgbd.launch

#別ターミナルより
rviz
```

### rivzで画像の確認の仕方
<details><summary>確認の方法</summary><div>
1. 右下のAddを押してimageを選択すると項目が追加される。
2. Displays-Global Options-Fixed Frameの項目をcamera_linkに変更する。
3. Image Topicの項目には
> /camera/aligned_depth_to_color/image_raw
> /camera/aligned_depth_to_infra1/image_raw
> /camera/color/image_raw
> /camera/color/image_rect_color
> /camera/depth/image_rect_raw
> /camera/infra1/image_rect_raw
> /camera/infra2/image_rect_raw
の7種類があると思いますので確認してみてください。
</details></div>
## ウインドウで表示
以下ではrvizを使わずにウインドウでRGB画像を表示させてみます。

```
# 画像を表示する
rosrun image_view image_view  image:=/camera/color/image_raw
```

![Screenshot from 2020-04-21 00-18-24.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/232369/cd7f6fae-b884-d681-100c-6c09b27bea52.png)
Errorがでないで画像が表示されたらOK

#実行結果
最後に実行結果についてまとめておきます。
基本的に画像については以下のように考えたらよいと思われます。

## 画像一覧
<details><summary>RGB画像</summary><div>
RGB画像は普通のカメラ画像と同じです。
（/camera/color/image_rect_colorの方も同じ？？）

```
/camera/color/image_raw
```

![Screenshot from 2020-04-27 18-39-52.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/232369/24a3fd2e-66ea-371c-5705-0ef4ce7df0f8.png)
</div></details>
<details><summary>深度画像</summary><div>
## 深度画像
深度画像は画像の奥行が表されている画像です。

```
/camera/depth/image_rect_raw
```

![Screenshot from 2020-04-27 18-34-56.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/232369/d889365d-97e5-0c03-4621-9fac47816630.png)

</div></details>
<details><summary>点群画像</summary><div>
## 点群画像
点群画像は物体が点群で表されている画像です。

```
/camera/infra1/image_rect_raw
```
![Screenshot from 2020-04-27 18-40-43.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/232369/7f427bd2-feb5-57a3-d4df-5a115ead0a04.png)
</div></details>
aligned image
alignedは「揃えた」という意味で、これらは2つの画像を合わせた画像になっています。<br>


<details><summary>深度画像+RGB画像</summary><div>
##深度画像+RGB画像

```
/camera/aligned_depth_to_color/image_raw
```

</div></details>
<details><summary>深度画像+点群画像</summary><div>

##深度画像+点群画像
```
/camera/aligned_depth_to_infra1/image_raw
```

</div></details>

#エラー
つまづいたところがいくつかあったので
## realsense-viewer
たまに読み込まないときがある
![Screenshot from 2020-04-27 18-49-27.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/232369/f587001c-01f4-1a98-2d1f-14d812cd0007.png)
カメラを抜き差ししたら認識されるようになる。


## depth image
rs_rgbd.launchの方なんですが私の環境では深度画像系の表示がうまくいかなかった。
時間がたつと表示される（されないときもある）、FPSもあんまり出てない。
一応、Topicは送られているようですがImageが表示されてない感じ。

![Screenshot from 2020-04-27 13-45-19.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/232369/8a35ccde-c8d5-7b25-c184-660f1d50e67a.png)

原因としては
① 電力不足？？
[PCがRealSense R200をUSBハブに接続するとPCから認識できない](https://yura2.hateblo.jp/entry/2016/02/06/PC%E3%81%8CRealSense_R200%E3%82%92USB%E3%83%8F%E3%83%96%E3%81%AB%E6%8E%A5%E7%B6%9A%E3%81%99%E3%82%8B%E3%81%A8PC%E3%81%8B%E3%82%89%E8%AA%8D%E8%AD%98%E3%81%A7%E3%81%8D%E3%81%AA%E3%81%84)
② ファームウェアの問題？？
[Intel RealSense D435 をROSで使う](https://naonaorange.hatenablog.com/entry/2018/11/04/174745)
③ いつも実行時にでている↓こいつが原因？？

```
WARNING[547180495232](uvc-device.cpp:644)interrupt event received: 1, 3, 0, 7, 0, 11,
[WARN][1587965285.372585427]: Hardware Notification:USB SCP overflow,1.58797e+12,Eroor,Hardware Error
```

##対策方法
やっぱり電源が問題でした。<br>
供給量の大きいUSB変えたらいけました。<br>