ROS1/ROS2とRaspberryPi4でRealSenseD435を動かしてみた


# 概要

RaspberryPi4が技適対応したということで購入し、Intel RealSense D435とROS2を組み合わせて動かしてみました。
Point CloudのデータをトピックでPublishして同じ端末内でSubscribeした際のパフォーマンスを見ています。
せっかくなので、ノートPCやROS1環境でのパフォーマンスやCPU負荷も見てみました。

# 実行環境

使用したROS1/ROS2環境はmelodicとdashingになります。
どちらもコンテナ環境で実行しています。
コンテナイメージはDocker HubからROS公式のイメージを使用しています（ros:melodicとros:dashing）。
その他のハードウェアやOSは以下の環境で実行しました。

## RealSense

- D435 (無印)
- firmware : 05.12.00.00

※ 2019/12/23日時点での最新のファームウェアに更新しています
※ D435はUSB3.2で接続しています

## 実行端末

### RasperriPi4

- CPU: BCM2835 (armv7l)
- メモリ: 4GB
- OS: Raspbian 10 (buster, 32bit)

### ノートPC

- CPU: Core i7-7600U
- メモリ: 16GB
- OS: Ubuntu 18.04

## ビルドと実行

## ROS1

### ビルド

公式の手順通りビルドします (https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md)。

```
> source /opt/ros/melodic/setup.bash
> https://github.com/IntelRealSense/realsense-ros.git src/realsense-ros
> catkin init
> catkin build
```

### 実行

端末を２つ立ち上げて、一方でlaunchコマンドを実行して、もう一方でtopicコマンドを使用します。

#### 端末1 launchコマンド

```
> source devel/setup.bash
> roslaunch realsense2_camera rs_camera.launch enable_pointcloud:=True
... logging to /root/.ros/log/664923ea-2671-11ea-8444-b808cf5a8c64/roslaunch-raspberrypi-444.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
（以下略)
```

#### 端末1 topicコマンド

念のため、publishされているトピックのリストを表示しています。

```
> rostopic list
/camera/color/camera_info
/camera/color/image_raw
/camera/depth/camera_info
/camera/depth/color/points
/camera/depth/image_rect_raw
/camera/extrinsics/depth_to_color
/camera/extrinsics/depth_to_infra1
/camera/extrinsics/depth_to_infra2
/camera/infra1/camera_info
/camera/infra1/image_rect_raw
/camera/infra2/camera_info
/camera/infra2/image_rect_raw
/camera/pointcloud/parameter_descriptions
/camera/pointcloud/parameter_updates
/camera/realsense2_camera_manager/bond
/camera/rgb_camera/auto_exposure_roi/parameter_descriptions
/camera/rgb_camera/auto_exposure_roi/parameter_updates
/camera/rgb_camera/parameter_descriptions
/camera/rgb_camera/parameter_updates
/camera/stereo_module/auto_exposure_roi/parameter_descriptions
/camera/stereo_module/auto_exposure_roi/parameter_updates
/camera/stereo_module/parameter_descriptions
/camera/stereo_module/parameter_updates
/diagnostics
/rosout
/rosout_agg
/tf
/tf_static
```

topic コマンドのサブコマンド hz でパフォーマンスを測定します。

```
rostopic hz /camera/depth/color/points
average rate: 4.209
	min: 0.209s max: 0.295s std dev: 0.04052s window: 4
average rate: 3.174
	min: 0.209s max: 0.480s std dev: 0.10466s window: 6
average rate: 2.765
	min: 0.209s max: 0.480s std dev: 0.11510s window: 8
average rate: 2.588
	min: 0.209s max: 0.480s std dev: 0.11165s window: 10
average rate: 2.493
	min: 0.209s max: 0.480s std dev: 0.10565s window: 12
average rate: 2.433
	min: 0.209s max: 0.480s std dev: 0.09996s window: 14
average rate: 2.371
	min: 0.209s max: 0.480s std dev: 0.09280s window: 17
（以下略)
```

## ROS2環境

### ビルド

公式 (https://github.com/intel/ros2_intel_realsense )の方法に従ってビルドしています。

```
> git clone https://github.com/intel/ros2_intel_realsense.git src/ros2_intel_realsense
> source /opt/ros/dashing/setup.bash
> colcon build --base-paths src/ros2_intel_realsense
```

### 実行

ROS1の時と同様に端末を２つ立ち上げて、一方でlaunchコマンドを実行して、もう一方でtopicコマンドを使用します。

#### 端末１ launchコマンド

```
> source install/local_setup.bash
> ros2 launch realsense_ros2_camera rs.launch.py
[INFO] [launch]: All log files can be found below /root/.ros/log/2019-12-24-19-43-04-059734-tk470s2-6736
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [realsense_ros2_camera-1]: process started with pid [6746]
```
#### 端末2 topicコマンド

念のため、publishされているトピックのリストを表示しています。

```
> ros2 topic list
/camera/aligned_depth_to_color/camera_info
/camera/aligned_depth_to_color/color/points
/camera/aligned_depth_to_color/image_raw
/camera/color/camera_info
/camera/color/image_raw
/camera/depth/camera_info
/camera/depth/image_rect_raw
/camera/infra1/camera_info
/camera/infra1/image_rect_raw
/camera/infra2/camera_info
/camera/infra2/image_rect_raw
/parameter_events
/rosout
/tf_static
```

```
> ros2 topic hz /camera/aligned_depth_to_color/color/points
average rate: 1.121
	min: 0.266s max: 1.452s std dev: 0.48636s window: 3
average rate: 0.923
	min: 0.266s max: 1.656s std dev: 0.53564s window: 4
average rate: 1.499
	min: 0.233s max: 1.656s std dev: 0.56281s window: 8
average rate: 1.894
	min: 0.233s max: 1.656s std dev: 0.49983s window: 12
average rate: 1.983
	min: 0.233s max: 1.656s std dev: 0.45991s window: 15
average rate: 1.947
	min: 0.233s max: 1.656s std dev: 0.43457s window: 17
average rate: 1.972
	min: 0.233s max: 1.656s std dev: 0.40761s window: 20
（以下略)
```

# 結果

depth, infra1, infra2, color の各フレームレートの設定を15[fps]と30[fps]に設定して比較しました。
また、実行中のCPU負荷をtopコマンドで確認しました。
ROS1に比べてROS2のほうがパフォーマンスが落ちている様に見えます。
特にノートPCの15[Hz]の場合に顕著にあらわれています。

## 15 [fps]

ノートPCの場合はどちらもCPU使用率は110%前後(全リソースの1/4)で落ち着いていました。

|              |     ROS      |     ROS2     | 備考                          | 
|:------------:|:------------:|:------------:|:-----------------------------| 
| RaspberryPi4 |  4.0 ± 0.5   | 3.0 ± 0.5    | CPU使用率 120%前後             |
| ノートPC      | 15.0 ± 0.5   | 7.0 ± 0.5    | CPU使用率 110%前後             |

## 30 [fps]

ROS2（ノートPC）で試した際は、ros2 topic hz コマンドを実行直後は 20 [fps]強出るのですが、徐々に下がっていき数分後には 14 [fps] にまで落ちました。

|              |     ROS      |     ROS2     | 備考                          | 
|:------------:|:------------:|:------------:|:-----------------------------| 
| RaspberryPi4 | 2.0 ± 0.5    | 1.5 ± 0.5    | CPU使用率 200%前後             |
| ノートPC      | 16.5 ± 0.5   | 14.0 〜 20.0  |                              |

# 所感

ROS2のほうがパフォーマンスでないとなると躊躇してしまいますね。
DDSの実装を標準のFastRTPS以外に変更してみてどうなるのかが気になります。
今回は同一端末内のプロセス間通信に絞って見てみましたが、端末間の通信の場合も気になるところです。
特にパケットロスが顕著に起こる環境での利用を想定しているので、QOSポリシー設定ができるROS2には期待せざるを得ません。

# はまったところ

## ros2 launch + INFOログ

ros2 launch で起動すると INFO レベルのログが表示されませんでした。
WARNレベル以上のログは表示されているので出力先を間違っているというわけでもないようです。
launchファイルのから__log_level:=infoと明示的に指定しても変わらず、__log_level:=debugとすると表示されましたがDEBUGレベルのログまで表示されます。

## USB2.1接続

使用するケーブルを間違えていて、USB2.1接続になっていてるのに気づかずに時間溶かしました。
(RealSenseのノードは立ち上がるのに一部のトピックがpublishされない)

# 宣伝

ROS1で動作するロボットを自作していたので、ROS2に乗り換えに挑戦してみましたという内容の同人誌を書いています。
間に合えば[コミケ C97](https://webcatalog-free.circle.ms/Circle/14821861)にて頒布予定です。