Ros2, callback with time synchronizer never called

edit

Hello,

I have a node that need to process two different images: one is the image from the camera, the other one is the depth map of the image (extrapolated through a neural network). The two images are published by 2 different topics, one just after the other (minimal difference). I'm trying to use a TimeSynchronizer with a callback to execute some operations on the images from the 2 topics at the same time, but the callback is never called.

This issue is partially related to this question here, where it was pointed out that the Synchronizer must be declared as a class member as well - so I did it. I also changed the header timestamps of the two images so that they are exactly the same, in order to be able to use exact policy for synchronization.

Here is the current code (minimal example)

node.hpp:

using ImageMsg = sensor_msgs::msg::Image;
class MonocularSlamNode : public rclcpp::Node
{
public:
    MonocularSlamNode(ORB_SLAM2::System* pSLAM, const string &strVocFile, const string &strSettingsFile, const string &strSensor);
    ~MonocularSlamNode();

private:         
    void GrabRGBD(const sensor_msgs::msg::Image::SharedPtr msgIm, const sensor_msgs::msg::Image::SharedPtr msgD);    

     message_filters::Subscriber<ImageMsg> image_sub_;
     message_filters::Subscriber<ImageMsg> depth_sub_; 

    using exact_policy = message_filters::sync_policies::ExactTime<ImageMsg,ImageMsg>;
    typedef message_filters::Synchronizer<exact_policy> Synchronizer;
    std::unique_ptr<Synchronizer> sync_;
}

node.cpp:

MonocularSlamNode::MonocularSlamNode(ORB_SLAM2::System* pSLAM, const string &strVocFile, const string &strSettingsFile, const string &strSensor)
:   Node("orbslam"),
    m_SLAM(pSLAM)
{

    // define quality of service: all messages that you want to receive must have the same
    rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default;
    custom_qos_profile.depth = 100;
    custom_qos_profile.reliability = rmw_qos_reliability_policy_t::RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
    custom_qos_profile.history = rmw_qos_history_policy_t::RMW_QOS_POLICY_HISTORY_KEEP_LAST;
    custom_qos_profile.durability = rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;

    image_sub_.subscribe(this, "camera", custom_qos_profile);
    depth_sub_.subscribe(this, "depth_map", custom_qos_profile);

    sync_.reset(new message_filters::Synchronizer<exact_policy>(exact_policy(100), image_sub_, depth_sub_));
    sync_->registerCallback(&MonocularSlamNode::GrabRGBD,this);
}

void MonocularSlamNode::GrabRGBD(const ImageMsg::SharedPtr msgIm, const ImageMsg::SharedPtr msgD)
{
    cout << "I'm here" << endl;
    }

And this is how the two topics are published in node.py:

    self.pub_image_raw = self.node.create_publisher(Image, 'camera', 100)
    self.pub_depth_map = self.node.create_publisher(Image, 'depth_map', 100)

    (...)
            timestamp = self.node.get_clock().now().to_msg()

            # Publish opencv frame using CV bridge
            msg = self.bridge.cv2_to_imgmsg(numpy.array(frame), 'bgr8')
            msg.header.frame_id = self.tf_drone
            msg.header.stamp = timestamp
            self.pub_image_raw.publish(msg)

            # Publish opencv depth map using CV bridge
            msg = self.bridge.cv2_to_imgmsg(numpy.array(depth_map), 'mono8')
            msg.header.frame_id = self.tf_drone
            msg.header.stamp = timestamp
            self.pub_depth_map.publish(msg)

The code compiles, it also runs correctly (the two topics are published), and tI also see their subscription count increase so the subscribe instructions in the .cpp work fine, however it never enters the callback. As you see from the code I'm using 100 as queue size which I'm sure is more than enough (the second image is published just a fraction of second after the first one, so they should be in the same window of the synchronizer). I've also tried this exact code using approximate_policy instead of exact_policy, but it doesn't work as well. I tried everything I could think, but the callback is ... (more)