From d5c604dafd718c4e08b1d588740cf7e82fa60479 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victor_reijgwart@hotmail.com>
Date: Tue, 29 Jun 2021 18:04:11 +0200
Subject: [PATCH] Reintroduce incremental ESDF updates and properly handle map
 pruning

---
 voxblox_ros/include/voxblox_ros/esdf_server.h |  1 -
 voxblox_ros/src/esdf_server.cc                | 13 ++++---------
 2 files changed, 4 insertions(+), 10 deletions(-)

diff --git a/voxblox_ros/include/voxblox_ros/esdf_server.h b/voxblox_ros/include/voxblox_ros/esdf_server.h
index b03c8bd8a..ec96bd0e1 100644
--- a/voxblox_ros/include/voxblox_ros/esdf_server.h
+++ b/voxblox_ros/include/voxblox_ros/esdf_server.h
@@ -105,7 +105,6 @@ class EsdfServer : public TsdfServer {
   std::unique_ptr<EsdfIntegrator> esdf_integrator_;
 
   // TODO(victorr): Add description
-  bool map_has_been_pruned_;
   void pruneMap() override;
 };
 
diff --git a/voxblox_ros/src/esdf_server.cc b/voxblox_ros/src/esdf_server.cc
index e983e3fc5..712bc177d 100644
--- a/voxblox_ros/src/esdf_server.cc
+++ b/voxblox_ros/src/esdf_server.cc
@@ -27,8 +27,7 @@ EsdfServer::EsdfServer(const ros::NodeHandle& nh,
       publish_traversable_(false),
       traversability_radius_(1.0),
       incremental_update_(true),
-      num_subscribers_esdf_map_(0),
-      map_has_been_pruned_(false) {
+      num_subscribers_esdf_map_(0) {
   // Set up map and integrator.
   esdf_map_.reset(new EsdfMap(esdf_config));
   esdf_integrator_.reset(new EsdfIntegrator(esdf_integrator_config,
@@ -198,12 +197,6 @@ bool EsdfServer::loadMap(const std::string& file_path) {
 }
 
 void EsdfServer::updateEsdf() {
-  if (map_has_been_pruned_) {
-    map_has_been_pruned_ = false;
-    updateEsdfBatch();
-    return;
-  }
-
   if (tsdf_map_->getTsdfLayer().getNumberOfAllocatedBlocks() > 0) {
     const bool clear_updated_flag_esdf = true;
     esdf_integrator_->updateFromTsdfLayer(clear_updated_flag_esdf);
@@ -287,7 +280,9 @@ void EsdfServer::pruneMap() {
     }
   }
 
-  map_has_been_pruned_ = true;
+  // Reset the ESDF integrator's internal queues, as they might contain
+  // indices of blocks that no longer exist after pruning
+  esdf_integrator_->clear();
 
   ROS_INFO_STREAM_COND(verbose_,
                        "Pruned " << num_pruned_blocks << " ESDF blocks");