From bb884ab9e17ea5450f2c38f15a6fae3aa45ca107 Mon Sep 17 00:00:00 2001
From: Michel Hidalgo <michel@ekumenlabs.com>
Date: Mon, 2 Nov 2020 11:32:17 -0300
Subject: [PATCH] Ensure rviz_common::MessageFilterDisplay processes messages
 in the main thread (#620)

Since tf2_ros::MessageFilter callback is called from tf2_ros::TransformListener
dedicated thread, concurrency issues will arise if a derived display (like
rviz_defaults_plugins::Polygon) attempts to modify the UI from
rviz_common::MessageFilterDisplay::processMessage().

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>
---
 .../rviz_common/message_filter_display.hpp    | 21 +++++++++------
 .../include/rviz_common/ros_topic_display.hpp | 26 +++++++++++++++++++
 2 files changed, 39 insertions(+), 8 deletions(-)

diff --git a/rviz_common/include/rviz_common/message_filter_display.hpp b/rviz_common/include/rviz_common/message_filter_display.hpp
index bbb79efad..d18430c00 100644
--- a/rviz_common/include/rviz_common/message_filter_display.hpp
+++ b/rviz_common/include/rviz_common/message_filter_display.hpp
@@ -127,7 +127,7 @@ class MessageFilterDisplay : public _RosTopicDisplay
       tf_filter_->connectInput(*subscription_);
       tf_filter_->registerCallback(
         std::bind(
-          &MessageFilterDisplay<MessageType>::incomingMessage, this,
+          &MessageFilterDisplay<MessageType>::messageTaken, this,
           std::placeholders::_1));
       setStatus(properties::StatusProperty::Ok, "Topic", "OK");
     } catch (rclcpp::exceptions::InvalidTopicNameError & e) {
@@ -174,18 +174,23 @@ class MessageFilterDisplay : public _RosTopicDisplay
     reset();
   }
 
-  /// Incoming message callback.
-  /**
-   * Checks if the message pointer
-   * is valid, increments messages_received_, then calls
-   * processMessage().
-   */
-  void incomingMessage(const typename MessageType::ConstSharedPtr msg)
+  void messageTaken(typename MessageType::ConstSharedPtr msg)
   {
     if (!msg) {
       return;
     }
 
+    // Do not process message right away, tf2_ros::MessageFilter may be
+    // calling back from tf2_ros::TransformListener dedicated thread.
+    // Use type erased signal/slot machinery to ensure messages are
+    // processed in the main thread.
+    Q_EMIT typeErasedMessageTaken(std::static_pointer_cast<const void>(msg));
+  }
+
+  void processTypeErasedMessage(std::shared_ptr<const void> type_erased_msg) override
+  {
+    auto msg = std::static_pointer_cast<const MessageType>(type_erased_msg);
+
     ++messages_received_;
     setStatus(
       properties::StatusProperty::Ok,
diff --git a/rviz_common/include/rviz_common/ros_topic_display.hpp b/rviz_common/include/rviz_common/ros_topic_display.hpp
index 01fe05e44..15411a319 100644
--- a/rviz_common/include/rviz_common/ros_topic_display.hpp
+++ b/rviz_common/include/rviz_common/ros_topic_display.hpp
@@ -51,6 +51,12 @@
 #include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp"
 #include "rviz_common/visibility_control.hpp"
 
+// Required, in combination with
+// `qRegisterMetaType<std::shared_ptr<const void>>` so that this
+// type can be queued by Qt slots.
+// See: http://doc.qt.io/qt-5/qmetatype.html#qRegisterMetaType-1
+Q_DECLARE_METATYPE(std::shared_ptr<const void>)
+
 namespace rviz_common
 {
 
@@ -66,6 +72,8 @@ class RVIZ_COMMON_PUBLIC _RosTopicDisplay : public Display
   : rviz_ros_node_(),
     qos_profile(5)
   {
+    qRegisterMetaType<std::shared_ptr<const void>>();
+
     topic_property_ = new properties::RosTopicProperty(
       "Topic", "",
       "", "", this, SLOT(updateTopic()));
@@ -92,9 +100,27 @@ class RVIZ_COMMON_PUBLIC _RosTopicDisplay : public Display
         this->qos_profile = profile;
         updateTopic();
       });
+
+    // Useful to _ROSTopicDisplay subclasses to ensure GUI updates
+    // are performed by the main thread only.
+    connect(
+      this,
+      SIGNAL(typeErasedMessageTaken(std::shared_ptr<const void>)),
+      this,
+      SLOT(processTypeErasedMessage(std::shared_ptr<const void>)),
+      // Force queued connections regardless of QObject thread affinity
+      Qt::QueuedConnection);
   }
 
+Q_SIGNALS:
+  void typeErasedMessageTaken(std::shared_ptr<const void> type_erased_message);
+
 protected Q_SLOTS:
+  virtual void processTypeErasedMessage(std::shared_ptr<const void> type_erased_message)
+  {
+    (void)type_erased_message;
+  }
+
   virtual void transformerChangedCallback()
   {
   }