ubuntu20.04上のROS2 FoxyでRealSenseを使用する手順

# はじめに

ubuntu20.04.01上のROS2 FoxyでRealSenseを使用したので、手順をまとめました。

ROS Noeticで使う方法についてはこちらにまとめてあります。
https://qiita.com/porizou1/items/be1eb78015828d43f9fb

ROS2 Foxyのインストール方法はこちらにまとめました。
https://qiita.com/porizou1/items/53053ce806304fd71f06


# 環境
・Ubuntu 20.04.01LTS
・Intel RealSense L515

他のRealSenseシリーズについても同様

# Intel Realsense SDK のインストール

以下の公式インストール手順に従って行う。
https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md



1. 公開鍵を登録

```bash
$ sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
```
1. サーバーをレポジトリリストに登録

```bash
$ sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo focal main" -u
```

3. ライブラリをインストール

```bash
$ sudo apt-get install librealsense2-dkms
$ sudo apt-get install librealsense2-utils
```

4. 開発者用ツール、デバッガツールをインストール

```bash
$ sudo apt-get install librealsense2-dev
$ sudo apt-get install librealsense2-dbg
```

1. RealSenseを接続し、以下コマンドでrealsense-viewerを起動

```bash
$ realsense-viewer
```

# realsense-rosのインストール


ros2_intel_realsenseというパッケージがあったのですが、もうメンテナンスされていないのとL515やD455に対応していないのでrealsense-rosのROS2ラッパーを使用します。

ros2_intel_realsense
https://github.com/intel/ros2_intel_realsense/tree/master

以下READMEに従ってインストール
https://github.com/IntelRealSense/realsense-ros/tree/foxy

ワークスペースの作成

```bash
$ mkdir -p ~/ros2_ws/src
$ cd ~/ros2_ws/src/
```

foxy用のrealsense-rosをインストール

```bash
$ git clone https://github.com/IntelRealSense/realsense-ros.git -b foxy
$ cd ~/ros2_ws
```

依存パッケージをインストール

```bash
$ sudo apt-get install python3-rosdep -y
$ sudo rosdep init
$ rosdep update
$ rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y
```

ビルド

```bash
$ colcon build
```

# 実行

RealSenseをPCのUSB3.0ポートに接続し、
ターミナルを開いて以下を実行

```bash
$ source ros2_ws/install/local_setup.bash 
$ ros2 launch realsense2_camera rs_launch.py
```

最後に以下のようなメッセージが出ていれば成功です。

```bash
[realsense2_camera_node-1] [INFO] [1611382613.824492586] [RealSenseCameraNode]: RealSense Node Is Up!
```

# rviz2で画像の表示

別のターミナルを開いて以下のコマンドでrviz2を起動します。

```bash
$ rviz2
```

RGB画像の表示

1. Fixed Frameをcamera_color_optical_frameに変更
2. 左下の「Add」ボタンを押して`By display type`から`Image`を選択してOKを押す
3. 左側`Image`タブの`Topic`を「`/camera/color/image_raw`」に変更
4. `Reliability Policy`を`Reliable`から`Best Effort`に変更

画像が表示されれば成功です。

Depth画像は同様に「Add」ボタンでImageを追加してTopic名を「/camera/depth/image_rect_raw」にします。

![2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/0bf547fa-265b-b8a6-5c66-5e7ba2567c98.png)

# rqt_image_viewで画像を表示

デフォルトのrs_launch.pyの設定では画像トピックのQoS設定が`SENSOR_DATA`になっており、このままではrqtで画像を表示できないので、rs_launch.pyの中身を書き換えます。
launchファイル最初に`configurable_parameters`という項目があるので、その中のQoS設定を書き換える。
RGB画像の場合は`color_qos`をデフォルトの`SENSOR_DATA`から`DEFAULT`に変更。

```python:rs_launch.py
                           {'name': 'color_qos',                    'default': 'DEFAULT', 'description': 'QoS profile name'},    
                           {'name': 'confidence_qos',               'default': 'SENSOR_DATA', 'description': 'QoS profile name'},    
                           {'name': 'depth_qos',                    'default': 'SENSOR_DATA', 'description': 'QoS profile name'},    
                           {'name': 'fisheye_qos',                  'default': 'SENSOR_DATA', 'description': 'QoS profile name'},    
                           {'name': 'infra_qos',                    'default': 'SENSOR_DATA', 'description': 'QoS profile name'},    
```

実行

```bash
$ ros2 launch realsense2_camera rs_launch.py
```

rqt_image_viewを起動

```bash
$ ros2 run rqt_image_view rqt_image_view 
```

左上のトピック一覧から`/camera/color/image_raw`を選択して画像が表示されればOK


![Screenshot from 2021-06-12 19-29-22.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/67e91822-dd14-d688-5f51-52eda456ce70.png)

# rviz2でPointCloudを表示させる

rs_launch.pyのデフォルトでは`PointCloud2`トピックをPublishする設定が無効になっているので、launchファイルの`configurable_parameters`を書き換えます。

`enable_pointcloud`という項目を探してデフォルトのfalseからtrueに変更。

```python:rs_launch.py
  {'name': 'enable_pointcloud',            'default': 'true', 'description': 'enable pointcloud'},
```


　
実行して別のターミナルでrviz2を起動

```bash
$ ros2 launch realsense2_camera rs_launch.py
```

```bash
$ rviz2
```



1. Fixed Frameを`camera_link`に変更
2. 左下の「Add」ボタンを押して`By display type`から`PointCloud2`を選択してOKを押す
3. 左側`PointCloud2`タブの`Topic`を「`/camera/depth/color/points`」に変更

![Screenshot from 2021-06-12 19-34-34.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/3e9c0ae4-c902-207e-9c48-987ef380e4e8.png)

rvizにPointCloudが表示されれば成功です

![rviz_screenshot_2021_06_12-19_35_49.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/96004c20-26de-c3c2-d60e-c0198959583d.png)


# 参考


https://qiita.com/takarakasai/items/91726199a4cb2a14c641


https://rb-station.com/blogs/article/jetson-nanoros2%E4%B8%8A%E3%81%A7%E3%81%AEintel-realsense%E3%81%AE%E5%8B%95%E4%BD%9C%E3%81%A8%E6%B3%A8%E6%84%8F%E7%82%B9