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/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);

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(

cartographer_ros/cartographer_ros/node.cc

@@ -428,21 +428,28 @@ bool Node::HandleWriteState(
 }
 
 void Node::FinishAllTrajectories() {
+  {
     carto::common::MutexLocker lock(&mutex_);
-  for (const auto& entry : is_active_trajectory_) {
+    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;
   }
+  map_builder_bridge_.RunFinalOptimization();
+}
 
 void Node::HandleOdometryMessage(const int trajectory_id,
                                  const string& sensor_id,