【ROS】Ubuntu16.04+ROS kinetic環境におけるRealsense D415を使った点群表示

ROSを学習するため、手持ちのRealsense D415を使った点群計測にチャレンジしました｡
Realsense D415で計測した深度とRGB情報を使って､点群可視化アプリ「RVIZ」で表示するところまでやってみます。

今回の環境はこちら:
PC：Microsoft Surface Pro3
OS：Ubuntu16.04LTS　※上記PCにデュアルブートでインストール済み
カメラ：Realsense D415

PCへのUbuntuをデュアルブートで起動するようにインストールする方法はネット上に各種ありますので省略します。

点群を取るまでの作業手順はこちらを参考にしました↓


基本的には上から順番にやっていけばインストールはできますが、念のため、各ステップごとに補足しながら進めます。

Step.1 Realsense SDK2.0をインストール

ソースコードをビルドする方法と、パッケージをダイレクトにインストールする方法の2種類が提示されてますが、簡単な後者で進みます。

作業手順やダウンロード先のURLが変更される場合があるので､下のサイトに記載されているやり方で進めていきましょう。

おおまかな手順
・サーバーの公開鍵の登録
　sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE

・リポジトリの更新
sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main" -u

・パッケージリストの更新
sudo apt update

・ライブラリのインストール
sudo apt-get install librealsense2-dkms
sudo apt-get install librealsense2-utils

・開発パッケージのインストール
sudo apt-get install librealsense2-dev
sudo apt-get install librealsense2-dbg

ここまで進んだら、一旦再起動してからターミナルでrealsense-viewerと入力してビューワーを起動させます｡

無事､RGB画像とデプス画像が取れたらviewerをいったん閉じて、次の手順に進みます。

Step.2 ROS kineticのインストール

続いて冒頭のGithubサイトStep2にある、ROSのインストールに進みます｡
ROSのインストール作業は、下記公式資料を上から順に進めていくのが確実です。

大まかな流れは下記の通り。
・ソースリストの設定
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'

・公開鍵の設定
sudo apt-key adv --keyserver 'hkp://ha.pool.sks-keyservers.net:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654

・パッケージの更新
sudo apt-get update

・フルパッケージのインストール　※今回使用するrvizも含んでいます
sudo apt-get install ros-kinetic-desktop-full

・ROSDEPの初期化
sudo rosdep init
rosdep update

・環境設定
echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
source ~/.bashrc

・rosinstallの準備
sudo apt-get install python-rosinstall

・インストール状態の確認
ターミナル画面でroscoreを入力して実行するとインストールしたROSのversion等が表示されます。無事、表示されたらROSのインストールは完了です。

[Ctr + C]で強制終了させ、次に進みます。

Step.3 realsense用ROSのビルド

再び冒頭のGithubサイトに戻り、次はStep3です｡
・catkinワークスペースを作成
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src/

ホームディレクトリ上にcatkin_wsディレクトリが作成され、その下にsrcディレクトリが作成されます。cdコマンドでsrcディレクトリに移動します。

続いて、
・RealSense用ROSのソースコードをGithubからクローン
git clone https://github.com/IntelRealSense/realsense-ros.git
cd realsense-ros/
git checkout `git tag | sort -V | grep -P "^\d+\.\d+\.\d+" | tail -1`
cd ..

　※ddynamic_reconfigureがインストールされていない場合はaptコマンドでインストールしてください。

続いて、
・ビルド
catkin_init_workspace
cd ..
catkin_make clean
catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
catkin_make install
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

以上でROSインストールが完了です｡

Step.3-2 D435iでIMUを使う場合

D435iに搭載されているIMUの機能を使ってROSを利用する場合は追加でインストールが必要になりますので下記コマンドを実行します。

sudo apt-get install ros-kinetic-imu-filter-madgwick

Step.4 カメラからRGBと深度データを点群で取得

roslaunch realsense2_camera rs_camera.launch filters:=pointcloud

※カメラから取得するデータの各種パラメータの説明については冒頭のGithubサイトに記載されているので参照してください｡

Step.5 RVIZの起動

点群を可視化するビューワーソフトを起動します。
二つ目のターミナルを起動させ、RVIZと入力して実行
するとこのような画面が出ます。

左側のDisplaysタブの二行目「Fixed Frame」欄がデフォルトでmapになっているので､ここを下のようにcamera_linkに変更します。

すると下のようにGlobal_Stutus欄がOKに変わります。

続いて、左下のAddボタンをクリックすると、下の図の中央のようなウィンドウが起動します｡

By topicタブを開き、上から順に/camera→/depth→/color→/pointsとたどって行くと､PointCloud2という項目がありますので、これをクリックして右下のokボタンを押します。

そうすると、下の画像のように点群をリアルタイムで取得できます。

マウスで画面の操作(パン､ズームなど)やDisplays欄のSizeで点群の直径を変更したりできます。

ROSの特徴の一つである、環境MAPの作成については､こちらが参考になります↓

計測した点群を記録し、環境MAPの作成などもできるようなので､興味のある方はチャレンジしてみてください｡