Desynced depth and color rs2::frame causing missing ROS messages

[njenwei]

Issue:

With Sync Mode:On there are instances when the framesets received by BaseRealSenseNode::frame_callback sometimes do not contain both depth and color frames, resulting no publication of the aligned_depth_to_color and pointcloud2 ROS message.

The color frame sometimes arrives at a slightly later timestamp to the depth frame, and rs2::asynchronous_syncer treats them as different framesets. The behaviour of asynchronous_syncer is corroborated by this issue: IntelRealSense/librealsense#2662

Within ROS - I use message_filters with an ExactTime policy (http://wiki.ros.org/message_filters) to retrieve synchronised pointcloud2, camera_info, color and depth messages, and this causes periods of no messages when aligned_depth_to_color and pointcloud2 is not published. These dropped frames are currently causing issues in my application. I would prefer to receive full framesets, even if the color frame is slightly off sync, rather than the frames being dropped.

Mitigation:

An aggregator filter (https://github.com/IntelRealSense/librealsense/blob/HEAD/src/pipeline/aggregator.h) which fills missing messages with frameset has been implemented for rs2::pipeline.

Will it be possible to port it as another filter?

I'm currently working around the issue with this workaround, but would love to see a solution merged into main:

diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp
index b654fc3..a589bc7 100644
--- a/realsense2_camera/src/base_realsense_node.cpp
+++ b/realsense2_camera/src/base_realsense_node.cpp
@@ -873,8 +873,40 @@ void BaseRealSenseNode::setupDevice()
         {
             frame_callback_function = _syncer;
 
+            rs2::filter* aggregate_filter_ptr = new rs2::filter([this](rs2::frame f, rs2::frame_source& src) {
+                auto frameset = f.as<rs2::frameset>();
+                std::vector<rs2::frame> bundle;
+                bool has_color = false;
+                for (auto it = frameset.begin(); it != frameset.end(); ++it) {
+                    auto f = (*it);
+                    bundle.push_back(f);
+
+                    if (f.get_profile().stream_type() == RS2_STREAM_COLOR) {
+                        has_color = true;
+                        aggregate_filter_color_history.clear();
+                        aggregate_filter_color_history.push_back(f);
+                    }
+                }
+
+                if (!has_color && !aggregate_filter_color_history.empty()) {
+                    bundle.push_back(aggregate_filter_color_history[0]);
+                }
+
+                auto fs = src.allocate_composite_frame(bundle);
+                src.frame_ready(fs);
+            });
+            aggregate_filter = std::shared_ptr<rs2::filter>(aggregate_filter_ptr);
+
             auto frame_callback_inner = [this](rs2::frame frame){
-                frame_callback(frame);
+                if (frame.is<rs2::frameset>()) {
+                    frame = aggregate_filter->process(frame);
+                    auto frameset = frame.as<rs2::frameset>();
+                    if (frameset.size() > 1) {
+                        frame_callback(frame);
+                    }
+                } else {
+                    frame_callback(frame);
+                }
             };
             _syncer.start(frame_callback_inner);
         }
diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h
index 42b2d2c..2c2514f 100644
--- a/realsense2_camera/include/base_realsense_node.h
+++ b/realsense2_camera/include/base_realsense_node.h
@@ -302,6 +302,8 @@ namespace realsense2_camera
         std::string _filters_str;
         stream_index_pair _pointcloud_texture;
         PipelineSyncer _syncer;
+        std::vector<rs2::frame> aggregate_filter_color_history;
+        std::shared_ptr<rs2::filter> aggregate_filter;
         std::vector<NamedFilter> _filters;
         std::shared_ptr<rs2::filter> _colorizer, _pointcloud_filter;
         std::vector<rs2::sensor> _dev_sensors;

[MartyG-RealSense]

Hi @njenwei Does stability improve if you do not use an ExactTime policy?

According to the RealSense ROS wrapper documentation, when the pointcloud filter is used then enable_sync is automatically enabled to gather the closest frames of different sensors, infra red, color and depth, to be sent with the same timetag.

[njenwei]

Hi @MartyG-RealSense

It is the same when I am subscribed to aligned_color_to_depth and the pointcloud individually.

For reference, I run at 15 fps, and I see gaps of 0.14s to 0.3s between frames intermittently (0.07s nominal), which is when the framset is missing either color or depth rs2::frame.

enable_sync gathers frames of the exact same timestamp, so when the color frame is delayed by 20ms compared to depth and infra, it does not follow in the same frameset in the callback. aligned_depth_to_color and pointcloud is not emitted when either is missing from the frameset.

[MartyG-RealSense]

I will refer your question to @doronhi the RealSense ROS wrapper developer and recommend that you continue using your successful workaround in the meantime.

[doronhi]

At first glance, it seems like an issue with the syncer. The aggregate_filter you added seems like an elaborate patch to something that should be done in the underlying librealsense library.
I'll try to reproduce. Could you please state what device you use? which ROS distribution, type of machine, and versions of librealsense2 and realsense2_camera?
Could you tell if this phenomenon is a regression? Did it happen in previous versions of librealsense2?

Regarding the data you provide, something is unclear to me. At 15 fps I expect a frame every 0.07s so the maximal gap between a color and a depth frame should usually be up to 0.03s. How can the nominal gaps be around 0.07s? Can you add some "rostopic echo" input to clarify?

[MartyG-RealSense]

Thanks very much @doronhi for your advice to @njenwei

Hi @njenwei Could you provide information about the questions asked by @doronhi above, please? Thanks!

[njenwei]

Hi @doronhi

Yes I hope you will be able to find a nicer implementation of the patch.

This was reproduced on Ubuntu 18.04, ROS Melodic with a D435i on USB 3.2 with the following combinations

Machine:

• AMD Ryzen 9 + RTX 3080
• Intel i7 + RTX 2070

Library

• librealsense2 v2.45.0 + realsense2_camera v2.3.0, both from apt
• librealsense2 v2.48.0 + realsense2_camera v2.3.1, both built from source

By nominal, I mean average gap between frames to be 0.07s.

Will try and get some rostopic echo data next week.

[doronhi]

Hi @njenwei
I think now that you show a valid use case. It seems that although sometimes conditions are created for some frame drops one may still prefer to have a frameset, even if slightly misaligned.
I prefer to have librealsense2's pipeline::aggregator exposed rather than have another implementation in the wrapper.
To that end, I opened 2 inner change requests: One for librealsense2 (LRS-276) to expose that processing block. Another for the ROS1, ROS2 wrappers, to incorporate that processing block (LRS-277).
With your permission, we'll keep this issue open until those CRs will be completed.

Thanks

[MartyG-RealSense]

Hi @njenwei Do you have an update about this case that you can provide, please? Thanks!

[MartyG-RealSense]

Hi @njenwei Do you require further assistance with this case, please? Thanks!

[njenwei]

Hi, sorry for the delay.

@doronhi Glad to hear that the CR has been submitted. I am happy to test them and report back if you're able to let me know when the changes have been merged into the public repos!

[MartyG-RealSense]

I have added an Enhancement tag to this case to indicate that it should be kept open until the CRs associated with it are completed.