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).

This is the implementation of the node, relevant parts only:

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();

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

private: 

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

node.cpp:

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

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

typedef message_filters::sync_policies::ApproximateTime<ImageMsg, ImageMsg> approximate_policy;
message_filters::Synchronizer<approximate_policy>syncApproximate(approximate_policy(10), image_sub_, depth_sub_);
syncApproximate.setMaxIntervalDuration(rclcpp::Duration(100,0)); 
syncApproximate.registerCallback(&MonocularSlamNode::GrabRGBD,this);
}

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

The problem is, the callback is never executed.

The two topics are published correctly, and if I use ros2 topic info I see they are both correctly subscribed - and if I use echo, I see their timestamps differ only by a fraction of a second (so by using the large value I'm using to test, 100, in setMaxIntervalDuration should work? Before I tried smaller values, like 1 and 3 seconds, but nothing works).

Just for reference, this is how the two topics are created in my node.py file:

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

and then published:

        msg = self.bridge.cv2_to_imgmsg(numpy.array(frame), 'bgr8')
        msg.header.frame_id = self.tf_drone
        msg.header.stamp = self.node.get_clock().now().to_msg()
        self.pub_image_raw.publish(msg)

        msg = self.bridge.cv2_to_imgmsg(numpy.array(depth_map), 'mono8')
        msg.header.frame_id = self.tf_drone
        msg.header.stamp = self.node.get_clock().now().to_msg()
        self.pub_depth_map.publish(msg)

Both of them work fine, as I can visualize the camera image and the depth map as well.

Can someone help me understand why my callback is never called? Thank you!

EDIT: I also tried to rewrite my node.py as follows:

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

        # Publish opencv frame using CV bridge
        msg.header.frame_id = self.tf_drone
        msg.header.stamp = timestamp
        self.pub_image_raw.publish(msg)

        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)

So that the images published by the 2 topics have exactly the same timestamp. Hence, I changed the implementation of the node as follows:

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

typedef message_filters::sync_policies::ExactTime<ImageMsg, ImageMsg> exact_policy;
message_filters::Synchronizer<exact_policy>syncExact(exact_policy(1000), image_sub_, depth_sub_);
syncExact.registerCallback(&MonocularSlamNode::GrabRGBD,this);

Nevertheless, when I run it, it never enters the callback. How is this possible? The two messages have the exact same timestamp...

MonocularSlamNode::MonocularSlamNode(...) ...
{

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

    typedef message_filters::sync_policies::ApproximateTime<...> approximate_policy;
    message_filters::Synchronizer<approximate_policy>syncApproximate(...);
    ...
}

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

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