ROS 2でmessage_filterを使用するサンプル

概要　

ROS 2でmessage_filtersを使いたいときにネット上に正常に動くサンプルコードがなかったので、なるべく分かりやすくて実用性のあるサンプルコードを作成する。

message_filtersの概要

複数のトピックからのメッセージを同期的に処理するためのライブラリ。
ロボットのようなリアルタイムシステムでは、複数のセンサからのデータを同時に取得し、それらのデータを使用して処理を行うことが頻繁にあり、message_filtersを使うとそれらのデータを同期して処理することができる。

message_filtersには以下の特徴がある

• 複数のソースからのメッセージ同期: 異なるトピックやソースからのメッセージを時間的に同期して、単一のコールバック関数で処理することができる。

• 様々な同期ポリシー: 厳密な同期や近似的な同期など複数の同期ポリシーを選択でき、用途に応じた同期ポリシーを選択できる。

message_filtersの説明はこちらの記事も参考になります。

作成したサンプル

realsense_rosからのRBG画像トピック(/camera/color/image_raw)　とDepth画像トピック(/camera/aligned_depth_to_color/image_raw)をSbscribe して横に並べて合成した画像を作成し Publishする。

作成したコード

実際にROS2 Humbleでcolcon buildして動いたやつ。callbackの中身は適当。

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>
#include <opencv2/core/core.hpp>

class ImageCombiner : public rclcpp::Node {
public:
  message_filters::Subscriber<sensor_msgs::msg::Image> rgb_sub_;
  message_filters::Subscriber<sensor_msgs::msg::Image> depth_sub_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr combined_image_pub_;
  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image> approximate_policy;
  message_filters::Synchronizer<approximate_policy> sync_;

  ImageCombiner()
      : Node("image_combiner_node"),
        sync_(approximate_policy(10), rgb_sub_, depth_sub_) {
    rgb_sub_.subscribe(this, "/camera/color/image_raw");
    depth_sub_.subscribe(this, "/camera/aligned_depth_to_color/image_raw");
    combined_image_pub_ = this->create_publisher<sensor_msgs::msg::Image>("/output_image", 10);

    sync_.registerCallback(&ImageCombiner::topic_callback, this);
  }

public:
  void topic_callback(const sensor_msgs::msg::Image::SharedPtr rgb_image, const sensor_msgs::msg::Image::SharedPtr depth_image) {
    // Convert ROS Image messages to OpenCV Mat
    cv::Mat rgb_cv_image = cv_bridge::toCvShare(rgb_image, "bgr8")->image;
    cv::Mat depth_cv_image = cv_bridge::toCvShare(depth_image, "16UC1")->image;

    // Normalize depth image for visualization
    cv::normalize(depth_cv_image, depth_cv_image, 0, 255, cv::NORM_MINMAX, CV_8UC1);

    // Convert normalized 8-bit depth image to 3-channel for visualization
    cv::cvtColor(depth_cv_image, depth_cv_image, cv::COLOR_GRAY2BGR);

    // Combine the images side by side
    cv::Mat combined_cv_image;
    cv::hconcat(rgb_cv_image, depth_cv_image, combined_cv_image);

    // Convert combined OpenCV Mat to ROS Image message
    sensor_msgs::msg::Image::SharedPtr combined_image = cv_bridge::CvImage(rgb_image->header, "bgr8", combined_cv_image).toImageMsg();
    combined_image_pub_->publish(*combined_image);
    RCLCPP_INFO(this->get_logger(), "Combined image published");
  }
};

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

CMakeLists.txt

details

cmake_minimum_required(VERSION 3.5)
project(message_filters_sample_ros2)

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(message_filters REQUIRED)
find_package(OpenCV REQUIRED)

include_directories(include)

add_executable(message_filters_sample src/message_filters_sample.cpp)

ament_target_dependencies(message_filters_sample
  rclcpp
  sensor_msgs
  cv_bridge
  message_filters
  OpenCV
)

install(TARGETS
  message_filters_sample
  DESTINATION lib/${PROJECT_NAME})

ament_package()

実行

realsense_rosの実行

ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true

ros2 run message_filters_sample_ros2 message_filters_sample 

余談
こんなことしなくてもRGBD.msg
というメッセージ型で時刻同期された画像が取得できるようになったようです。

RealSenseの時刻同期したRGBD画像をSubscribeする(realsense2_camera_msgs)
https://ar-ray.hatenablog.com/entry/2023/11/30/083000

各Sync Policyの説明

ApproximateEpsilonTime、LatestTimeはIronで追加された機能。(説明はChatGPTで要約したので間違っているかも)

ExactTime

ExactTimeポリシーは、メッセージが完全に同じタイムスタンプを持つ場合にのみメッセージを同期させるために使用されます。この厳密な時間同期は、高度な同時性が要求される場合に適していますが、センサーからのデータが完全に一致するタイミングでないとメッセージを処理しません。

ApproximateTime

ApproximateTimeポリシーは、タイムスタンプが完全に一致しなくても近い時間内にあるメッセージを同期させるために使用されます。これは、異なるセンサーからのデータが完全に一致するタイミングを持たないが、ある程度近い時間内に取得される場合に適しています。このポリシーは、実用上はより柔軟性があり、多くの実際のシナリオに適用可能です。

ApproximateEpsilonTime

ApproximateEpsilonTimeポリシーは、ExactTimeのように動作しますが、指定された許容範囲（イプシロン）内のタイムスタンプの差異を許容します。これにより、厳密な同期が必要ながらも、わずかなタイムスタンプのずれが許容される場合に有効です。イプシロンの値はユーザーが定義でき、より柔軟な同期が可能になります。

LatestTime

LatestTimeポリシーは、最大9つまでのメッセージをそれぞれのレートで同期させることができる新しいタイムポリシーです。ゼロオーダーホールドを用いてアップサンプリングすることで、最新のメッセージを取得し、それらを同期させます。これは、複数のセンサーからの最新のデータを効果的に組み合わせたい場合に有効です。

おわりに

RealSenseの画像を同期してSubscribeしてcallbackを実行するサンプルを作成した。

参照するコードによって記法がバラバラなので、ROS2のドキュメントにmessage_filtersの項目を追加して記法を統一するようにしてほしい。

後日 ROS2 Iron環境でSync Policyを変更した時のcallbackの実行タイミングを調査して追記したい。

参考

Iron での新機能について