realsense-ros의 rgb 데이터와 depth 데이터를 동기화하려고했습니다.

경위

최근 realsense의 입력 노드인 realsense-ros (인텔 공식)에 접할 기회가 있었지만, 있는 것을 깨달았습니다. 어쨌든, 이것은 rgb 데이터와 depth 데이터가 세트로 메시지로 보내지지 않습니다. 자신은 아래와 같은 상태를 기대하고 있었습니다.

그러나 실제로는 아래와 같이 rgb 데이터와 depth 데이터는 별도의 노드에서 게시되었습니다.

이것은 같은 순간에 촬영된 rgb 데이터와 depth 데이터를 세트로 사용하고 싶다면 불편합니다. 거기서, 자신이 생각해 낸 방법은 2개 있습니다. 하나는 같은 타임 스탬프의 rgb와 depth의 message를 결합하고 publish하는 방법입니다. 다른 하나는, 수신하는 노드측에서, 같은 타임 스탬프의 rgb와 depth의 message가 왔을 경우만 callback 함수를 부른다는 것입니다. 이번에는 두 번째 방법을 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 재미있게되었습니다.

실수나 질문, 의견등 있으면 부담없이 코멘트해 주세요. 열심히 대답하니까(웃음).