Home > Hello, world! > ROS・ロボット技術 > ROSでOpenCVにて取得した画像をPublish,Subscribeする

ROSでOpenCVにて取得した画像をPublish,Subscribeする

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

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

CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
    roscpp
    ...
    cv_bridge
    image_transport
    )

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

ソースコード Publisher

#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
int main(int argc, char** argv) {
　　　　ros::init (argc, argv, "img_publisher");
　　　　ros::NodeHandle nh("~"); 
　　　　mage_transport::ImageTransport it(nh);
　　　　image_transport::Publisher image_pub = it.advertise("image", 10);
　　　　cv::Mat image;
　　　　cv::VideoCapture camera(0);
　　　　if (!camera.isOpened()) {
　　　　　　　　ROS_INFO("failed to open camera.");
　　　　　　　　return -1;
　　　　}
　　　　ros::Rate looprate (10); // capture image at 10Hz
　　　　while(ros::ok()) {
　　　　　　　　camera >> image;
　　　　　　　　sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
　　　　　　　　image_pub.publish(msg);
　　　　　　　　ros::spinOnce();
　　　　　　　　looprate.sleep();
　　　　}
　　　　return 0;
}

コードの解説

ros::init (argc, argv, "img_publisher");
ros::NodeHandle nh("~");

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

image_transport::ImageTransport it(nh);
image_transport::Publisher image_pub = it.advertise("image", 10);

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

cv::Mat image;
cv::VideoCapture camera(0);
if (!camera.isOpened()) {
　　　　ROS_INFO("failed to open camera.");
　　　　return -1;
}

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

ros::Rate looprate (10); // capture image at 10Hz
while(ros::ok()) {
　　　　camera >> image;
　　　　sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
　　　　image_pub.publish(msg);
　　　　ros::spinOnce();
　　　　looprate.sleep();
}

この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

#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
cv::Mat image;
void imageCallback(const sensor_msgs::ImageConstPtr& msg) {
　　　　try {
　　　　　　　　image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8)->image;
　　　　}
　　　　catch (cv_bridge::Exception& e) {
　　　　　　　　ROS_ERROR("cv_bridge exception: %s", e.what());
　　　　}
　　　　cv::imshow("image", image);
　　　　cv::waitKey(1);
}
int main(int argc, char** argv) {
　　　　ros::init (argc, argv, "img_subscriber");
　　　　ros::NodeHandle nh("~");
　　　　image_transport::ImageTransport it(nh);
　　　　image_transport::Subscriber image_sub = it.subscribe("/img_publisher/image", 10, imageCallback);
　　　　ros::spin();
　　　　return 0;
}

コードの解説

ros::init (argc, argv, "img_subscriber");
ros::NodeHandle nh("~");

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

image_transport::ImageTransport it(nh);
image_transport::Subscriber image_sub = it.subscribe("/img_publisher/image", 10, imageCallback);

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

ros::spin();

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

void imageCallback(const sensor_msgs::ImageConstPtr& msg) {
　　　　try {
　　　　　　　　image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8)->image;
　　　　}
　　　　catch (cv_bridge::Exception& e) {
　　　　　　　　ROS_ERROR("cv_bridge exception: %s", e.what());
　　　　}
　　　　cv::imshow("image", image); cv::waitKey(1);
}

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

以上、ROSでOpenCVを用いて取得した画像あをpublish, subscribeする方法についてでした。

関連記事

ROS

ROSのナビゲーションamclについて理解を深めてみる

ROSのロボットの動作計画のスタック（パッケージの集合）としてnavigationスタックが有名です。 そこで今回はamclを使いこなせるようになってみたいと思います。 amclを使う上での事前知識 パーティクルフィルタを用いた2次元平面での自己位置推定（x, y, yawの推定） パー […]