From 8b329efc8e997fca5d6ec5050f86b7bc65255227 Mon Sep 17 00:00:00 2001
From: Wolfgang Hess <whess@lyft.com>
Date: Thu, 26 Jul 2018 17:05:23 +0200
Subject: [PATCH] Don't process PoseGraph2D work items in foreground. (#1341)

This is #1285 for 2D.
Fixes #1250.
---
 .../mapping/internal/2d/pose_graph_2d.cc      | 40 +++++++++----------
 .../mapping/internal/2d/pose_graph_2d.h       |  7 ++++
 2 files changed, 26 insertions(+), 21 deletions(-)

diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc
index dbcad28..7b4d0ec 100644
--- a/cartographer/mapping/internal/2d/pose_graph_2d.cc
+++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc
@@ -48,7 +48,8 @@ PoseGraph2D::PoseGraph2D(
     common::ThreadPool* thread_pool)
     : options_(options),
       optimization_problem_(std::move(optimization_problem)),
-      constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
+      constraint_builder_(options_.constraint_builder_options(), thread_pool),
+      thread_pool_(thread_pool) {}
 
 PoseGraph2D::~PoseGraph2D() {
   WaitForAllComputations();
@@ -155,26 +156,19 @@ NodeId PoseGraph2D::AddNode(
 
 void PoseGraph2D::AddWorkItem(
     const std::function<WorkItem::Result()>& work_item) {
-  {
-    common::MutexLocker locker(&work_queue_mutex_);
-    if (work_queue_ != nullptr) {
-      const auto now = std::chrono::steady_clock::now();
-      work_queue_->push_back({now, work_item});
-      kWorkQueueDelayMetric->Set(
-          common::ToSeconds(now - work_queue_->front().time));
-      return;
-    }
-  }
-  if (work_item() == WorkItem::Result::kRunOptimization) {
-    {
-      common::MutexLocker locker(&work_queue_mutex_);
-      work_queue_ = common::make_unique<WorkQueue>();
-    }
-    constraint_builder_.WhenDone(
-        [this](const constraints::ConstraintBuilder2D::Result& result) {
-          HandleWorkQueue(result);
-        });
+  common::MutexLocker locker(&work_queue_mutex_);
+  if (work_queue_ == nullptr) {
+    work_queue_ = common::make_unique<WorkQueue>();
+    auto task = common::make_unique<common::Task>();
+    task->SetWorkItem([this]() { DrainWorkQueue(); });
+    thread_pool_->Schedule(std::move(task));
   }
+  const auto now = std::chrono::steady_clock::now();
+  work_queue_->push_back({now, work_item});
+  kWorkQueueDelayMetric->Set(
+      std::chrono::duration_cast<std::chrono::duration<double>>(
+          now - work_queue_->front().time)
+          .count());
 }
 
 void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) {
@@ -469,8 +463,12 @@ void PoseGraph2D::HandleWorkQueue(
     num_nodes_since_last_loop_closure_ = 0;
   }
 
-  size_t work_queue_size;
+  DrainWorkQueue();
+}
+
+void PoseGraph2D::DrainWorkQueue() {
   bool process_work_queue = true;
+  size_t work_queue_size;
   while (process_work_queue) {
     std::function<WorkItem::Result()> work_item;
     {
diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h
index a71a928..55f1cfc 100644
--- a/cartographer/mapping/internal/2d/pose_graph_2d.h
+++ b/cartographer/mapping/internal/2d/pose_graph_2d.h
@@ -198,6 +198,10 @@ class PoseGraph2D : public PoseGraph {
   void HandleWorkQueue(const constraints::ConstraintBuilder2D::Result& result)
       EXCLUDES(mutex_) EXCLUDES(work_queue_mutex_);
 
+  // Process pending tasks in the work queue on the calling thread, until the
+  // queue is either empty or an optimization is required.
+  void DrainWorkQueue() EXCLUDES(mutex_) EXCLUDES(work_queue_mutex_);
+
   // Waits until we caught up (i.e. nothing is waiting to be scheduled), and
   // all computations have finished.
   void WaitForAllComputations() EXCLUDES(mutex_) EXCLUDES(work_queue_mutex_);
@@ -245,6 +249,9 @@ class PoseGraph2D : public PoseGraph {
   std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_;
   constraints::ConstraintBuilder2D constraint_builder_;
 
+  // Thread pool used for handling the work queue.
+  common::ThreadPool* const thread_pool_;
+
   // List of all trimmers to consult when optimizations finish.
   std::vector<std::unique_ptr<PoseGraphTrimmer>> trimmers_ GUARDED_BY(mutex_);