From f060815a7fe9a4cb0bdd03c884c5996e85449f9f Mon Sep 17 00:00:00 2001
From: gaschler <gaschlera@gmail.com>
Date: Thu, 25 Oct 2018 15:30:06 +0200
Subject: [PATCH] Fix locking while modifying counter (#1454)

Clang with thread safety does not compile because
num_nodes_since_last_loop_closure_
is modified without holding a mutex.
This fixes it.
---
 cartographer/mapping/internal/2d/pose_graph_2d.cc | 2 ++
 cartographer/mapping/internal/2d/pose_graph_2d.h  | 3 ++-
 cartographer/mapping/internal/3d/pose_graph_3d.cc | 2 ++
 cartographer/mapping/internal/3d/pose_graph_3d.h  | 3 ++-
 4 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc
index 88addd7..4e4c141 100644
--- a/cartographer/mapping/internal/2d/pose_graph_2d.cc
+++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc
@@ -377,6 +377,7 @@ WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
     }
   }
   constraint_builder_.NotifyEndOfNode();
+  absl::MutexLock locker(&mutex_);
   ++num_nodes_since_last_loop_closure_;
   if (options_.optimize_every_n_nodes() > 0 &&
       num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) {
@@ -997,6 +998,7 @@ transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform(
 }
 
 std::vector<std::vector<int>> PoseGraph2D::GetConnectedTrajectories() const {
+  absl::MutexLock locker(&mutex_);
   return data_.trajectory_connectivity_state.Components();
 }
 
diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h
index 75c1d6d..31267e8 100644
--- a/cartographer/mapping/internal/2d/pose_graph_2d.h
+++ b/cartographer/mapping/internal/2d/pose_graph_2d.h
@@ -114,7 +114,8 @@ class PoseGraph2D : public PoseGraph {
       const std::vector<Constraint>& constraints) override;
   void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) override;
   void RunFinalOptimization() override;
-  std::vector<std::vector<int>> GetConnectedTrajectories() const override;
+  std::vector<std::vector<int>> GetConnectedTrajectories() const override
+      LOCKS_EXCLUDED(mutex_);
   PoseGraphInterface::SubmapData GetSubmapData(const SubmapId& submap_id) const
       LOCKS_EXCLUDED(mutex_) override;
   MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() const
diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc
index 7801be8..5a2842a 100644
--- a/cartographer/mapping/internal/3d/pose_graph_3d.cc
+++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc
@@ -371,6 +371,7 @@ WorkItem::Result PoseGraph3D::ComputeConstraintsForNode(
     }
   }
   constraint_builder_.NotifyEndOfNode();
+  absl::MutexLock locker(&mutex_);
   ++num_nodes_since_last_loop_closure_;
   if (options_.optimize_every_n_nodes() > 0 &&
       num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) {
@@ -1005,6 +1006,7 @@ transform::Rigid3d PoseGraph3D::GetLocalToGlobalTransform(
 }
 
 std::vector<std::vector<int>> PoseGraph3D::GetConnectedTrajectories() const {
+  absl::MutexLock locker(&mutex_);
   return data_.trajectory_connectivity_state.Components();
 }
 
diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h
index 6a1b862..2ce6413 100644
--- a/cartographer/mapping/internal/3d/pose_graph_3d.h
+++ b/cartographer/mapping/internal/3d/pose_graph_3d.h
@@ -112,7 +112,8 @@ class PoseGraph3D : public PoseGraph {
       const std::vector<Constraint>& constraints) override;
   void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) override;
   void RunFinalOptimization() override;
-  std::vector<std::vector<int>> GetConnectedTrajectories() const override;
+  std::vector<std::vector<int>> GetConnectedTrajectories() const override
+      LOCKS_EXCLUDED(mutex_);
   PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id) const
       LOCKS_EXCLUDED(mutex_) override;
   MapById<SubmapId, SubmapData> GetAllSubmapData() const