Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Haven't compiled it yet, but something like this works for me:

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

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

class OverheadCamNode : public rclcpp::Node{

    public:
        OverheadCamNode() : Node("overhead_camera_node)
        {

            rclcpp::QoS qos(10);
            auto rmw_qos_profile = qos.get_rmw_qos_profile();

            overhead_cam1_sub_.subscribe(this, "/image", rmw_qos_profile );
            camera_info_sub_.subscribe(this, "/camera_info",rmw_qos_profile);

            sync.reset(new Sync(new Sync(MySyncPolicy(10),overhead_cam1_sub_,camera_info_sub_));
            sync->registerCallback(std::bind(&OverheadCamNode::synchronized_images_callback, 
                      this, _1, _2));

        }

    private:
        void synchronized_images_callback(
            const sensor_msgs::msg::Image::ConstSharedPtr& image_1,
            const sensor_msgs::msg::CameraInfo::ConstSharedPtr& cam_info)

        {
            RCLCPP_INFO(this->get_logger(), "in CB");
        }



    message_filters::Subscriber<sensor_msgs::msg::Image> overhead_cam1_sub_;
    message_filters::Subscriber<sensor_msgs::msg::CameraInfo> camera_info_sub_;


    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image , 
    sensor_msgs::msg::CameraInfo> MySyncPolicy;
    typedef message_filters::Synchronizer<MySyncPolicy> Sync;
    std::shared_ptr<Sync> sync;

};

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