Create message filter ROS2 package



1. Create package

cd ~/robot_ws/src
ros2 pkg create --build-type ament_cmake image_laser_message_filter --dependencies rclcpp sensor_msgs std_msgs cv_bridge message_filters image_transport pcl_conversions


2. src/image_laser_message_filter.cpp

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>

using std::placeholders::_1;
using std::placeholders::_2;

class ImageLaserSyncNode : public rclcpp::Node
{
public:
  ImageLaserSyncNode()
  : Node("image_laser_sync_node")
  {
    // Subscribers using message_filters
    image_sub_.subscribe(this, "/camera_array/left/image_raw");
    lidar_sub_.subscribe(this, "/ouster/points");

    // ApproximateTime policy
    sync_ = std::make_shared<Sync>(SyncPolicy(10), image_sub_, lidar_sub_);
    sync_->registerCallback(std::bind(&ImageLaserSyncNode::callback, this, _1, _2));

    RCLCPP_INFO(this->get_logger(), "Image-LiDAR Synchronizer Initialized.");
  }

private:
  void callback(
    const sensor_msgs::msg::Image::ConstSharedPtr image_msg,
    const sensor_msgs::msg::PointCloud2::ConstSharedPtr lidar_msg)
  {
    rclcpp::Time image_stamp = image_msg->header.stamp;
    rclcpp::Time lidar_stamp = lidar_msg->header.stamp;

    RCLCPP_INFO(this->get_logger(),
                "Synchronized Messages:\n  - Image timestamp: %.3f\n  - LiDAR timestamp: %.3f",
                image_stamp.seconds(), lidar_stamp.seconds());
  }

  // Message filters
  message_filters::Subscriber<sensor_msgs::msg::Image> image_sub_;
  message_filters::Subscriber<sensor_msgs::msg::PointCloud2> lidar_sub_;

  using SyncPolicy = message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::PointCloud2>;
  using Sync = message_filters::Synchronizer<SyncPolicy>;
  std::shared_ptr<Sync> sync_;
};
 
int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<ImageLaserSyncNode>());
  rclcpp::shutdown();
  return 0;
}


3. CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(image_laser_message_filter)

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(message_filters REQUIRED)
find_package(image_transport REQUIRED)
find_package(pcl_conversions REQUIRED)


add_executable(image_laser_message_filter_node src/image_laser_message_filter.cpp)

ament_target_dependencies(image_laser_message_filter_node
  rclcpp
  sensor_msgs
  message_filters
)

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

ament_package()


4. Build and Run

$ cd ~/robot_ws
$ colcon build --packages-select image_laser_message_filter
$ source install/setup.bash
$ ros2 run image_laser_message_filter laser_message_filter_node
$ ros2 bag play {rosbag}


5. Results

You can check synchronized time stamp.

Leave a Comment