Ros message_filter

이 글은 http://wiki.ros.org/message_filters를 참고(및 번역)하였음을 밝힙니다.

message_filter는 이름 그대로 메시지를 필터링 합니다. 메시지를 필터링한다는 것은  특정 메시지만 통과시키고 그 이외의 메시지는 통과하지 않게 하는 것입니다. 메시지 필터링 input은 아래와 같이 두가지 방법으로 설정합니다. 일반적인 형식과 같이 괄호 안에 input값을 넣거나 connectInput함수의 인자로 파라미터를 넘길 수 있습니다. 

FooFilter foo;
BarFilter bar(foo);
or 
FooFilter foo, bar;
bar.connectInput(foo);

필터링 결과는 필터링 후 메소드를 호출하여 사용 할 수 있습니다. 이 경우 필터링을 통과한 메시지를 받으면 registercallback함수의 인자로 메소드를 호출하면 메소드의 파라미터로 필터링 결과값이 넘어갑니다. 사용 방법은 아래와 같습니다.

bar.registerCallback(myCallback); 

만약 필터링 결과중 두개 이상의 메시지를 파라미터로 넘기고 싶다면  메소드와 파라미터 인자를 를 바인딩하여 registerCallback의 인자로 넘기면 됩니다. 방법은 다음과 같습니다. 

bar.registerCallback(boost::bind(&callback, _1, _2));

메시지필터는 subscriber로 ROS 토픽을 읽어와 데이터를 필터링합니다.  Subscriber의 파라미터로는 ros::NodeHandle, topic, buffer_size가 입력되어야 하며 선언시 읽어올 메시지 타입을 정의해 주어야 합니다.  

message_filters::Subscriber sub(nh, "my_topic", 1);

TimeSynchronizer의 필터는 헤더에 포함된 타임 스탬프에 의한 수신 채널을 동기화하여 수신된 채널을 단일 콜백의 형태로 출력한다. (C ++ 사용시 최대 9 개의 채널을 동기화 할 수 있다.) 아래와 같은 방법으로 사용할 수 있습니다.

message_filters::Subscriber<Image> image_sub(nh, "image", 1);

message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);

TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 10);

sync.registerCallback(boost::bind(&callback, _1, _2));

 

TimeSequencer filter는 받은 메시지를 stamp의 시간에 따라 우선순위 큐에서 삽입하고 일정시간 동안 delay 시킨 후 메시지가 만료되었을때 callback을 호출합니다. 이미 만료되어 메소드를 호출 한 메시지보다 이전 메시지가 오면 이전 메시지는 폐기됩니다.

Cache는 받은 메시지를 원형 큐 buffer에 기록하여 차후에 지난 메시지를 불러올 수 있게 합니다.

 

Policy-Based Synchronizer

message-filter를 사용하면 서로 다른 여러 메시지를 동기화 하여 사용할 수 있습니다. 즉, 같은 time stamp를 갖은 메시지를 한번에 받아 처리하도록 합니다 . 이 작업을 수행하기 위해서는 작업에 따른 policy가 필요하다. 이 policy는 Approximate Time policy와 Exact Time policy 두개가 있다. Exact Time policy는 header의 stamp time-stamp가 정확히 일치하는 경우에만 메시지를 하나의 채널로 보내도록 하며 Approximate Time policy는 “an adaptive algorithm”을 사용하여 time-stamp가 달라도 메시지를 통과시킵다.

 

TEST Example code

test_scenario

 

test_code.cpp

 

#include <message_filters/sync_policies/approximate_time.h>

#include <sync_time/H_image.h>

#include <message_filters/subscriber.h>

#include <message_filters/time_synchronizer.h>

#include <sensor_msgs/Image.h>

 

using namespace sensor_msgs;

using namespace message_filters;

 

void waitColorImage(const ImageConstPtr& image)

{

  ros::NodeHandle n;

  ros::Publisher pub = n.advertise<sync_time::H_image>("/h_color_image", 5);

  sleep(0.6);

  

  sync_time::H_image msg;

  msg.header.stamp = ros::Time::now();

  msg.header.frame_id = "/color_image";

  msg.image = *image;

  pub.publish(msg);

}

void waitDepthImage(const ImageConstPtr& image)

{

  ros::NodeHandle n;

  ros::Publisher pub = n.advertise<sync_time::H_image>("/h_depth_image", 100);

  sleep(0.8);

  

  sync_time::H_image msg;

  msg.header.stamp = ros::Time::now();

  msg.header.frame_id = "/depth_image";

  msg.image = *image;

  pub.publish(msg);

}

 

void callback(const ImageConstPtr& color_image, const ImageConstPtr& depth_image)

{

  std::cout<<"in callback"<<"\n";

  // Solve all of perception here...

  std::cout<< color_image->header.stamp <<"\n";

  std::cout<< depth_image->header.stamp <<"\n";

 

   waitColorImage(color_image);

   waitDepthImage(depth_image);

}

typedef sync_policies::ApproximateTime<Image, Image> MySyncPolicy;

 

void mycallback(const sync_time::H_imageConstPtr& msg)

{

  std::cout <<msg->header.stamp<<"\n";

  std::cout <<msg->image.header.stamp<<"\n";

}

int main(int argc, char** argv)

{

  ros::init(argc, argv, "vision_node");

 

  ros::NodeHandle nh;

 

  message_filters::Subscriber<Image> color_image_sub(nh, "/kinect2/hd/image_color",100);

  message_filters::Subscriber<Image> depth_image_sub(nh, "/kinect2/qhd/image_depth_rect", 100);

  

  Synchronizer<MySyncPolicy> sync(MySyncPolicy(1000), color_image_sub, depth_image_sub);

  sync.registerCallback(&callback);

 

  ros::Subscriber sub = nh.subscribe("h_color_image", 1000, mycallback);

  ros::spin();

 

  return 0;

}

 

 

sync_time/H_image.msg

 

Header header

sensor_msgs/Image image