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

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

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

 

1. 시간 동기화의 필요성

지난 1년 동안 몇몇 센서 데이터들을 다루며 동기화의 필요성에 대해 알게 된 것 같다....
서로 다른 센서들이 내뱉는 서로 다른 주기의 데이터들
이러한 데이터를 동기화 없이 그대로 사용하다 보니 실시간성이 중요한 곳에서 따로 노는 문제들이 발생하였다.

다양한 센서 데이터 주기

 

그래서 이를 해결하고자 ROS에서 제공하는 라이브러리 중 message_filters를 활용하기로 하였다.

(아래 사이트에는 ROS1에 대한 내용만 있어 ROS2에 적용하는데에 약간의 어려움 있었다....)

https://wiki.ros.org/message_filters

message_filters - ROS Wiki

 

message_filters 라이브러리를 통해 LiDAR 데이터와 Camera 데이터를 동기화 시킬 수 있었고,

LiDAR의 10hz 주기에 맞게 데이터를 안정적으로 처리할 수 있었다. 

---

2. 메시지 필터란?

메시지 필터는 이름에서 유추해 볼 수 있듯이 특정한 기준의 필터를 통해 메시지를 거른다고 생각하면 좋을 것 같다.

여기서 특정한 기준은 시간이다. 즉, 시간을 기준으로 동시간 대의 데이터면 사용하고 그렇지 않으면 패스하는 방식인 것이다.

우리는 이 메시지 필터를 이용해서 데이터를 동기화 할 것이다.

---

3. 메시지 필터의 종류

메시지 필터 종류

 

메시지 필터에는 크게 위 두 가지 방식이 있는 것으로 보인다.

 

먼저 ExactTime Policy의 경우,

어느 정도의 오차인지는 모르겠지만 거의 완전 유사한 시간대가 아니라면 전부 버려지는 것 같다.

 

그리고 현재 나의 거의 모든 코드에 포함되어 있는 방식인 Approximate Time Policy의 경우,

큐 사이즈라는 어느 정도 유도리 있게 사용하겠다는 값을 설정하여 어느 정도의 시간 차이는 눈감아 주는 방식이 있다.

---

4. 메시지 필터를 사용하기 위해?

 

메시지 필터를 사용하기 위해 Time stamp를 모든 데이터를 발행(pub)할 때 Header 부분에 포함 시켜줘야 한다.

사용하고자 하는 데이터 타입에 타임 스탬프를 포함할 수 있는지 검색해보면 다음과 같이 헤더가 있는 경우도 있고

없는 경우도 있을 것이다. 나는 그냥 최대한 커스텀 메시지를 만들지 않고 코드를 작성하고 싶었기에  다른 메시지

타입에 쑉쑉 타임 스탬프와 데이터를 끼워넣는 식으로 사용하였다.

헤더 유무

 

혹시 나만의 데이터 타입을 만들고 타임 스탬프도 만들고 싶으면 다음 사이트를 참고해서

나만의 커스텀 메시지를 만들어 보는 것도 좋은 방법이 될 것 같다.

https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Custom-ROS2-Interfaces.html

Creating custom msg and srv files — ROS 2 Documentation: Humble documentation

---

5. 메시지 필터 사용 방법

사용 방법

 

완벽하진 않지만 현재 이해한 바로는 위와 같은 순서로 코드를 작성하면 무난하게 시간 동기화 가능한 것으로

확인되긴 하는데 아직 공부가 더 필요한 상황....어쨌든 동작은 한다!!

 

---

6. message_filters를 사용하기 위한 준비의 준비 코드

6번에서 작성하는 코드는 time stamp가 포함된 두 데이터를 퍼블리시 하는 예제 코드이다.

 

6.1 패키지 만들기

ros2 pkg create msg_sync --build-type ament_cmake --dependencies rclcpp std_msgs message_filters

 

 

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

#include <rclcpp/rclcpp.hpp>
#include <chrono>
#include <memory>
#include <functional>
#include <std_msgs/msg/header.hpp>

class MsgPub : public rclcpp::Node
{
public:
    MsgPub();
    
private:
    void msg_publish();

    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::Header>::SharedPtr first_msg_;
    rclcpp::Publisher<std_msgs::msg::Header>::SharedPtr second_msg_;

};

 

메세지 타입은 std_msgs::msg::Header이고 다음과 같이 구성되어 있다.

std_msgs::msg::Header 설명

 

코드에서는 Header의 stamp 부분에 time stamp 데이터를 넣고
frame_id 부분에 메시지 구분을 위해 내용을 추가하였다.

 

6.3 src 폴더에서 msg1_pub.cpp, msg2_pub.cpp 생성 및 다음 코드 붙여넣기

#include "msg_sync/msg_pub.hpp"

MsgPub::MsgPub()
: Node("msg1_publisher")
{
    first_msg_ = this->create_publisher<std_msgs::msg::Header>("msg_1", 1);
    timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&MsgPub::msg_publish, this));

    RCLCPP_INFO(this->get_logger(), "node starts");
}

void MsgPub::msg_publish()
{
    auto msg1 = std_msgs::msg::Header();
    msg1.frame_id = "first_msg";
    msg1.stamp = this->get_clock()->now();

    first_msg_->publish(msg1);
}

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

#include "msg_sync/msg_pub.hpp"

MsgPub::MsgPub()
: Node("msg2_publisher")
{
    second_msg_ = this->create_publisher<std_msgs::msg::Header>("msg_2", 1);
    timer_ = this->create_wall_timer(std::chrono::milliseconds(33), std::bind(&MsgPub::msg_publish, this));

    RCLCPP_INFO(this->get_logger(), "node starts");
}

void MsgPub::msg_publish()
{
    auto msg2 = std_msgs::msg::Header();
    msg2.frame_id = "second_msg";
    msg2.stamp = this->get_clock()->now();

    second_msg_->publish(msg2);
}

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

 

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

include_directories(include)

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

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

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

 

 

6.5 빌드해서 데이터 publish 되는지 확인하기

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

# 실행하기
ros2 run msg_sync msg1_pub
ros2 run msg_sync msg2_pub

# 데이터 확인하기
ros2 topic echo /msg_1
ros2 topic echo /msg_2

실행 결과

 

7. message_filters 진짜 진짜 사용하기

https://narmstudy.tistory.com/5

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