[ROS-python 기초] message_filter로 Timestamp 동기화하기

카메라와 라이다 센서를 캘리브레이션하는 과정에서, 같은 시간상의 센서 데이터가 필요하고 이를 위해 두 센서 간의 Timestamp를 동기화를 해야된다는 것을 알게 되었다.

message_filters는 이러한 상황에서 유용하게 사용할 수 있는, 서로 다른 메시지들을 효율적으로 동기화해주는 라이브러리이다.

message_filter를 활용한 간단한 예제를 통해서 센서 메시지를 동기화해 보자.

import rospy
import message_filters
from sensor_msgs.msg import Image, PointCloud2

def callback(image_data, pcl_data):
    # synchronized messages
    rospy.loginfo(f"Image Timestamp :{image_data.header.stamp}, PCL Timestamp : {pcl_data.header.stamp}")
    
    

def listener():
    rospy.init_node('my_synchronizer', anonymous=True)
    
    image_sub = message_filters.Subscriber('/zed2/zed_node/rgb/image_rect_color', Image)
    pcl_sub = message_filters.Subscriber('/hesai/pandar_points', PointCloud2)

    ts = message_filters.ApproximateTimeSynchronizer([image_sub, pcl_sub], queue_size=10, slop=0.1)
    ts.registerCallback(callback)

    rospy.spin()

if __name__ == '__main__':
    listener()

위 코드의 listener() 함수에서는 message_filter를 사용하여 Image 토픽 (/zed2/zed_node/rgb/image_rect_color)과 PointCloud2 토픽 (/hesai/pandar_points)을 subscribe하고, ApproximateTimeSynchronizer 함수를 통해 이미지 토픽과 포인트클라우드 토픽의 Timestamp를 synchronize한다.

ApproximateTimeSynchronizer는 시간 동기화가 완벽히 정확하지 않을 때 사용되며, slope는 그러한 시간차를 근사하기 위한 파라미터이다. (slope 파라미터 값 이내에 있는 메시지를 근사하여 동기화 수행. 초 단위로 설정된다.)

시간차를 근사할 필요가 없을 경우에는, TimeSynchronizer 함수를 사용하면 된다. synchronizer 함수의 첫 인자에는 리스트 형태로 시간 동기화할 subscriber를 넣어주면 된다.

queue_size는 각 subscriber의 메시지 큐에 저장할 수 있는 메시지 양이다.

그러면, 이러한 Synchronizer 기능을 활용해서 동일 시간상의 이미지, 포인트 클라우드 쌍으로 구성된 데이터셋을 만들 수 있다.

import rospy
import message_filters
from sensor_msgs.msg import Image, PointCloud2
from datetime import datetime
import os, cv2, numpy as np
from cv_bridge import CvBridge
import sensor_msgs.point_cloud2 as pc2
from pcl import PointCloud_PointXYZI
import pcl
from pcl import pcl_visualization

image_counter = 0
cloud_counter = 0
image_dir = "./data/image"
cloud_dir = "./data/pc"
if not os.path.exists(image_dir):
    os.makedirs(image_dir)
if not os.path.exists(cloud_dir):
    os.makedirs(cloud_dir)

bridge = CvBridge()

def callback(image, pcl_data):
    global image_counter, cloud_counter
    # synchronized messages
    rospy.loginfo(f"Image Timestamp :{image.header.stamp} // PCL Timestamp : {pcl_data.header.stamp}")

    cvimg = bridge.imgmsg_to_cv2(image, "bgr8")
    cv2.imwrite(os.path.join(image_dir, f"{image_counter:06}.png"), cvimg)
    
    gen = pc2.read_points(pcl_data, skip_nans=True, field_names=("x", "y", "z", "intensity"))
    points = []
    for p in gen:
        # check point validation
        if p[0] == p[1] == p[2] == 0:
            continue
        else :
            points.append([p[0], p[1], p[2], p[3]])

    # pcl pointcloud() x, y , z only : no intensity 
    # p = pcl.PointCloud()
    # p.from_list(points) 
    
    p = PointCloud_PointXYZI()
    p.from_list(points)
    pcl.save(p, os.path.join(cloud_dir, f"{cloud_counter:06}.pcd"))

    image_counter+=1
    cloud_counter+=1
    rospy.loginfo(f"{image_counter}-th Image, {cloud_counter}-th PCD saved . . .")
    

def listener():
    rospy.init_node('my_synchronizer', anonymous=True)
    
    image_sub = message_filters.Subscriber('/zed2/zed_node/rgb/image_rect_color', Image)
    pcl_sub = message_filters.Subscriber('/hesai/pandar_points', PointCloud2)

    ts = message_filters.ApproximateTimeSynchronizer([image_sub, pcl_sub], queue_size=10, slop=0.1)
    ts.registerCallback(callback)

    rospy.spin()

if __name__ == '__main__':
    listener()

위 코드는 ApproximateTimeSynchronizer를 통해 이미지 토픽과 포인트 클라우드 토픽을 동기화하고, 동기화 된 순간의 이미지 데이터, 포인트클라우드 데이터를 각각 ".png", ".pcd" 로 저장하는 코드이다.