From dd6773395841ba774201a6e2fe68045ccf0cfa90 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Wed, 15 May 2024 11:04:47 +0200
Subject: [PATCH] Fix #1826: jitter in Map display updates

The queued connection update introduced in #1793 caused the map display
to first update the pose of the map and then the map itself (in the next update cycle).
This can be perfectly seen when throttling the rviz frame rate.

Updates to the visualization should be handled in update() only.
Thus, now, if a map update is received, a flag is set to perform the costly map update.
If that flag is not set, only the transform is updated.
---
 src/rviz/default_plugin/map_display.cpp | 22 +++++++++-------------
 src/rviz/default_plugin/map_display.h   | 12 ++++--------
 2 files changed, 13 insertions(+), 21 deletions(-)

diff --git a/src/rviz/default_plugin/map_display.cpp b/src/rviz/default_plugin/map_display.cpp
index 2a61069ad9..8a8b6a3494 100644
--- a/src/rviz/default_plugin/map_display.cpp
+++ b/src/rviz/default_plugin/map_display.cpp
@@ -219,12 +219,9 @@ void Swatch::updateData()
 }
 
 
-MapDisplay::MapDisplay() : Display(), loaded_(false), resolution_(0.0f), width_(0), height_(0)
+MapDisplay::MapDisplay()
+  : Display(), loaded_(false), map_updated_(false), resolution_(0.0f), width_(0), height_(0)
 {
-  // HACK: Using a direct connection triggers a segfault on NVIDIA hardware (#1793) when rendering
-  //       *and* having performed a depth rendering before (e.g. due to raycasting for selections)
-  // A queued connection delays the update of renderables after the current VisualizationManager::onUpdate() call
-  connect(this, &MapDisplay::mapUpdated, this, &MapDisplay::showMap, Qt::QueuedConnection);
   topic_property_ = new RosTopicProperty(
       "Topic", "", QString::fromStdString(ros::message_traits::datatype<nav_msgs::OccupancyGrid>()),
       "nav_msgs::OccupancyGrid topic to subscribe to.", this, &MapDisplay::updateTopic);
@@ -548,6 +545,7 @@ void MapDisplay::clear()
   }
 
   loaded_ = false;
+  map_updated_ = false;
 }
 
 bool validateFloats(const nav_msgs::OccupancyGrid& msg)
@@ -561,9 +559,8 @@ bool validateFloats(const nav_msgs::OccupancyGrid& msg)
 void MapDisplay::incomingMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
 {
   current_map_ = *msg;
-  // updated via signal in case ros spinner is in a different thread
-  Q_EMIT mapUpdated();
   loaded_ = true;
+  map_updated_ = true;
 }
 
 
@@ -589,8 +586,7 @@ void MapDisplay::incomingUpdate(const map_msgs::OccupancyGridUpdate::ConstPtr& u
     memcpy(&current_map_.data[(update->y + y) * current_map_.info.width + update->x],
            &update->data[y * update->width], update->width);
   }
-  // updated via signal in case ros spinner is in a different thread
-  Q_EMIT mapUpdated();
+  map_updated_ = true;
 }
 
 void MapDisplay::createSwatches()
@@ -653,7 +649,7 @@ void MapDisplay::createSwatches()
   }
 }
 
-void MapDisplay::showMap()
+void MapDisplay::updateMap()
 {
   if (current_map_.data.empty())
   {
@@ -758,9 +754,7 @@ void MapDisplay::showMap()
   position_property_->setVector(position);
   orientation_property_->setQuaternion(orientation);
 
-  transformMap();
-
-  context_->queueRender();
+  map_updated_ = false;
 }
 
 void MapDisplay::updatePalette()
@@ -843,6 +837,8 @@ void MapDisplay::setTopic(const QString& topic, const QString& /*datatype*/)
 
 void MapDisplay::update(float /*wall_dt*/, float /*ros_dt*/)
 {
+  if (map_updated_)
+    updateMap();
   transformMap();
 }
 
diff --git a/src/rviz/default_plugin/map_display.h b/src/rviz/default_plugin/map_display.h
index b201bed1d6..b73a15f161 100644
--- a/src/rviz/default_plugin/map_display.h
+++ b/src/rviz/default_plugin/map_display.h
@@ -122,17 +122,12 @@ class MapDisplay : public Display
 
   void setTopic(const QString& topic, const QString& datatype) override;
 
-Q_SIGNALS:
-  /** @brief Emitted when a new map is received*/
-  void mapUpdated();
-
 protected Q_SLOTS:
   void updateAlpha();
   void updateTopic();
   void updateDrawUnder();
   void updatePalette();
-  /** @brief Show current_map_ in the scene. */
-  void showMap();
+  void updateMap();
   void transformMap();
 
 protected:
@@ -144,10 +139,10 @@ protected Q_SLOTS:
   virtual void unsubscribe();
   void update(float wall_dt, float ros_dt) override;
 
-  /** @brief Copy msg into current_map_ and call showMap(). */
+  /** @brief Copy msg into current_map_ and set flag map_updated_ */
   void incomingMap(const nav_msgs::OccupancyGrid::ConstPtr& msg);
 
-  /** @brief Copy update's data into current_map_ and call showMap(). */
+  /** @brief Copy update's data into current_map_ and set flag map_updated_ */
   void incomingUpdate(const map_msgs::OccupancyGridUpdate::ConstPtr& update);
 
   void clear();
@@ -158,6 +153,7 @@ protected Q_SLOTS:
   std::vector<Ogre::TexturePtr> palette_textures_;
   std::vector<bool> color_scheme_transparency_;
   bool loaded_;
+  bool map_updated_;
 
   std::string topic_;
   float resolution_;