Reintroduce incremental ESDF updates and properly handle map pruning

ethz-asl · Jun 29, 2021 · d5c604d · d5c604d
1 parent af3a951
commit d5c604d
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 10 deletions.
diff --git 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
@@ -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");