From f8452821d7480bf636efa3a56225a9d8f0dcbe84 Mon Sep 17 00:00:00 2001
From: gaschler <gaschlera@gmail.com>
Date: Wed, 13 Dec 2017 18:49:22 +0100
Subject: [PATCH] WaitForAllComputation waits for work queue (#754)

---
 cartographer/mapping/pose_graph_interface.h | 2 +-
 cartographer/mapping_2d/pose_graph.cc       | 5 +++--
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h
index c1625de..f0626f1 100644
--- a/cartographer/mapping/pose_graph_interface.h
+++ b/cartographer/mapping/pose_graph_interface.h
@@ -60,7 +60,7 @@ class PoseGraphInterface {
   PoseGraphInterface(const PoseGraphInterface&) = delete;
   PoseGraphInterface& operator=(const PoseGraphInterface&) = delete;
 
-  // Computes optimized poses.
+  // Waits for all computations to finish and computes optimized poses.
   virtual void RunFinalOptimization() = 0;
 
   // Returns data for all submaps.
diff --git a/cartographer/mapping_2d/pose_graph.cc b/cartographer/mapping_2d/pose_graph.cc
index 02dc11b..c3d9fb5 100644
--- a/cartographer/mapping_2d/pose_graph.cc
+++ b/cartographer/mapping_2d/pose_graph.cc
@@ -351,8 +351,9 @@ void PoseGraph::WaitForAllComputations() {
       constraint_builder_.GetNumFinishedNodes();
   while (!locker.AwaitWithTimeout(
       [this]() REQUIRES(mutex_) {
-        return constraint_builder_.GetNumFinishedNodes() ==
-               num_trajectory_nodes_;
+        return ((constraint_builder_.GetNumFinishedNodes() ==
+                 num_trajectory_nodes_) &&
+                !work_queue_);
       },
       common::FromSeconds(1.))) {
     std::ostringstream progress_info;