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.