ROSでOpenCVから取得した画像をPublish,Subscribeする

今回はROSでOpenCVにて取得した画像をPublish,Subscribeする方法について紹介していきます.

cv::Matの画像をROSのTopicに変換しpublishするために,ROSにはcv_bridgeとimage_transportというものが用意されています.この2つはROSをインストールした時にすでに入っているので追加でコンパイルする必要はありません.

 

 

CMakeLists.txt

まず,作成するコードはcv_bridgeとimage_transportに依存するので,CMakeLists.txtのfind_packageに書き加えます.

 

 

ソースコード Publisher

 

 

コードの解説

この2行はROSを使うためのお決まりのコードです.
rosのinitializeを呼び出し,このノードの名前がimg_publisherであることを明示しています.

image_transportを用いての画像のPublisherを定義します.
Topic名はimageとしています.用途に合わせて適宜変更してください.
it.advertiseの2つめの引数はキューサイズを表しています.

取得した画像を格納するためのMatの定義と,カメラから画像をCaptureするクラスの初期化をおこないます.
デバイスは0を指定しています.(0を指定すると/dev/video0のデバイスから画像を取得することになります.)
下の行はエラー処理となっています.もし,何らかの理由でカメラがOpenできなかった場合には-1を返し,プログラムを終了します.

このwhile文の中がメインの処理になります.
looprate(10)にてカメラ画像を取得して画像TopicをPublishする頻度を設定しています.
ここでは10Hzで画像を取得,Publishすることにしています.
cv::bridge::CvImage(std::Header(), ….. の部分でopencvで用いられている画像のデータ型cv::MatからROSのメッセージ型(seosor_msgs/PmagePtr型)への変換を行い,image_pub.publish(msg)の部分でメッセージをpublishしています.
最後の行のlooprate.sleep()関数は,先ほど設定した画像の取得周期10Hzとなるようにsleepしてくれる関数となります.

 

 

ソースコード Subscriber

 

 

コードの解説

この2行は先ほどのpublisherと同様に,ノードのinitialiseをおこなうお決まりのコードです.
このノードの名前がimg_subscriberであることを明示しています.

image_transportを用いての画像のSubscriberを定義します.
こちらのSubscriberは先ほどのimg_publisherが発行したimageというトピックをsubscribeできるようにトピック名を指定しています.
it.advertiseの2つめの引数はキューサイズを表しています.
3つめの引数はこのトピックが流れてきた時に呼ばれる関数を指定しています.
ROSでは新しいメッセージが受信される度にCallback関数というものが呼ばれ,Callback関数内で受信したデータの内容を処理します.

このspin()関数で,新しいメッセージが来る度にCallback関数を呼び出しています.

こちらのimageCallback関数が先ほど解説したspin()関数によって呼び出され,メッセージの内容が処理されます.
上記コードでは正しく/img_publisher/imageトピックが受信できているかを確認するために,受信した画像メッセージをウィンドウに表示するコードとなっています.
受信する画像メッセージはsensor_msgs::ImageConstPtr型となっているので,これをcv::bridgeを用いてopencvのcv::Mat型に変換しています.