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を使わずにデータを受け取ることを試す。