[ROS2] 데이터 시간동기화 C++로 작성하기 #2

해당 글에서는 서로 다른 토픽으로 부터 데이터를 받아 message_filters를 통해 시간 동기화하는
방법 및 예제 코드를 포함하고 있습니다.
(예제 코드는 페이지 아래에 있습니다.)

[테스트 환경]
os : ubuntu 20.04 | ubuntu 22.04
ros : ros2 foxy | ros2 humble

https://narmstudy.tistory.com/4

ROS2 데이터 시간동기화 C++로 작성하기 #1

 

7. message_filters 진짜 진짜 사용하기

7번에서 작성하는 코드는 6번에서 작성된 코드가 pub 하는 데이터를 sub 받아 데이터를 동기화 하는 예제 코드이다.

 

 

7.1 include 폴더에서 msg_sync.hpp 생성 및 다음 코드 붙여넣기

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/header.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>

using StampedHeaderMsg = std_msgs::msg::Header;
using StampedHeaderMsgSubscriber = message_filters::Subscriber<StampedHeaderMsg>;
using ApproximateSyncPolicy = message_filters::sync_policies::ApproximateTime<StampedHeaderMsg, StampedHeaderMsg>;
using ApproximateSync = message_filters::Synchronizer<ApproximateSyncPolicy>;

class MsgSync : public rclcpp::Node
{
public:
    MsgSync();

private:
    message_filters::Subscriber<StampedHeaderMsg> msg1_sub_;
    message_filters::Subscriber<StampedHeaderMsg> msg2_sub_;
    ApproximateSync approximate_sync_;

    void approximateSyncCallback(
    	const StampedHeaderMsg& msg1,
    	const StampedHeaderMsg& msg2
    );
};

 

 

7.2 src 폴더에서 msg_sync.cpp 생성 및 다음 코드 붙여넣기

#include "msg_sync/msg_sync.hpp"
#include <iostream>

MsgSync::MsgSync() :
    Node("msg_sync_node"),
    msg1_sub_(this, "/msg_1"),
    msg2_sub_(this, "/msg_2"),
    approximate_sync_(ApproximateSyncPolicy(5), msg1_sub_, msg2_sub_)
{
    approximate_sync_.registerCallback(&MsgSync::approximateSyncCallback, this);
}


void MsgSync::approximateSyncCallback(
    const StampedHeaderMsg& msg1,
    const StampedHeaderMsg& msg2)
{
    std::cout << "Message 1: [Timestamp: " << msg1.stamp.sec << "." << msg1.stamp.nanosec << "]" << std::endl;
    std::cout << "Message 2: [Timestamp: " << msg2.stamp.sec << "." << msg2.stamp.nanosec << "]" << std::endl;
}

int main(int argc, char * argv[])
{
 rclcpp::init(argc, argv);
 auto node = std::make_shared<MsgSync>();
 rclcpp::spin(node);
 rclcpp::shutdown();
 return 0;
}

 

 

7.3 CmakeLists.txt에 다음 내용 추가하기

include_directories(include)

add_executable(msg1_pub src/msg1_pub.cpp)
ament_target_dependencies(msg1_pub rclcpp std_msgs)

add_executable(msg_pub2 src/msg2_pub.cpp)
ament_target_dependencies(msg_pub2 rclcpp std_msgs)

add_executable(msg_sync src/msg_sync.cpp)
ament_target_dependencies(msg_sync rclcpp std_msgs message_filters)

install(TARGETS
msg1_pub
msg1_pub
msg_sync
DESTINATION lib/${PROJECT_NAME}
)

 

 

7.4 패키지 빌드 및 확인하기

# 패키지 빌드하기
colcon build --packages-select msg_sync && source install/setup.bash

======[1,2번 터미널]======
# 실행하기
ros2 run msg_sync msg1_pub
ros2 run msg_sync msg2_pub

======[3,4번 터미널]======
# 데이터 확인하기
ros2 topic echo /msg_1
ros2 topic echo /msg_2

======[5번 터미널]======
# 동기화 여부 확인하기
ros2 run msg_sync msg_sync

 

최종 결과

 

최종 동기화 결과를 보면 유사한 시간대의 데이터가 출력되는 것을 볼 수 있다!

 

[코드]

https://github.com/hi-g1/ros2_message_filters_example.git

GitHub - hi-g1/ros2_message_filters_example