Added Cache to camera display for TimeExact (#1138)

Signed-off-by: Alejandro Hernández Cordero <[email protected]>
ros2 · Feb 19, 2024 · fdf1957 · fdf1957
1 parent b308385
commit fdf1957
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 7 deletions.
diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp b/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_;

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp
@@ -472,13 +472,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;