DockerからREALSENSE D435iを繋いで、ROS2で表示してみる

#やりたいこと
+ RealSense D435iをDockerコンテナで使って表示したい
+ DockerコンテナでD435iをROS2で扱いたい

※現在はこんな感じ。
RealSenseViewerで表示されます。
![Screenshot from 2020-05-30 09-15-31.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/328520/3f8ac2bf-7934-c4a3-8b52-7389c9a31bf4.png)

ROS2ではこんな感じ。
4sほどディレイしたものが表示されます。
![Screenshot from 2020-06-03 15-00-47.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/328520/358495db-f782-5aef-c78f-a61036df2538.png)



前回の記事でDocker環境からGUIをうまく使えるようになったので、USBデバイスも使えるようにしようとしたが、USB機器をつなぐところではまったのでやり方を記録しました。

#環境
+ amd64のUbuntu18の入ったノートPC
+ GPUはGTX1660Ti
+ RealSense D435iを使用する

#実施
DockerfileにRealSenseのドライバやアプリのセットアップを追加します。
[IntelRealSense/librealsense Linux Distribution](https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md)

```dockerfile:Dockerfile
#(前は省略)
# REALSENSEの追加
RUN cd /home/developer
RUN apt-get update && apt-get install -y software-properties-common
RUN sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
RUN sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" -u
RUN sudo apt-get install -y librealsense2-dkms
RUN sudo apt-get install -y librealsense2-utils
RUN sudo apt-get install -y librealsense2-dev
RUN sudo apt-get install -y librealsense2-dbg
RUN sudo apt-get install -y v4l-utils 
```

v4l-utilsは動作確認用で追加しました。
ビルドして実行しますが、うまく動きませんでした。
docker runのオプションの方に問題がありそうです。


##REALSENSEをホストマシンで確認する
ホストマシンにv4l2-ctlをインストールして、デバイスを確認します。

```
$ sudo apt install v4l-utils 
$ v4l2-ctl --list-device
Intel(R) RealSense(TM) Depth Ca (usb-0000:00:14.0-2):
	/dev/video2
	/dev/video3
	/dev/video4
	/dev/video5
	/dev/video6
	/dev/video7

Integrated_Webcam_HD: Integrate (usb-0000:00:14.0-5):
	/dev/video0
	/dev/video1
```

REALSENSEは2〜7に割り当てられています。

##REALSENSEをDockerの中から確認する

```
$ v4l2-ctl --list-device
Failed to open /dev/video0: Permission denied
```
パーミッションで弾かれます。

```
$ sudo v4l2-ctl --list-device
Integrated_Webcam_HD: Integrate (usb-0000:00:14.0-5):
	/dev/video0
```
特権モードでは問題ないようです。

--deviceオプションでビデオデバイスをいちいち登録するよりも、ホストのユーザーとかファイルなどを全部マウントしてしまったほうが、設定が楽そうです。
色々試したところ、こんな形に落ち着きました。

```bash:launch.sh
docker run --gpus all -it --rm \
    --privileged \
    --user=$(id -u $USER):$(id -g $USER) \
    --env="DISPLAY" \
    --workdir="/home/$USER" \
    --volume="/home/$USER:/home/$USER" \
    --volume="/etc/group:/etc/group:ro" \
    --volume="/etc/passwd:/etc/passwd:ro" \
    --volume="/etc/shadow:/etc/shadow:ro" \
    --volume="/etc/sudoers.d:/etc/sudoers.d:ro" \
    --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
    -e DISPLAY=$DISPLAY --name glvnd cuda_gui

# 参考
# http://wiki.ros.org/docker/Tutorials/GUI

```

ros.orgを参考に、ホストの構成を全部突っ込む感じで動かしてみます。

```bash:terminal
$ sudo realsense-viewer
```
sudo つけないと動きませんでした。（この部分は要調査）

![Screenshot from 2020-05-30 09-54-10.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/328520/df9dc773-405c-e3b3-61bc-4f18817dbc0b.png)

とりあえず、問題なく表示されていますね。
そして、一応使えているけど、汚いDockerfileですね。
一応、ここに載せておきます。

```dockerfile:Dockerfile
# GPUとGUIが使えるDockerfileをamd64で動かす

FROM nvidia/cudagl:10.2-base-ubuntu18.04

# これを入れないとtzdataで設定入力を求められて停止する
ENV DEBIAN_FRONTEND=noninteractive

RUN apt-get update && \
    apt-get install -yq wget curl git build-essential vim sudo lsb-release locales bash-completion tzdata

# Replace 1000 with your user / group id
RUN export uid=1000 gid=1000 && \
    mkdir -p /home/developer && \
    echo "developer:x:${uid}:${gid}:Developer,,,:/home/developer:/bin/bash" >> /etc/passwd && \
    echo "developer:x:${uid}:" >> /etc/group && \
    echo "developer ALL=(ALL) NOPASSWD: ALL" > /etc/sudoers.d/developer && \
    chmod 0440 /etc/sudoers.d/developer && \
    chown ${uid}:${gid} -R /home/developer

RUN curl -sSL http://get.gazebosim.org | sh

ENV ROS_DISTRO dashing

# ロケールのセットアップ
RUN apt-get update && apt-get install -y locales && \
    dpkg-reconfigure locales && \
    locale-gen ja_JP ja_JP.UTF-8 && \
    update-locale LC_ALL=ja_JP.UTF-8 LANG=ja_JP.UTF-8
ENV LC_ALL   ja_JP.UTF-8
ENV LANG     ja_JP.UTF-8
ENV LANGUAGE ja_JP.UTF-8

# APTソースリストの設定
RUN apt-get update && \
    apt-get install -y curl gnupg2 lsb-release && \
    curl http://repo.ros2.org/repos.key | apt-key add - && \
    sh -c 'echo "deb [arch=amd64,arm64] http://packages.ros.org/ros2/ubuntu \
    `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list' && \
    apt-get update

# ROS2パッケージのインストール
RUN export ROS_DISTRO=dashing && \
    apt-get install -y ros-$ROS_DISTRO-desktop \
    python3-colcon-common-extensions python3-rosdep python3-argcomplete && \
    rosdep init && \
    rosdep update

# REALSENSEの追加
RUN cd /home/developer
RUN apt-get update && apt-get install -y software-properties-common
RUN sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
RUN sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" -u
RUN sudo apt-get install -y librealsense2-dkms
RUN sudo apt-get install -y librealsense2-utils
RUN sudo apt-get install -y librealsense2-dev
RUN sudo apt-get install -y librealsense2-dbg
RUN sudo apt-get install -y v4l-utils 

USER developer
ENV HOME /home/developer

## 環境設定
RUN    echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc

```

整理が必要です。
適当なタイミングで

#ROS2でPointCloudしてみる（試行錯誤中）
こちらを参考にして、セットアップして動かしてみます。
[ros2_intel_realsense](https://index.ros.org/r/ros2_intel_realsense/)
https://github.com/intel/ros2_intel_realsense

[このissueの方もDockerで動かそうとしています](https://github.com/IntelRealSense/realsense-ros/issues/642)

Dockerfileに書き込んであれこれするまえに、コンテナの中でセットアップしてみます。
※事前にホストPCで **sudo usermod -a -G video username** (usernameにユーザー名を入力)して、置かないと、ROS2でD435iのパーミッションに引っかかります。

カメラの確認方法。

```bash:terminal
$ ls -al /dev/video*
$ v4l2-ctl --list-device
```

どちらもsudoをつけないと動かないですね。
うまい方法が思いつかなかったので、chmodを使うこととします。

```bash:terminal
sudo chmod 777 /dev/video*
```

うーん、もっとスマートな方法はないものか・・・。

```
sudo usermod -a -G users "$(whoami)"
sudo usermod -a -G video "$(whoami)"
```

はどうか？という記事も見ましたが、すでにグループ登録されているはず・・・
※この件は一旦保留。

```bash:terminal
$ sudo apt-get install ros-dashing-cv-bridge ros-dashing-librealsense2 ros-dashing-message-filters ros-dashing-image-transport
$ sudo apt-get install -y libssl-dev libusb-1.0-0-dev pkg-config libgtk-3-dev
$ sudo apt-get install -y libglfw3-dev libgl1-mesa-dev libglu1-mesa-dev
$ sudo apt-get install ros-dashing-realsense-camera-msgs ros-dashing-realsense-ros2-camera

$ mkdir -p ~/ros2_ws/src
$ cd ~/ros2_ws/src
$ git clone https://github.com/intel/ros2_intel_realsense.git

$ cd ~/ros2_ws
$ source /opt/ros/dashing/setup.bash
$ colcon build --base-paths src/ros2_intel_realsense

$ source /opt/ros/dashing/setup.bash
$ source ~/ros2_ws/install/local_setup.bash
# To launch with "ros2 run"
$ ros2 run realsense_ros2_camera realsense_ros2_camera
# OR, to invoke the executable directly
$ realsense_ros2_camera
```

という流れで、セットアップしたのですが、異常が出ます。

```bash:terminal
$ ros2 run realsense_ros2_camera realsense_ros2_camera
[INFO] [RealSenseCameraNode]: RealSense ROS v2. 0.1
[INFO] [RealSenseCameraNode]: Running with LibRealSense v2.16.5
[INFO] [RealSenseCameraNode]: getParameters...
[INFO] [RealSenseCameraNode]: setupDevice...
 30/05 06:26:38,998 ERROR [140404533152256] (types.h:180) get_xu(...). xioctl(UVCIOC_CTRL_QUERY) failed Last Error: Device or resource busy
[ERROR] [RealSenseCameraNode]: An exception has been thrown: get_xu(...). xioctl(UVCIOC_CTRL_QUERY) failed Last Error: Device or resource busy
terminate called after throwing an instance of 'rs2::backend_error'
  what():  get_xu(...). xioctl(UVCIOC_CTRL_QUERY) failed Last Error: Device or resource busy

```

ソースコードを追うと、ここで異常が出ているようです。

```cpp:realsense_camera_node.cpp  
void setupDevice()
(中略 errorは358行目)

    } catch (const std::exception & ex) {
      RCLCPP_ERROR(logger_, "An exception has been thrown: %s", ex.what());
      throw;
    } catch (...) {
      RCLCPP_ERROR(logger_, "Unknown exception has occured!");
      throw;
    }
```

なんですかね？RCLCPP_ERROR?
types.h:180を確認したほうが良い？

長くなってきたので、次の記事に続きます。
[(試行錯誤中)DockerからREALSENSE D435iを繋いで、ROS2で表示してみるエラーをデバッグする](https://qiita.com/t_kumazawa/items/ea64812b52bf7695de7b)

#Dockerファイルを調整したら、加速度計が動かなくなった
Dockerfileの中から、もう意味のなさそうな記述を削除しました。

ホストPCの構成を千部取り込むことにし、不要と判断。

![Screenshot from 2020-05-31 08-04-28.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/328520/077afda6-e5d5-8c7b-9db4-89c5bf2d100b.png)

これで、実行してみると、RealSenseViewerを実行すると、D435iの加速度計だけがPermisson deniedが発生して動きません。

```bash
Backend in rs2_open_multiple(sensor:0x7fdba80168a0, profiles:0x7fdba8d76168, count:2):
Failed to open scan_element /sys/devices/pci0000:00/0000:00:14.0/usb2/2-2/2-2:1.5/0003:8086:0B3A.000A/HID-SENSOR-200076.3.auto/
iio:device1/scan_elements/in_anglvel_x_en Last Error: Permission denied

```

検索すると
>権限エラーは不適切にインストールされたことが原因である可能性がありますudev-rules。インストールガイドで指定されているスクリプトを実行しましたか？ ./scripts/setup_udev_rules.sh

この現象をなんとかしようとインストールガイドを読んでいたら・・・
[Linux Ubuntu Installation](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md)

>LinuxでRealSenseデプスカメラを実行するには、修正したカーネルドライバーにパッチを適用して挿入する必要があります。

通常のインストールではDockerと相性悪すぎなのか。
・・・とりあえず、加速度計は使わないので、次行ってみよう。
もしかしたら、ROS2で今出ている異常も、加速度計を除外したら動くのかもしれない。

[(試行錯誤中)DockerからREALSENSE D435iを繋いで、ROS2で表示してみるエラーをデバッグする](https://qiita.com/t_kumazawa/items/ea64812b52bf7695de7b)

#ホストマシンからDockerに戻ってきた
ホストマシンだと加速度計がrealsense-viewerで表示できませんでしたが、こちらだと正常に検出できますね。
これは、ホストマシンの方で画面の回転で取られてしまったセンサを開放した効果なのでしょうか？

![Screenshot from 2020-06-03 14-20-56.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/328520/438a580d-a594-4e3b-0e72-8a683406fb49.png)

ホストマシンでやってみたノウハウをDockerに適用してみて、どうなるかやってみます。

#ROS2でRealsenseを動かす

ホストPCで実行した内容を丸写しにして実行してみました。
（整理してあとでここに書き込みます。）
??な異常が発生。

```bash:terminal1
$ ros2 launch realsense_ros2_camera ros2_intel_realsense.launch.py
[ERROR] [rcl]: Error getting RMW implementation identifier / RMW implementation not installed (expected identifier of 'rmw_connext_cpp'), with error message 'failed to find shared library of rmw implementation. Searched rmw_connext_cpp, at /tmp/binarydeb/ros-dashing-rmw-implementation-0.7.2/src/functions.cpp:130', exiting with 1.
```

起動中のノードがどのRMWを使っているのかわからない異常らしいので、以下のように実行。
デフォルトで固定する方法や、そもそもこの状態になるのを解決しておきたいところ。

```bash:terminal1
$ RMW_IMPLEMENTATION=rmw_opensplice_cpp ros2 launch realsense_ros2_camera ros2_intel_realsense.launch.py

```

![Screenshot from 2020-06-03 15-00-47.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/328520/b96df6c9-5cbe-c589-e041-4e2f5de0e5ee.png)

動きました！
ホストマシンでは動かないので、カーネルの問題である可能性が高いです。

問題点として、動きが4秒程度ディレイして動くところでしょうか。
これだと、ちょっとリアルタイム制御には使いにくいですね。
バッファのとり方でなんとかなるかと思います。

#整理してインストールし直し

※作業中
以下のコマンドを順番に実行したら動きます。

```bash:terminal

export RMW_IMPLEMENTATION=rmw_fastrtps_cpp

sudo apt-get install ros-dashing-cv-bridge ros-dashing-librealsense2 ros-dashing-message-filters ros-dashing-image-transport
sudo apt-get install -y libssl-dev libusb-1.0-0-dev pkg-config libgtk-3-dev
sudo apt-get install -y libglfw3-dev libgl1-mesa-dev libglu1-mesa-dev
sudo apt-get install ros-dashing-realsense-camera-msgs ros-dashing-realsense-ros2-camera

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
git clone https://github.com/intel/ros2_intel_realsense.git

cd ~/ros2_ws
source /opt/ros/dashing/setup.bash
colcon build
ros2 launch realsense_ros2_camera ros2_intel_realsense.launch.py

```