From 102a3b0db3f88e942c8a46ffbf57c92fa5072f48 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= <juraj.orsulic@fer.hr>
Date: Thu, 10 Aug 2017 09:55:52 +0200
Subject: [PATCH] Release mutex in Node during final optimization (#480)

Enables servicing callbacks during final optimization, thus
enabling visualization of its progress in RViz. Fixes #476.
---
 .../cartographer_ros/map_builder_bridge.cc    |  6 ++++-
 .../cartographer_ros/map_builder_bridge.h     |  1 +
 cartographer_ros/cartographer_ros/node.cc     | 25 ++++++++++++-------
 3 files changed, 22 insertions(+), 10 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
index 6343430..c907bd6 100644
--- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc
+++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
@@ -77,10 +77,14 @@ void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
   // Make sure there is a trajectory with 'trajectory_id'.
   CHECK_EQ(sensor_bridges_.count(trajectory_id), 1);
   map_builder_.FinishTrajectory(trajectory_id);
-  map_builder_.sparse_pose_graph()->RunFinalOptimization();
   sensor_bridges_.erase(trajectory_id);
 }
 
+void MapBuilderBridge::RunFinalOptimization() {
+  LOG(INFO) << "Running final trajectory optimization...";
+  map_builder_.sparse_pose_graph()->RunFinalOptimization();
+}
+
 void MapBuilderBridge::SerializeState(const std::string& filename) {
   cartographer::io::ProtoStreamWriter writer(filename);
   map_builder_.SerializeState(&writer);
diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h
index 5a974aa..7cc6c19 100644
--- a/cartographer_ros/cartographer_ros/map_builder_bridge.h
+++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h
@@ -54,6 +54,7 @@ class MapBuilderBridge {
   int AddTrajectory(const std::unordered_set<string>& expected_sensor_ids,
                     const TrajectoryOptions& trajectory_options);
   void FinishTrajectory(int trajectory_id);
+  void RunFinalOptimization();
   void SerializeState(const string& filename);
 
   bool HandleSubmapQuery(
diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc
index dbc1c60..21c631e 100644
--- a/cartographer_ros/cartographer_ros/node.cc
+++ b/cartographer_ros/cartographer_ros/node.cc
@@ -428,20 +428,27 @@ bool Node::HandleWriteState(
 }
 
 void Node::FinishAllTrajectories() {
-  carto::common::MutexLocker lock(&mutex_);
-  for (const auto& entry : is_active_trajectory_) {
-    const int trajectory_id = entry.first;
-    if (entry.second) {
-      map_builder_bridge_.FinishTrajectory(trajectory_id);
+  {
+    carto::common::MutexLocker lock(&mutex_);
+    for (auto& entry : is_active_trajectory_) {
+      const int trajectory_id = entry.first;
+      if (entry.second) {
+        map_builder_bridge_.FinishTrajectory(trajectory_id);
+        entry.second = false;
+      }
     }
   }
+  map_builder_bridge_.RunFinalOptimization();
 }
 
 void Node::FinishTrajectory(const int trajectory_id) {
-  carto::common::MutexLocker lock(&mutex_);
-  CHECK(is_active_trajectory_.at(trajectory_id));
-  map_builder_bridge_.FinishTrajectory(trajectory_id);
-  is_active_trajectory_[trajectory_id] = false;
+  {
+    carto::common::MutexLocker lock(&mutex_);
+    CHECK(is_active_trajectory_.at(trajectory_id));
+    map_builder_bridge_.FinishTrajectory(trajectory_id);
+    is_active_trajectory_[trajectory_id] = false;
+  }
+  map_builder_bridge_.RunFinalOptimization();
 }
 
 void Node::HandleOdometryMessage(const int trajectory_id,