Added Cache to camera display for TimeExact (#1138) (#1143)

Signed-off-by: Alejandro Hernández Cordero <[email protected]>
(cherry picked from commit fdf1957)

Co-authored-by: Alejandro Hernández Cordero <[email protected]>

Author: mergify[bot]

rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp

@@ -46,6 +46,8 @@
 # include <OgreRenderTargetListener.h>
 # include <OgreSharedPtr.h>
 
+#include <message_filters/cache.h>
+
 # include "sensor_msgs/msg/camera_info.hpp"
 # include "tf2_ros/message_filter.h"
 
@@ -191,6 +193,8 @@ private Q_SLOTS:
   std::unique_ptr<ROSImageTextureIface> texture_;
   std::unique_ptr<rviz_common::RenderPanel> render_panel_;
 
+  std::shared_ptr<message_filters::Cache<sensor_msgs::msg::Image>> cache_images_;
+
   rviz_common::properties::FloatProperty * alpha_property_;
   rviz_common::properties::EnumProperty * image_position_property_;
   rviz_common::properties::FloatProperty * zoom_property_;

rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp

@@ -471,13 +471,28 @@ bool CameraDisplay::updateCamera()
     return false;
   }
 
-  rclcpp::Time rviz_time = context_->getFrameManager()->getTime();
-  if (timeDifferenceInExactSyncMode(image, rviz_time)) {
-    setStatus(
-      StatusLevel::Warn, TIME_STATUS,
-      QString("Time-syncing active and no image at timestamp ") +
-      QString::number(rviz_time.nanoseconds()) + ".");
-    return false;
+  if (context_->getFrameManager()->getSyncMode() == rviz_common::FrameManagerIface::SyncExact) {
+    if (cache_images_ == nullptr) {
+      cache_images_ = std::make_shared<message_filters::Cache<sensor_msgs::msg::Image>>(
+        *subscription_, 20);
+    }
+    rclcpp::Time rviz_time = context_->getFrameManager()->getTime();
+    std::vector<sensor_msgs::msg::Image::ConstSharedPtr> interval_data = cache_images_->getInterval(
+      rviz_time, rviz_time);
+    if (!interval_data.empty()) {
+      image = interval_data[0];
+      if (timeDifferenceInExactSyncMode(image, rviz_time)) {
+        setStatus(
+          StatusLevel::Warn, TIME_STATUS,
+          QString("Time-syncing active and no image at timestamp ") +
+          QString::number(rviz_time.nanoseconds()) + ".");
+        return false;
+      }
+    }
+  } else {
+    if (cache_images_ != nullptr) {
+      cache_images_.reset();
+    }
   }
 
   Ogre::Vector3 position;