Reset image displays when no new image has been received for a while (#1852)

Co-authored-by: Robert Haschke <[email protected]>

Author: pazeshun

src/rviz/image/image_display_base.cpp

@@ -41,7 +41,8 @@
 
 namespace rviz
 {
-ImageDisplayBase::ImageDisplayBase() : Display(), sub_(), tf_filter_(), messages_received_(0)
+ImageDisplayBase::ImageDisplayBase()
+  : Display(), sub_(), tf_filter_(), messages_received_(0), last_received_(ros::Time())
 {
   topic_property_ = new RosTopicProperty(
       "Image Topic", "", QString::fromStdString(ros::message_traits::datatype<sensor_msgs::Image>()),
@@ -65,6 +66,16 @@ ImageDisplayBase::ImageDisplayBase() : Display(), sub_(), tf_filter_(), messages
 
   unreliable_property_ = new BoolProperty("Unreliable", false, "Prefer UDP topic transport", this,
                                           &ImageDisplayBase::updateTopic);
+
+  timeout_property_ =
+      new FloatProperty("Timeout", 1.0,
+                        "Seconds to wait before resetting when no new image has been received.\n"
+                        "Zero disables the feature.",
+                        this, &ImageDisplayBase::updateResetTimeout);
+
+  reset_timer_ = new QTimer(this);
+  connect(reset_timer_, &QTimer::timeout, this, &ImageDisplayBase::onResetTimer);
+  updateResetTimeout();
 }
 
 ImageDisplayBase::~ImageDisplayBase()
@@ -113,6 +124,7 @@ void ImageDisplayBase::incomingMessage(const sensor_msgs::Image::ConstPtr& msg)
   setStatus(StatusProperty::Ok, "Image", QString::number(messages_received_) + " images received");
 
   emitTimeSignal(msg->header.stamp);
+  last_received_ = ros::Time::now();
 
   processMessage(msg);
 }
@@ -137,6 +149,8 @@ void ImageDisplayBase::reset()
 
   messages_received_ = 0;
   setStatus(StatusProperty::Warn, "Image", "No Image received");
+
+  last_received_ = ros::Time();
 }
 
 void ImageDisplayBase::updateQueueSize()
@@ -261,6 +275,14 @@ void ImageDisplayBase::updateTopic()
   context_->queueRender();
 }
 
+void ImageDisplayBase::updateResetTimeout()
+{
+  if (timeout_property_->getFloat() > 0)
+    reset_timer_->start(33); // frequently check for receive timeout
+  else
+    reset_timer_->stop();
+}
+
 void ImageDisplayBase::fillTransportOptionList(EnumProperty* property)
 {
   property->clearOptions();
@@ -305,4 +327,11 @@ void ImageDisplayBase::fillTransportOptionList(EnumProperty* property)
   }
 }
 
+void ImageDisplayBase::onResetTimer()
+{
+  ros::Time last = last_received_;
+  if (!last.isZero() && ros::Time::now() > last + ros::Duration(timeout_property_->getFloat()))
+    reset();
+}
+
 } // end namespace rviz

src/rviz/image/image_display_base.h

@@ -29,7 +29,9 @@
 #ifndef IMAGE_DISPLAY_BASE_H
 #define IMAGE_DISPLAY_BASE_H
 
+#include <atomic>
 #include <QObject>
+#include <QTimer>
 
 #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829
 #include <message_filters/subscriber.h>
@@ -44,6 +46,7 @@
 #include "rviz/properties/ros_topic_property.h"
 #include "rviz/properties/enum_property.h"
 #include "rviz/properties/int_property.h"
+#include "rviz/properties/float_property.h"
 
 #include "rviz/display.h"
 #include "rviz/rviz_export.h"
@@ -77,9 +80,15 @@ protected Q_SLOTS:
   /** @brief Update queue size of tf filter  */
   virtual void updateQueueSize();
 
+  /** @brief Start or stop reset_timer_ */
+  void updateResetTimeout();
+
   /** @brief Fill list of available and working transport options */
   void fillTransportOptionList(EnumProperty* property);
 
+  /** @brief Check for timeout and reset() if necessary */
+  void onResetTimer();
+
 protected:
   void onInitialize() override;
 
@@ -131,6 +140,10 @@ protected Q_SLOTS:
   std::set<std::string> transport_plugin_types_;
 
   BoolProperty* unreliable_property_;
+
+  FloatProperty* timeout_property_;
+  std::atomic<ros::Time> last_received_;
+  QTimer* reset_timer_;
 };
 
 } // end namespace rviz