From 2de0e5f04b843af9eaf29d1c1eeffb6edb10e6f6 Mon Sep 17 00:00:00 2001
From: danielsievers <35999903+danielsievers@users.noreply.github.com>
Date: Fri, 20 Jul 2018 14:41:57 +0200
Subject: [PATCH] Don't process PoseGraph3D work items in foreground (#1285)

This helps the foreground thread in keeping up real time.
Also see issue #1250.
---
 .../mapping/internal/3d/pose_graph_3d.cc      | 40 +++++++++----------
 .../mapping/internal/3d/pose_graph_3d.h       |  7 ++++
 2 files changed, 26 insertions(+), 21 deletions(-)

diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc
index 85c43f7..e651a0b 100644
--- a/cartographer/mapping/internal/3d/pose_graph_3d.cc
+++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc
@@ -47,7 +47,8 @@ PoseGraph3D::PoseGraph3D(
     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) {}
 
 PoseGraph3D::~PoseGraph3D() {
   WaitForAllComputations();
@@ -152,26 +153,19 @@ NodeId PoseGraph3D::AddNode(
 
 void PoseGraph3D::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::ConstraintBuilder3D::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 PoseGraph3D::AddTrajectoryIfNeeded(const int trajectory_id) {
@@ -484,8 +478,12 @@ void PoseGraph3D::HandleWorkQueue(
     num_nodes_since_last_loop_closure_ = 0;
   }
 
-  size_t work_queue_size;
+  DrainWorkQueue();
+}
+
+void PoseGraph3D::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/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h
index dc271cc..0608e45 100644
--- a/cartographer/mapping/internal/3d/pose_graph_3d.h
+++ b/cartographer/mapping/internal/3d/pose_graph_3d.h
@@ -199,6 +199,10 @@ class PoseGraph3D : public PoseGraph {
   void HandleWorkQueue(const constraints::ConstraintBuilder3D::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_);
+
   // Runs the optimization. Callers have to make sure, that there is only one
   // optimization being run at a time.
   void RunOptimization() EXCLUDES(mutex_);
@@ -246,6 +250,9 @@ class PoseGraph3D : public PoseGraph {
   std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem_;
   constraints::ConstraintBuilder3D 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_);