ROS Message filter 메시지 필터

지토·2023년 2월 6일

캡디_객체 이름을 포함하는 2D 지도 생성 SLAM

목록 보기

2/3

1. Message filter 메시지 필터란?

메시지가 메시지 필터에 도달하면 즉시 출력되지 않고 특정 조건이 충족되면 나중에 출력될 수 있습니다.

예를 들어 LiDAR , 카메라 등 여러가지 센서를 사용해 로봇 프로젝트를 수행할 때, 두 개 이상의 메시지 내용을 포함하는 알고리즘을 설계한다고 가정합시다.

두 토픽의 발행 주기가 다를 경우에 개중 느린 토픽의 주기에 맞추어 callback 함수를 작동시켜야 할 것입니다.

따라서 message filter 함수는 두 개 이상의 토픽을 인자로 받아 조건이 충족될 경우에만 callback 함수 loop 안에 진입합니다.

Turtlebot SLAM with Yolo Object Detection 의 경우 다섯 개의 토픽이 메시지 필터에 들어간다는 것을 의미합니다.

2. 필터의 종류

ROS message filter 는 다양한 필터를 지원합니다. 가장 일반적으로 사용되는 것은 Time Synchronizer 와 Approximate Time Synchronizer 입니다. 각각 절대 시간 동기화가 필요한 경우에 사용하고 시간이 오차 범위 이내에 들어왔을 때 작동시킵니다.

1) Time Synchronizer

절대 시간 동기화가 필요한 경우에 사용합니다. 타임 스탬프가 완벽히 같을 때만 동작합니다.

2) Approximate Time Synchronizer

서로 다른 센서의 타임스탬프가 완전히 동일한 것은 때로 불가능할 수 있습니다. 따라서 ApproximateTime Synchronizer 는 시간이 오차 범위 이내에 들어왔을 때 작동시킵니다.

5. 구현

이 문서에서는 C++ ROS 패키지에 예시로 Message filter 와 특히 Approximate Time Synchronizer 를 포함하는 방법을 소개합니다.

ROS 패키지는 노드 핸들러, 서브스크라이버, 메시지 필터, 메시지 싱크로나이저, 메시지 싱크로나이저 프리셋들을 포함합니다.

메시지 싱크로나이저 프리셋은 ApproximateTimeSyncPolicy 로 정의되어 있습니다.

이를 통해 다섯 개의 토픽과 각각의 콜백 함수들을 연결할 수 있습니다.

#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

using namespace message_filters;

class YoloTurtle{
private:
	ros::NodeHandle nh_;
	typedef sync_policies::ApproximateTime
		<
		Image,
		CameraInfo,
		LaserScan,
		darknet_ros_msgs::BoundingBoxes
		//이 다섯 개의 토픽이 message filter 에 들어간다는 것을 의미합니다.
		>
		MySyncPolicy;
	typedef Synchronizer<MySyncPolicy> Sync;
	boost::shared_ptr<Sync> sync_;

public:
	YoloTurtle()
		: it_(nh_)
	{
		image_sub_.subscribe(nh_, image_topic, 1); //카메라 스트리밍 이미지
		info_sub_.subscribe(nh_, info_topic, 1); //카메라 information
		scan_sub_.subscribe(nh_, scan_topic, 1); //라이다 스캔 데이터
		bounding_boxes_sub_.subscribe(nh_, bounding_boxes_topic, 1); //darknet_ros 에서 나온 bounding box 데이터

		sync_.reset(new Sync(MySyncPolicy(80),
                         image_sub_,info_sub_, scan_sub_, bounding_boxes_sub_));

		sync_->registerCallback(boost::bind(&YoloTurtle::callback,
                                        this, _1, _2, _3,_4));
		//여기서 요건이 맞으면 callback 함수가 실행됩

3. 입력과 출력

1) 입력

입력은 필터의 생성자 또는 connectInput() 메소드를 통해 연결됩니다.

2) 출력

출력은 registerCallback() 메소드를 통해 연결됩니다.

4. 사용 예시

저의 프로젝트 “Turtlebot SLAM with Yolo Object Detection” 에서 message filter ApproximateTime 을 사용하고 있는 예시입니다.

전체 코드 설명은 이 포스팅을 참고해 주세요.

#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

using namespace message_filters;

class YoloTurtle{
private:
	ros::NodeHandle nh_;
	typedef sync_policies::ApproximateTime
		<
		Image,
		CameraInfo,
		LaserScan,
		darknet_ros_msgs::BoundingBoxes
		//이 다섯 개의 토픽이 message filter 에 들어간다는 것을 의미합니다. 
		>
		MySyncPolicy; 
	typedef Synchronizer<MySyncPolicy> Sync;
	boost::shared_ptr<Sync> sync_;

public:
	YoloTurtle()
		: it_(nh_)
	{
		image_sub_.subscribe(nh_, image_topic, 1); //카메라 스트리밍 이미지
		info_sub_.subscribe(nh_, info_topic, 1); //카메라 information
		scan_sub_.subscribe(nh_, scan_topic, 1); //라이다 스캔 데이터
		bounding_boxes_sub_.subscribe(nh_, bounding_boxes_topic, 1); //darknet_ros 에서 나온 bounding box 데이터

		sync_.reset(new Sync(MySyncPolicy(80), 
                         image_sub_,info_sub_, scan_sub_, bounding_boxes_sub_));

		sync_->registerCallback(boost::bind(&YoloTurtle::callback,
                                        this, _1, _2, _3,_4));
		//여기서 요건이 맞으면 callback 함수가 실행됩니다. 
	}	

	void callback(
		const ImageConstPtr &image_msg,
		const CameraInfoConstPtr &info_msg,
		const LaserScanConstPtr &scan_msg,
		const darknet_ros_msgs::BoundingBoxesConstPtr &bounding_boxes_msg)
	)
	{
		cv::Mat image;
    cv_bridge::CvImagePtr input_bridge;
    cout << "inside callback" << endl;
	}

	try
	{
		
	}
}