realsense-rosからのrgbデータとdepthデータを同期させてみた

経緯

最近realsenseの入力ノードであるrealsense-ros(intel公式)に触れる機会があったのですが、あることに気が付きました。あれ、これってrgbデータとdepthデータがセットでメッセージとして送られてないやん、と。自分は下のような状態を期待していました。

しかし、実際は下のようにrgbデータとdepthデータは別々のノードでpublishされていました。

これでは、同じ瞬間に撮影されたrgbデータとdepthデータをセットとして使いたい場合、不便です。そこで、自分が思いついた方法は２つあります。1つは同じタイムスタンプのrgbとdepthのmessageを結合してpublishする方法です。もう１つは、受け取るノード側で、同じタイムスタンプのrgbとdepthのmessageが来た場合のみcallback関数を呼ぶというものです。今回は2つ目の方法を、ROSのmessage_filtersを使ってやってみた手順を共有します。

手順

1. realsense-ros側の設定

まず入力ノードであるrealsense-ros側での設定をしていきます。rs_camera.launchのenable_syncというパラメーターをtrueにします。これにより、rgbとdepthで同じタイムスタンプがあった場合、同じタイミングでまとめてpublishしてくれます。ただし、同じタイムスタンプが無い場合もどちらか１つだけでpublishするので、以下のようにソースコードを変更しました。if文を加えて、publishするvectorのサイズが2(rgbとdetphが揃っている)の場合のみpublishするようにしています。

void BaseRealSenseNode::frame_callback(rs2::frame frame)
{
    ~~省略~~
    if (frames_to_publish.size() == 2)
    {
        for (auto it = frames_to_publish.begin(); it != frames_to_publish.end(); ++it)
        {

        }
    }
}

これにより、pubilshされるrgbとdepthのメッセージのタイムスタンプは同じになりました。次に、メッセージを受け取るノード側の処理を書いていきます。

2. message_filtersによる同期処理

メッセージのタイムスタンプは同じですが、rgbとdepthでノードは別々です。そこで、message_filtersを使います。message_filtersは複数のメッセージタイムスタンプが同じ場合、指定のcallback関数を呼び出せるというものです。最大9つまで同期させることができます。公式に詳しい説明が書いてあります。今回は公式通りにやってみました。

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& rgb_msg, const ImageConstPtr& depth_msg)
{
  // Solve all of perception here...
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");

  ros::NodeHandle nh;
  message_filters::Subscriber<Image> rgb_sub(nh, "/camera/image_raw", 1);
  message_filters::Subscriber<Image> depth_sub(nh, "/camera/depth_raw", 1);

  typedef sync_policies::ExactTime<Image, Image> MySyncPolicy;
  // ExactTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), rgb_sub, depth_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}

これで、rbgとdepthからのメッセージのタイムスタンプが同じ場合のみcallback関数が呼ばれるようになりました。ただ、気をつけてほしいのは、受けとり側のノードをクラスとして作成している場合、subscriberをどこで初期化すればいいかわかりませんでした。クラスのコンストラクタで初期化してみましたが、うまくいかなかったので、クラスでやるのをやめました。もし、やり方を知ってる方いらしたら教えていただきたいです。タイムスタンプが近い場合にcallback関数を呼び出したい場合は、ApproximateTime Policyを使えばいけます。

まとめ

ROS面白くなってきました。

間違いや質問、ご意見等ありましたらお気軽にコメントください。頑張って答えますので(笑)。