ROSでIntel Realsenseカメラから深度情報を取得する(ROS+Python)

UbuntuにてPython+ROSを使ってIntel Realsenseカメラから深度情報を取得する方法。

ROS, Intel Realsenseカメラのセットアップ方法は前回の記事参照。
UbuntuでIntel Realsense D415を使えるようにするまで(ROSあり・なし両方)

今回はRealsenseカメラからRGB画像情報、深度情報をROS+Pythonを使って取得していく。
Jupyter notebookからROSを操作できるJupyter ROSを使用したが、つまるポイントがいくつかあったため、記す。
また次回はJupyterを使わない方法を試していく。

目次

1. セットアップ
2. ROS Publisher, Subscriber, topicについて
3. PythonでSubscriberを書く

環境

Ubuntu 18.04
ROS Melodic
Python 3.6 (Anaconda)
Realsense D415

1. セットアップ

1.1 rospy

PythonでROSを扱えるようにするためにrospyを用いる。
インストールはpipにて行う。
pip install rospy

インストール後以下のコードを実行するとエラーメッセージがでた。エラーメッセージが出る原因はいくつかあるみたいだが、自分の場合はpip install rospkgによりrospkgインストールすることで解決できた。

import rospy

1.2 jupyter-rosのセットアップ

今回はROSの結果をすぐに確認するためにjupyterからROSを操作する方法を試した。jupyterからの操作は標準的なものではなく開発途中であるため動作が安定しない。
https://github.com/RoboStack/jupyter-ros

インストール方法はgithubに載っている。
https://github.com/RoboStack/jupyter-ros#installation-and-dependencies

pip install jupyter-ros
jupyter nbextension enable --py --sys-prefix jupyros

2. ROS Publisher, Subscriber, topicについて

ROSはデータをtopicとしてやりとりする。Topicを作り発信するnodeをpublisher, 受け取るnodeをsubscriberと呼ぶ。
今回はrealsenseが画像のtopicをpublishし、Pythonで定義したnodeがそのtopicをsubscribeする。

realsense立ち上げ

まずはrealsenseのnodeを立ち上げる。
ターミナルにて
roslaunch realsense2_camera rs_camera.launch

Topicを確認する

次にrealsenseからどのようなtopicがpublishされているか確認する。これにはrostopicコマンドを使用する。
別のターミナルにて
rostopic list

relasense-rosでも一部を確認できる
https://github.com/IntelRealSense/realsense-ros#usage-instructions

それぞれのtopicは型が決まっており、後にtopicをsubscribeするときに型を指定する必要がある前、確認する。
これにはrostopic typeコマンドを使用する。
例えば/camera/color/image_raw/compressedの型を確認するには以下のようにする。
rostopic type /camera/color/image_raw/compressed

sensor_msgs/CompressedImageという型であることがわかる。

topic name	type
/camera/color/image_raw	Image
/camera/color/image_raw/compressed	CompressedImage
/camera/depth/image_rect_raw	Image
/camera/depth/image_rect_raw/compressed	CompressedImage

3. PythonでSubscriberを書く

今回はJuypterでsubscriberを書く方法について記す。いくつかつまずいたので次回はJupyterを使わない方法を試す。
ROS+PythonのチュートリアルにはJupyterを使わない方法について記されていて参考になる。
http://wiki.ros.org/ja/ROS/Tutorials/WritingPublisherSubscriber%28python%29

rospyとjupyrosのインポート

import rospy as rp
import jupyros as jr

topicの型のインポート

from sensor_msgs.msg import CompressedImage, Image
from std_msgs.msg import String

Nodeの立ち上げ

rp.init_node('test')

Subscriberの準備
callback関数(受けた入力を処理する関数)を定義する
受け取ったデータをnumpy arrayに変換する
用途によってはここから更に処理することが可能

def call_back(ros_data):
    np_arr = np.fromstring(ros_data.data, np.uint8)
    print(np_arr)

Subscribeする
jr.subscribe(topic name, topic type, call back function)

jr.subscribe('camera/depth/image_rect_raw/compressed',CompressedImage,call_back)

これによりnumpy arrayがプリントされるようになれば成功。

jupyter rosの不具合修正
しかし自分の場合はエラー以下のようなエラーが出た。

どうやらjupyros pubsub.pyの25行目が悪さをしているらしい。
たぶんdictに入れるkeyの問題？

試行錯誤の結果、パッケージの24-25行目を以下のように直接編集したらうまく動くようになった。
根本的な解決にはなっていないような気がするが、今の所これでうまく行っている。

        if thread_name != 'MainThread':
            # output_registry[threading.currentThread().getName()].append_stdout(msg)
            self.original.write(msg)

また本題とはずれるが、pipでインストールできるバージョンでは49-50行目が以下のようになっているが、

if subscriber_registry.get(topic): 
    raise Error("Already registerd...") 

これは正しくは以下のとおりである。githubでは修正されている。

if subscriber_registry.get(topic): 
    raise RuntimeError("Already registerd...") 

自分の場合はこれによりrealsenseからめでたく画像、深度データを取得できるようになった。

次回

今回はJupyterでROSを扱うことを試みたため苦戦した。次回はJupyterを使わずにデータを受け取ることを試す。