4
Help us understand the problem. What are the problem?

More than 1 year has passed since last update.

posted at

updated at

Organization

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

経緯

最近realsenseの入力ノードであるrealsense-ros(intel公式)に触れる機会があったのですが、あることに気が付きました。あれ、これってrgbデータとdepthデータがセットでメッセージとして送られてないやん、と。自分は下のような状態を期待していました。
Screenshot from 2020-12-11 14-37-59.png
しかし、実際は下のようにrgbデータとdepthデータは別々のノードでpublishされていました。
Screenshot from 2020-12-11 14-41-41.png
これでは、同じ瞬間に撮影されたrgbデータとdepthデータをセットとして使いたい場合、不便です。そこで、自分が思いついた方法は2つあります。1つは同じタイムスタンプのrgbとdepthのmessageを結合してpublishする方法です。もう1つは、受け取るノード側で、同じタイムスタンプのrgbとdepthのmessageが来た場合のみcallback関数を呼ぶというものです。今回は2つ目の方法を、ROSのmessage_filtersを使ってやってみた手順を共有します。

手順

1. realsense-ros側の設定

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

base_realsense_node.cpp
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つまで同期させることができます。公式に詳しい説明が書いてあります。今回は公式通りにやってみました。

sample.cpp
#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面白くなってきました。

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

Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Sign upLogin
4
Help us understand the problem. What are the problem?