Synchronization issues between image_raw and camera_info in ROS2 AprilTag with RealSense D435

[m11112089]

Hello, I am experiencing a synchronization issue when using the ROS2 AprilTag package with a RealSense camera. The warning suggests that the image messages and CameraInfo messages are not properly synchronized.

kai@4070:~/ros2_ws$ ros2 run apriltag_ros apriltag_node --ros-args     -r image_rect:=/camera/camera/color/image_raw     -r camera_info:=/camera/camera/color/camera_info     --params-file `ros2 pkg prefix apriltag_ros`/share/apriltag_ros/cfg/tags_36h11.yaml
[WARN] [1715749104.420966646] [apriltag]: [image_transport] Topics '/camera/camera/color/image_raw' and '/camera/camera/color/camera_info' do not appear to be synchronized. In the last 10s:
    Image messages received:      7
    CameraInfo messages received: 30
    Synchronized pairs:           7
[WARN] [1715749121.421794903] [apriltag]: [image_transport] Topics '/camera/camera/color/image_raw' and '/camera/camera/color/camera_info' do not appear to be synchronized. In the last 10s:
    Image messages received:      5
    CameraInfo messages received: 30
    Synchronized pairs:           5
^C[INFO] [1715749288.887109937] [rclcpp]: signal_handler(signum=2)

This is my video.

https://github.com/christianrauch/apriltag_ros/assets/114904643/64c66fe4-e9f0-4f40-a4b6-b5c3ae6f177a

This is my yaml file.

/**:
    ros__parameters:
        image_transport: raw    # image format
        family: 36h11           # tag family name
        size: 0.173             # tag edge size in meter
        max_hamming: 0          # maximum allowed hamming distance (corrected bits)

        # see "apriltag.h" 'struct apriltag_detector' for more documentation on these optional parameters
        detector:
            threads: 1          # number of threads
            decimate: 2.0       # decimate resolution for quad detection
            blur: 0.0           # sigma of Gaussian blur for quad detection
            refine: True        # snap to strong gradients
            sharpening: 0.25    # sharpening of decoded images
            debug: False        # write additional debugging images to current working directory

        # optional list of tags
        # tag:
        #     ids: [9, 14]            # tag ID
        #     frames: [base, object]  # optional frame name
        #     sizes: [0.162, 0.162]   # optional tag-specific edge size

This is my bag file. https://drive.google.com/file/d/157_6F8xu8NOlGZZjppNdDwQOWVVKNb27/view?usp=sharing

[christianrauch]

The node is using the image_transport::CameraSubscriber with the rmw_qos_profile_sensor_data QoS profile. Can you check that the QoS profiles of the publisher and subscriber are compatible (ros2 topic -v)? See https://docs.ros.org/en/rolling/Concepts/Intermediate/About-Quality-of-Service-Settings.html for the QoS compatibility matrix.

If the QoS profile are not compatible, then you either need to adjust the settings in the camera node or the CameraSubscriber should use a setting that is better compatible with the defaults of other camera nodes.

[m11112089]

This is my 'ros2 topic -v' result. The policies don't seem to be in conflict.🤔

kai@4070:~/ros2_ws$ ros2 topic info /camera/camera/color/image_raw -v
Type: sensor_msgs/msg/Image

Publisher count: 1

Node name: camera
Node namespace: /camera
Topic type: sensor_msgs/msg/Image
Endpoint type: PUBLISHER
GID: 01.0f.4b.0a.72.57.32.5e.01.00.00.00.00.00.15.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: TRANSIENT_LOCAL
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 2

Node name: _NODE_NAME_UNKNOWN_
Node namespace: _NODE_NAMESPACE_UNKNOWN_
Topic type: sensor_msgs/msg/Image
Endpoint type: SUBSCRIPTION
GID: 01.0f.4b.0a.25.28.b4.6d.01.00.00.00.00.00.21.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Node name: apriltag
Node namespace: /
Topic type: sensor_msgs/msg/Image
Endpoint type: SUBSCRIPTION
GID: 01.0f.4b.0a.5d.65.da.c4.01.00.00.00.00.00.12.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

kai@4070:~/ros2_ws$ ros2 topic info /camera/camera/color/camera_info -v
Type: sensor_msgs/msg/CameraInfo

Publisher count: 1

Node name: camera
Node namespace: /camera
Topic type: sensor_msgs/msg/CameraInfo
Endpoint type: PUBLISHER
GID: 01.0f.4b.0a.72.57.32.5e.01.00.00.00.00.00.16.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 2

Node name: _NODE_NAME_UNKNOWN_
Node namespace: _NODE_NAMESPACE_UNKNOWN_
Topic type: sensor_msgs/msg/CameraInfo
Endpoint type: SUBSCRIPTION
GID: 01.0f.4b.0a.25.28.b4.6d.01.00.00.00.00.00.22.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Node name: apriltag
Node namespace: /
Topic type: sensor_msgs/msg/CameraInfo
Endpoint type: SUBSCRIPTION
GID: 01.0f.4b.0a.5d.65.da.c4.01.00.00.00.00.00.13.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

[christianrauch]

Are you sending data over network? Since you are receiving many more "info" than "image" messages, could it be that this is a network/bandwidth issue? Can you test if the "compressed" image transport works better?

[m11112089]

I just using another computer confirmed with ROS1 that the ReslSense D435 topic frequencies (image_raw & camera_info)are all 30 fps.

I don't know why there is such a big difference between the ROS2 topic frequencies. I'll check out the RealSense issue.

P.S. The devices are all running locally.

Thank you very much for your answer.

[m11112089]

I'm currently using "message_filters" to temporarily solve the problem of unequal frequency of the camera topic. AprilTag is working now.

https://github.com/m11112089/apriltag_ros.git

#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <image_transport/subscriber_filter.hpp>
class AprilTagNode : public rclcpp::Node {
private:
    // const image_transport::CameraSubscriber sub_cam;
    image_transport::SubscriberFilter image_subscriber_;
    // Message filters subscriber for camera info
    message_filters::Subscriber<sensor_msgs::msg::CameraInfo> camera_info_subscriber_;
    // Synchronizer
    using SyncPolicy = message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo>;
    using Synchronizer = message_filters::Synchronizer<SyncPolicy>;
    std::shared_ptr<Synchronizer> synchronizer_;
}
AprilTagNode::AprilTagNode(const rclcpp::NodeOptions& options){
image_subscriber_.subscribe(this, declare_parameter("image_rect", "raw", descr({}, true)), "raw", rclcpp::SensorDataQoS().get_rmw_qos_profile());
    camera_info_subscriber_.subscribe(this, declare_parameter("camera_info", "raw", descr({}, true)));
    synchronizer_ = std::make_shared<Synchronizer>(SyncPolicy(10), image_subscriber_, camera_info_subscriber_);
    synchronizer_->registerCallback(std::bind(&AprilTagNode::onCamera, this, std::placeholders::_1, std::placeholders::_2));
}

[christianrauch]

Since you are comparing with the ROS 1 node, do you not get the same frame rate on the info and image topics for the ROS 2 node?

[m11112089]

https://github.com/IntelRealSense/realsense-ros/issues/2961

Unfortunately, after reviewing the related issues with RealSense, I found that the problem of differing frame rates between image_raw and camera_info cannot be resolved at this time. The ROS2 AprilTag package does not function properly when image_raw and camera_info have different frame rates.

My current solution is to use message filters to handle the differing publication rates of image_raw and camera_info.

Alternatively, in the future, it might be beneficial to subscribe to image_raw and camera_info separately to avoid similar issues.

[christianrauch]

The ROS2 AprilTag package does not function properly when image_raw and camera_info have different frame rates.

Can you specify what exactly is happening and what you would expect? From your screencast from above, it is clear that you do not get the full framerate for the image topic. Since the image framerate is limited to 7 Hz, this is the maximum the CameraSubscriber can synchronise and the AprilTagNode can process. Do you not get something close to 7 Hz of tag detection rate?

My current solution is to use message filters to handle the differing publication rates of image_raw and camera_info.

The CameraSubscriber is also just using image_transport::SubscriberFilter for the images and the message_filters::Subscriber for the camera info. However, it is using the message_filters::TimeSynchronizer which uses the sync_policies::ExactTime policy for exact synchronising via matching time stamps.

Instead, you are proposing the sync_policies::ApproximateTime, which relaxes this constraint and allows delays between the image and the camera info. Do your images and the camera info message not have the same timestamp?

Alternatively, in the future, it might be beneficial to subscribe to image_raw and camera_info separately to avoid similar issues.

Do you mean without synchronisation? The images and intrinsics would then not be synchronised and you would still be limited to the framerate of the images.

[m11112089]

1. When there is a large frame rate difference between image_raw and camera_info, the AprilTag cannot detect any poses (0 Hz), and a warning message appears.

2. I am a beginner in ROS and am not very familiar with the detailed usage of message_filters. I extracted a code snippet from vision-visp, which allows for a delay between image_raw and camera_info to avoid issues with differing frame rates.

3. I suspect that the warning messages and inability to detect any AprilTags are due to the AprilTagNode::onCamera function subscribing to both image_raw and camera_info. When there is a large frame rate difference, these warnings appear, and no AprilTags are detected. In the future, subscribing to image_raw and camera_info separately might avoid this issue. However, this synchronization problem seems to be caused by the RealSense itself, and theoretically, subscribing to both image_raw and camera_info simultaneously should be correct.

Sorry for taking up your time. My issue has been temporarily resolved. Thank you for your patience and for helping me clarify the problem.

[christianrauch]

Can you provide details on what "large frame rate difference" means? In your example in the beginning, you got around 7 images and 30 info messages, resulting in 7~9 synchronisations. Does the apriltag node not process these 7~9 synchronised pairs? What is the frequency (ros2 topic hz) of these topics?

Essentially, I would like to know if in your case, the missed synchronisations are caused by different topic frequencies with exact matching timestamps, or by slightly mismatching timestamps at full framerate for both topics.

Can you provide a bag file of your image and info topic or details about their frequency and header time stamps?

[m11112089]

Sorry for the late reply.

Essentially, I would like to know if in your case, the missed synchronisations are caused by different topic frequencies with exact matching timestamps, or by slightly mismatching timestamps at full framerate for both topics.

I'm not sure how to confirm if the two different topics have matching timestamps. Here is the link to my bag file, in case it maybe could helps you: This is my bag file.

https://github.com/christianrauch/apriltag_ros/issues/33#:~:text=specific%20edge%20size-,This%20is%20my%20bag%20file.,-https%3A//drive.google

The "large frame rate difference" occurs because, in my computer, the RealSense D435 publishes /image_raw approximately 10 times per second, while the /camera_info topic is published about 30 times per second. During this time, I receive the following warning message, and no AprilTag poses are detected:

[WARN] [1715749104.420966646] [apriltag]: [image_transport] Topics '/camera/camera/color/image_raw' and '/camera/camera/color/camera_info' do not appear to be synchronized. In the last 10s:
    Image messages received:      7
    CameraInfo messages received: 30
    Synchronized pairs:           7

[christianrauch]

On that bag file, I get an average framerate of 30 Hz for both topics:

Files:             rosbag2_2024_05_15-12_51_59_0.db3
Bag size:          630.5 MiB
Storage id:        sqlite3
ROS Distro:        unknown
Duration:          23.861s
Start:             May 15 2024 06:51:59.564 (1715748719.564)
End:               May 15 2024 06:52:23.425 (1715748743.425)
Messages:          1432
Topic information: Topic: /camera/camera/color/camera_info | Type: sensor_msgs/msg/CameraInfo | Count: 716 | Serialization Format: cdr
                   Topic: /camera/camera/color/image_raw | Type: sensor_msgs/msg/Image | Count: 716 | Serialization Format: cdr

and also ros2 topic hz /camera/camera/color/image_raw reports average rate: 30.027.

Using your bag file with ros2 run apriltag_ros apriltag_node --ros-args -r image_rect:=/camera/camera/color/image_raw -r camera_info:=/camera/camera/color/camera_info --params-file config.yaml (with config.yaml being your config above) and checking the detection rate:

$ ros2 topic hz /detections
average rate: 28.644
    min: 0.015s max: 0.081s std dev: 0.01103s window: 30
average rate: 28.360
    min: 0.015s max: 0.081s std dev: 0.01078s window: 59
average rate: 28.862
    min: 0.015s max: 0.081s std dev: 0.00979s window: 89
average rate: 29.239
    min: 0.015s max: 0.081s std dev: 0.00895s window: 120
average rate: 29.377
    min: 0.014s max: 0.081s std dev: 0.00832s window: 150

also shows that the synchronisation and detection run at the full framerate minus a delay for the actual processing.

Are you sending the images over the network and they just arrive with 10 Hz? Is your detection rate close to the framerate of your image_raw topic? If so, how would your synchronisation via the ApproximateTime improve this?

[m11112089]

I am not sure why, but today I re-tested the same setup and equipment (both running locally without using the network) as before with my previously recorded ROS2 bag file. This time, I did not encounter the topic synchronization issue that I had shown in the previous video. Both image_raw and camera_info are running at 30 Hz. I am certain that this issue happened before (as evidenced by the video).

I am now very confused. Perhaps it was because I hadn't run sudo apt update & sudo apt upgrade to update my system in a while?

I apologize for taking up so much of your time. The issue seems to be resolved now. Thank you for your patience and for your detailed responses.

[christianrauch]

I am certain that this issue happened before (as evidenced by the video).

But this still does not say if the RealSense node ran on another machine etc.

For completeness, I checked the timestamps of the info and image messages by first exporting them to two text files:

# terminal 1
ros2 bag play rosbag2_2024_05_15-12_51_59
# terminal 2
ros2 topic echo --field header.stamp --csv /camera/camera/color/camera_info > info.txt
# terminal 3
ros2 topic echo --field header.stamp --csv /camera/camera/color/image_raw > image.txt

and then comparing the timestamps:

diff -y -W 60 info.txt image.txt

which shows me that the timestamps are identical.

Hence, using the exact time synchronisation via sync_policies::ExactTime is the best one can do here, since sync_policies::ApproximateTime might synchronise slightly different messages.

This is of course neglecting that the intrinsic parameters in the camera info message rarely change on a per-message basis. In most cases, it does not matter if the image and the static intrinsics are not synchronised.

In any case, I don't see how the sync_policies::ApproximateTime can improve the synchronised framerate for exact time stamp matches when your image topic has a lower framerate than the camera info message.

I am closing this now as a low image framerate cannot be addressed in the subscriber node. I will consider making the synchronisation configurable when there is a realistic case where the camera hardware generates images and intrinsics at different points in time.