From d9847e9adcd1a87b90f720bf2235eef1c014b6a3 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@users.noreply.github.com>
Date: Mon, 16 Nov 2020 18:13:37 +0100
Subject: [PATCH] MessageFilterDisplay: process messages synchronously (#1560)

---
 src/rviz/message_filter_display.h | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/src/rviz/message_filter_display.h b/src/rviz/message_filter_display.h
index 5b0d364859..dfaa3f04d6 100644
--- a/src/rviz/message_filter_display.h
+++ b/src/rviz/message_filter_display.h
@@ -42,6 +42,8 @@
 #include <rviz/display.h>
 #include <rviz/rviz_export.h>
 
+Q_DECLARE_METATYPE(boost::shared_ptr<const void>)
+
 namespace rviz
 {
 /** @brief Helper superclass for MessageFilterDisplay, needed because
@@ -63,11 +65,14 @@ class RVIZ_EXPORT _RosTopicDisplay : public Display
                         "w.r.t. your data, but it can greatly increase memory usage as well.",
                         this, SLOT(updateQueueSize()));
     queue_size_property_->setMin(0);
+    qRegisterMetaType<boost::shared_ptr<const void>>(); // required to send via queued connection
   }
 
 protected Q_SLOTS:
   virtual void updateTopic() = 0;
   virtual void updateQueueSize() = 0;
+private Q_SLOTS:
+  virtual void processTypeErasedMessage(boost::shared_ptr<const void> type_erased_msg) = 0;
 
 protected:
   RosTopicProperty* topic_property_;
@@ -203,7 +208,15 @@ class MessageFilterDisplay : public _RosTopicDisplay
     {
       return;
     }
+    // process message synchronously in main GUI thread to avoid race conditions
+    QMetaObject::invokeMethod(this, "processTypeErasedMessage", Qt::QueuedConnection,
+                              Q_ARG(boost::shared_ptr<const void>,
+                                    boost::static_pointer_cast<const void>(msg)));
+  }
 
+  void processTypeErasedMessage(boost::shared_ptr<const void> type_erased_msg) override
+  {
+    auto msg = boost::static_pointer_cast<const MessageType>(type_erased_msg);
     ++messages_received_;
     setStatus(StatusProperty::Ok, "Topic", QString::number(messages_received_) + " messages received");