Add methods for subscribing/unsubscribing to map update separately

ros2 · Sep 19, 2019 · a060815 · a060815
1 parent 9584141
commit a060815
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 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
@@ -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
@@ -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