From a060815da48f36261aa3f68c6c8f1431446db9aa Mon Sep 17 00:00:00 2001
From: Martin Idel <martin.idel@googlemail.com>
Date: Thu, 19 Sep 2019 03:09:00 +0200
Subject: [PATCH] Add methods for subscribing/unsubscribing to map update
 separately

---
 .../displays/map/map_display.hpp              |  3 +++
 .../displays/map/map_display.cpp              | 22 +++++++++++++------
 2 files changed, 18 insertions(+), 7 deletions(-)

diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/map/map_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/map/map_display.hpp
index b08acd9a1..20040a2c0 100644
--- a/rviz_default_plugins/include/rviz_default_plugins/displays/map/map_display.hpp
+++ b/rviz_default_plugins/include/rviz_default_plugins/displays/map/map_display.hpp
@@ -141,6 +141,9 @@ protected Q_SLOTS:
 
   void clear();
 
+  void subscribeToUpdateTopic();
+  void unsubscribeToUpdateTopic();
+
   void showValidMap();
   void resetSwatchesIfNecessary(size_t width, size_t height, float resolution);
   void createSwatches();
diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/map/map_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/map/map_display.cpp
index 76dc41d07..36449d4dc 100644
--- a/rviz_default_plugins/src/rviz_default_plugins/displays/map/map_display.cpp
+++ b/rviz_default_plugins/src/rviz_default_plugins/displays/map/map_display.cpp
@@ -205,11 +205,16 @@ void MapDisplay::subscribe()
 
   MFDClass::subscribe();
 
+  subscribeToUpdateTopic();
+}
+
+void MapDisplay::subscribeToUpdateTopic()
+{
   try {
-    update_subscription_ = rviz_ros_node_.lock()->get_raw_node()->
+    update_subscription_ =
+      rviz_ros_node_.lock()->get_raw_node()->
       template create_subscription<map_msgs::msg::OccupancyGridUpdate>(
-      update_topic_property_->getTopicStd(),
-      update_profile_,
+      update_topic_property_->getTopicStd(), update_profile_,
       [this](const map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr message) {
         incomingUpdate(message);
       });
@@ -224,6 +229,11 @@ void MapDisplay::subscribe()
 void MapDisplay::unsubscribe()
 {
   MFDClass::unsubscribe();
+  unsubscribeToUpdateTopic();
+}
+
+void MapDisplay::unsubscribeToUpdateTopic()
+{
   update_subscription_.reset();
 }
 
@@ -288,7 +298,6 @@ void MapDisplay::processMessage(nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg
   Q_EMIT mapUpdated();
 }
 
-// TODO(wjwwood): Use again once map_msgs are ported
 void MapDisplay::incomingUpdate(const map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update)
 {
   // Only update the map if we have gotten a full one first.
@@ -607,13 +616,12 @@ void MapDisplay::onEnable()
 
 void MapDisplay::updateMapUpdateTopic()
 {
-  unsubscribe();
+  unsubscribeToUpdateTopic();
   reset();
-  subscribe();
+  subscribeToUpdateTopic();
   context_->queueRender();
 }
 
-
 }  // namespace displays
 }  // namespace rviz_default_plugins