(試行錯誤中)REALSENSE D435iをROS2で表示してみる

(ご意見随時受付中)

やりたいこと

DockerコンテナにROS2とCUDAが使えるようにして、D435iを使えるようにしてみようと思いましたが、うまく動かないので、ホスト側にもROS2をつっこんで調査してみます。
前の記事 (調査中)DockerからREALSENSE D435iを繋いで、ROS2で表示してみる

教科書はこれ。
「ROS2ではじめよう次世代ロボットプログラミング」を見ながら、ROS2をインストールして動かしてみます。

ROSインデックス
ROS2インストールオプション

terminal

sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8

sudo apt update && sudo apt install curl gnupg2 lsb-release
curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
sudo sh -c 'echo "deb http://packages.ros.org/ros2/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list'

sudo apt update
sudo apt install ros-dashing-desktop

source /opt/ros/dashing/setup.bash
sudo apt install python3-argcomplete

環境設定を登録。

export ROS_DISTRO=dashing
echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc
echo "source /opt/ros/$ROS_DISTRO/setup.zsh" >> ~/.zshrc
export RMW_IMPLEMENTATION=rmw_opensplice_cpp

実行してみます。

terminal

ros2 run demo_nodes_cpp talker
ros2 run demo_nodes_py listener

はい、普通に動きました。
ほかも色々入れて動かしてみます。
教科書のサンプルが実行できるようにセットアップします。

terminal

sudo apt update
sudo apt install ros-dashing-rmw-opensplice-cpp # for OpenSplice
sudo apt install ros-dashing-rmw-connext-cpp # for RTI Connext (requires license agreement)

cd ~/ && git clone https://github.com/youtalk/get-started-ros2.git
cd get-started-ros2 && git submodule update --init
mkdir ~/ros2 && cd ~/ros2
ln -s ~/get-started-ros2/ros2 src

sudo apt install python3-colcon-common-extensions
sudo apt install python3-rosdep
sudo rosdep init
rosdep update
rosdep install --from-paths src --ignore-src -r -y
colcon build
. ~/ros2/install/setup.bash

Hello_worldで異常が出ますが、よくわからないので、とりあえず置いといて。

sudo apt install ros-$ROS_DISTRO-realsense-camera-msgs ros-$ROS_DISTRO-realsense-ros2-camera
ros2 launch realsense_ros2_camera ros2_intel_realsense.launch.py

おや？
Dockerコンテナだけじゃなくて、ホストPCからもRealSenseが見えない！

さらにトラブルが続きます。

画面が反転する

D435iを動かしたら、突然画面がひっくり返り？？？？なに？

どうやら、タブレットなどディスプレイの向きを加速度センサで検出する機能がD435iと連動してしまっています。
以下のコマンドで画面を固定できました。

terminal

gsettings set org.gnome.settings-daemon.peripherals.touchscreen orientation-lock true

加速度のデータのデバイスを取るときに、異常が出る原因は、画面コントロールのドライバが加速度センサを使ってしまっていたからのようです。
ここの部分を切り離せるのでしょうか。

こちらの情報を参考に、もう一度実施してみましたが、
Jetson Nano+ROS2上でのIntel RealSenseの動作と注意点

terminal

$ ros2 run realsense_ros2_camera realsense_ros2_camera
========================================================================================
Report      : WARNING
Date        : 2020-06-01T07:33:18+0900
Description : using network interface wlo1 (192.168.1.22) selected arbitrarily from: wlo1, br-35f089619e19, br-4086a7d6b7f6, docker0

Node        : kuma-G5-5590
Process     : realsense_ros2_camera <20995>
Thread      : ddsi2 7f8d2522f700
Internals   : 6.9.190705OSS///ddsi2/q_nwif.c/989/0/1590964398.983064842/0
[INFO] [RealSenseCameraNode]: RealSense ROS v2. 0.1
[INFO] [RealSenseCameraNode]: Running with LibRealSense v2.16.5
[INFO] [RealSenseCameraNode]: getParameters...
[INFO] [RealSenseCameraNode]: setupDevice...
 01/06 07:33:20,285 ERROR [140244703418880] (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

iio-sensor-proxy.service を切ります。

$ sudo systemctl stop iio-sensor-proxy.service
$ sudo systemctl mask iio-sensor-proxy.service
Created symlink /etc/systemd/system/iio-sensor-proxy.service → /dev/null.

そして実行。
画像が表示されました。
しかし、点群は表示されません。

terminal1

$ ros2 run realsense_ros2_camera realsense_ros2_camera
========================================================================================
Report      : WARNING
Date        : 2020-06-02T07:36:47+0900
Description : using network interface wlo1 (192.168.1.22) selected arbitrarily from: wlo1, br-35f089619e19, br-4086a7d6b7f6, docker0

Node        : kuma-G5-5590
Process     : realsense_ros2_camera <7864>
Thread      : ddsi2 7fe403682700
Internals   : 6.9.190705OSS///ddsi2/q_nwif.c/989/0/1591051007.770383100/0
[INFO] [RealSenseCameraNode]: RealSense ROS v2. 0.1
[INFO] [RealSenseCameraNode]: Running with LibRealSense v2.16.5
[INFO] [RealSenseCameraNode]: getParameters...
[INFO] [RealSenseCameraNode]: setupDevice...
 02/06 07:36:49,324 ERROR [140617799682560] (ds5-motion.cpp:313) Motion Device is not calibrated! Motion Data Correction will not be available! Error: OpCodes do not match! Sent 79 but received -21!
[INFO] [RealSenseCameraNode]: Device Name: Intel RealSense D435I
[INFO] [RealSenseCameraNode]: Device Serial No: 834412071978
[INFO] [RealSenseCameraNode]: Device FW version: 05.12.03.00
255.255.255.255
[INFO] [RealSenseCameraNode]: Device Product ID: 0B3A
[INFO] [RealSenseCameraNode]: Sync Mode: On
[INFO] [RealSenseCameraNode]: Device Sensors: 
[INFO] [RealSenseCameraNode]: Stereo Module was found.
[INFO] [RealSenseCameraNode]: RGB Camera was found.
[INFO] [RealSenseCameraNode]: Motion Module was found.
[INFO] [RealSenseCameraNode]: Fisheye sensor isn't supported by current device! -- Skipping...
[INFO] [RealSenseCameraNode]: setupPublishers...
[INFO] [RealSenseCameraNode]: setupStreams...
[INFO] [RealSenseCameraNode]: depth stream is enabled - width: 640, height: 480, fps: 30
[INFO] [RealSenseCameraNode]: infra1 stream is enabled - width: 640, height: 480, fps: 30
[INFO] [RealSenseCameraNode]: infra2 stream is enabled - width: 640, height: 480, fps: 30
[INFO] [RealSenseCameraNode]: color stream is enabled - width: 640, height: 480, fps: 30
 02/06 07:36:50,412 ERROR [140615493936896] (backend-v4l2.h:210) xioctl(VIDIOC_QBUF) guard failed
 02/06 07:36:50,412 ERROR [140615493936896] (backend-v4l2.h:210) xioctl(VIDIOC_QBUF) guard failed
 02/06 07:36:50,412 ERROR [140615493936896] (backend-v4l2.h:210) xioctl(VIDIOC_QBUF) guard failed
 02/06 07:36:50,412 ERROR [140615493936896] (backend-v4l2.h:210) xioctl(VIDIOC_QBUF) guard failed
 02/06 07:36:50,646 ERROR [140615415473920] (backend-v4l2.h:210) xioctl(VIDIOC_QBUF) guard failed
 02/06 07:36:50,646 ERROR [140615415473920] (backend-v4l2.h:210) xioctl(VIDIOC_QBUF) guard failed
 02/06 07:36:50,646 ERROR [140615415473920] (backend-v4l2.h:210) xioctl(VIDIOC_QBUF) guard failed
 02/06 07:36:50,646 ERROR [140615415473920] (backend-v4l2.h:210) xioctl(VIDIOC_QBUF) guard failed
[INFO] [RealSenseCameraNode]: publishStaticTransforms...
[INFO] [RealSenseCameraNode]: RealSense Node Is Up!
========================================================================================
Report      : WARNING
Date        : 2020-06-02T07:38:15+0900
Description : writer 970823804:441:1:13059 topic rt/camera/color/image_raw waiting on high watermark due to reader 1729690071:441:1:9988

Node        : kuma-G5-5590
Process     : realsense_ros2_camera <7864>
Thread      : xmit.user 7fe4033d9700
Internals   : 6.9.190705OSS///ddsi2/q_transmit.c/845/0/1591051095.262717838/0

terminal2

$ ros2 run rviz2 rviz2 -d ~/ros2_ws/src/ros2_intel_realsense/realsense_ros2_camera/rviz/ros2.rviz
========================================================================================
Report      : WARNING
Date        : 2020-06-02T07:38:17+0900
Description : using network interface wlo1 (192.168.1.22) selected arbitrarily from: wlo1, br-35f089619e19, br-4086a7d6b7f6, docker0

Node        : kuma-G5-5590
Process     : rviz2 <8794>
Thread      : ddsi2 7f40203d8700
Internals   : 6.9.190705OSS///ddsi2/q_nwif.c/989/0/1591051097.422183971/0
[INFO] [rviz2]: Stereo is NOT SUPPORTED
[INFO] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[INFO] [rviz2]: Stereo is NOT SUPPORTED
[INFO] [rviz2]: Stereo is NOT SUPPORTED
[INFO] [rviz2]: Stereo is NOT SUPPORTED
[INFO] [rviz2]: Stereo is NOT SUPPORTED
[INFO] [rviz2]: Stereo is NOT SUPPORTED

このあたりが参考になるかな？
Ubuntuのカーネルに対応していないのかな？
https://qiita.com/eduidl/items/ebf40d79dc03de03abb3

とりあえず、ここまでとして、Docker版に戻ります。

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

こちらの、「ホストマシンからDockerに戻ってきた」のところから続きます。