ROSのあるNodeでほぼ同期のとれた二つのtopicからの情報両方を一緒にcallbackに受け取って処理したい時があります.
例えば深度画像とカメラ画像を使って色の付いた点群を出力したいときなどは時間的に同期が取れていないとずれた画像が点群にマッピングされてしまいます.
tfなども時間の一致が大切なためそういったときにも二つの入力がほぼ同じ時間かどうかというのは大切になります.
そんなときに使うのはmessage_filtersパッケージに含まれるTimeSyncronizerを使用します.
http://wiki.ros.org/message_filters
に詳細が乗っているのでご確認ください.
https://github.com/ros-perception/image_pipeline/blob/indigo/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp#L68-L73
例えば上記の箇所に当たる以下の処理はまさしく先ほど紹介したカラーやデプスを扱う処理のために使用されているものです.ここでは深度画像と通常画像とカメラ情報を受け取ってコールバックを設定します.
typedef ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> SyncPolicy;
typedef ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> ExactSyncPolicy;
typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
typedef message_filters::Synchronizer<ExactSyncPolicy> ExactSynchronizer;
boost::shared_ptr<Synchronizer> sync_;
boost::shared_ptr<ExactSynchronizer> exact_sync_;
上記のように同期時刻判定をする二つのポリシーExactTimeとApproximateTimeが存在します.
ExactTimeは名前の通り、msgのタイムスタンプが完全に一致した時のみコールバックを発動します.同じノードからmsgが出てくるときはタイムスタンプは揃うのでカメラ情報とカメラ画像とかはおそらく揃っています.
もう一つのApproximateTimeはこれも名前の通りちょっと時間のずれがあることを許容してだいたい同時期にきたらコールバックを発動します.これはtfや画像など別のnodeから出てきたものは完全一致することがないのでだいたい揃っていたらコールバックを呼ぶといったポリシーです.
書き方がやや凝ってますが、以下で登録完了でコールバックが呼ばれるようになります.
if (use_exact_sync)
{
exact_sync_.reset( new ExactSynchronizer(ExactSyncPolicy(queue_size), sub_depth_, sub_rgb_, sub_info_) );
exact_sync_->registerCallback(boost::bind(&PointCloudXyzrgbNodelet::imageCb, this, _1, _2, _3));
}
else
{
sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_, sub_rgb_, sub_info_) );
sync_->registerCallback(boost::bind(&PointCloudXyzrgbNodelet::imageCb, this, _1, _2, _3));
}
のregisterCallbackがコールバック関数を結びつけているところです.
トピックのサブスクライバをmessage_filtersやimage_transportを使って宣言しておいて、各syncのreset時に指定しています.これでsyncはどのトピックを確認すればいいかわかります.
3トピックまではできるのを見ていますがそれ以上ができるかはわかりません….
以上です.
上記の
- Subscriberをmessage_filters::Subscriberにする
- Syncを宣言する
- SyncをSubscriber等で初期化してregisterCallbackをする
を行えばほぼ同時刻の二つのmsgのコールバックを受け取るNodeを作ることができます.
