From cdc38640ec035e13f319a962ac974c406364f076 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Tue, 1 Sep 2020 12:19:34 -0400
Subject: [PATCH] MapCloudDisplay: detect if previous node has been discarded
 to cleanup memory (e.g., when the robot is not moving, #458)

---
 src/rviz/MapCloudDisplay.cpp | 27 +++++++++++++++++++++++++--
 src/rviz/MapCloudDisplay.h   |  2 ++
 2 files changed, 27 insertions(+), 2 deletions(-)

diff --git a/src/rviz/MapCloudDisplay.cpp b/src/rviz/MapCloudDisplay.cpp
index a030beed..12d8f37f 100644
--- a/src/rviz/MapCloudDisplay.cpp
+++ b/src/rviz/MapCloudDisplay.cpp
@@ -86,6 +86,7 @@ void MapCloudDisplay::CloudInfo::clear()
 
 MapCloudDisplay::MapCloudDisplay()
   : spinner_(1, &cbqueue_),
+	lastCloudAdded_(-1),
     new_xyz_transformer_(false),
     new_color_transformer_(false),
     needs_retransform_(false),
@@ -648,6 +649,8 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 {
 	rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt();
 
+	int lastCloudAdded = -1;
+
 	if (needs_retransform_)
 	{
 		retransform();
@@ -687,6 +690,7 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 
 				cloud_infos_.erase(it->first);
 				cloud_infos_.insert(*it);
+				lastCloudAdded = it->first;
 			}
 
 			new_cloud_infos_.clear();
@@ -761,15 +765,33 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 				}
 			}
 			//hide not used clouds
-			for(std::map<int, CloudInfoPtr>::iterator iter = cloud_infos_.begin(); iter!=cloud_infos_.end(); ++iter)
+			for(std::map<int, CloudInfoPtr>::iterator iter = cloud_infos_.begin(); iter!=cloud_infos_.end();)
 			{
 				if(current_map_.find(iter->first) == current_map_.end())
 				{
-					iter->second->scene_node_->setVisible(false);
+					if(iter->first == lastCloudAdded_)
+					{
+						// remove from cache, the node has been discarded
+						cloud_infos_.erase(iter++);
+						lastCloudAdded_ = -1;
+					}
+					else
+					{
+						iter->second->scene_node_->setVisible(false);
+						++iter;
+					}
+				}
+				else
+				{
+					++iter;
 				}
 			}
 		}
 	}
+	if(lastCloudAdded>0)
+	{
+		lastCloudAdded_ = lastCloudAdded;
+	}
 
 	this->setStatusStd(rviz::StatusProperty::Ok, "Points", tr("%1").arg(totalPoints).toStdString());
 	this->setStatusStd(rviz::StatusProperty::Ok, "Nodes", tr("%1 shown of %2").arg(totalNodesShown).arg(cloud_infos_.size()).toStdString());
@@ -777,6 +799,7 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 
 void MapCloudDisplay::reset()
 {
+	lastCloudAdded_ = -1;
 	{
 		boost::mutex::scoped_lock lock(new_clouds_mutex_);
 		cloud_infos_.clear();
diff --git a/src/rviz/MapCloudDisplay.h b/src/rviz/MapCloudDisplay.h
index d751919f..86f1d1a1 100644
--- a/src/rviz/MapCloudDisplay.h
+++ b/src/rviz/MapCloudDisplay.h
@@ -174,6 +174,8 @@ private:
 	std::map<int, rtabmap::Transform> current_map_;
 	boost::mutex current_map_mutex_;
 
+	int lastCloudAdded_;
+
 	struct TransformerInfo
 	{
 		rviz::PointCloudTransformerPtr transformer;