diff --git a/cartographer/cloud/internal/map_builder_server.cc b/cartographer/cloud/internal/map_builder_server.cc index 8d592f3..a5df785 100644 --- a/cartographer/cloud/internal/map_builder_server.cc +++ b/cartographer/cloud/internal/map_builder_server.cc @@ -200,7 +200,7 @@ void MapBuilderServer::OnLocalSlamResult( ->EnqueueSensorData(std::move(sensor_data)); } - common::MutexLocker locker(&subscriptions_lock_); + absl::MutexLock locker(&subscriptions_lock_); for (auto& entry : local_slam_subscriptions_[trajectory_id]) { auto copy_of_insertion_result = insertion_result @@ -224,7 +224,7 @@ void MapBuilderServer::OnLocalSlamResult( void MapBuilderServer::OnGlobalSlamOptimizations( const std::map& last_optimized_submap_ids, const std::map& last_optimized_node_ids) { - common::MutexLocker locker(&subscriptions_lock_); + absl::MutexLock locker(&subscriptions_lock_); for (auto& entry : global_slam_subscriptions_) { if (!entry.second(last_optimized_submap_ids, last_optimized_node_ids)) { LOG(INFO) << "Removing subscription with index: " << entry.first; @@ -237,7 +237,7 @@ MapBuilderContextInterface::LocalSlamSubscriptionId MapBuilderServer::SubscribeLocalSlamResults( int trajectory_id, MapBuilderContextInterface::LocalSlamSubscriptionCallback callback) { - common::MutexLocker locker(&subscriptions_lock_); + absl::MutexLock locker(&subscriptions_lock_); local_slam_subscriptions_[trajectory_id].emplace(current_subscription_index_, callback); return MapBuilderContextInterface::LocalSlamSubscriptionId{ @@ -247,7 +247,7 @@ MapBuilderServer::SubscribeLocalSlamResults( void MapBuilderServer::UnsubscribeLocalSlamResults( const MapBuilderContextInterface::LocalSlamSubscriptionId& subscription_id) { - common::MutexLocker locker(&subscriptions_lock_); + absl::MutexLock locker(&subscriptions_lock_); CHECK_EQ(local_slam_subscriptions_[subscription_id.trajectory_id].erase( subscription_id.subscription_index), 1u); @@ -255,19 +255,19 @@ void MapBuilderServer::UnsubscribeLocalSlamResults( int MapBuilderServer::SubscribeGlobalSlamOptimizations( MapBuilderContextInterface::GlobalSlamOptimizationCallback callback) { - common::MutexLocker locker(&subscriptions_lock_); + absl::MutexLock locker(&subscriptions_lock_); global_slam_subscriptions_.emplace(current_subscription_index_, callback); return current_subscription_index_++; } void MapBuilderServer::UnsubscribeGlobalSlamOptimizations( int subscription_index) { - common::MutexLocker locker(&subscriptions_lock_); + absl::MutexLock locker(&subscriptions_lock_); CHECK_EQ(global_slam_subscriptions_.erase(subscription_index), 1u); } void MapBuilderServer::NotifyFinishTrajectory(int trajectory_id) { - common::MutexLocker locker(&subscriptions_lock_); + absl::MutexLock locker(&subscriptions_lock_); for (auto& entry : local_slam_subscriptions_[trajectory_id]) { MapBuilderContextInterface::LocalSlamSubscriptionCallback callback = entry.second; diff --git a/cartographer/cloud/internal/map_builder_server.h b/cartographer/cloud/internal/map_builder_server.h index d709c3c..83c7e3c 100644 --- a/cartographer/cloud/internal/map_builder_server.h +++ b/cartographer/cloud/internal/map_builder_server.h @@ -135,7 +135,7 @@ class MapBuilderServer : public MapBuilderServerInterface { std::unique_ptr map_builder_; common::BlockingQueue> incoming_data_queue_; - common::Mutex subscriptions_lock_; + absl::Mutex subscriptions_lock_; int current_subscription_index_ = 0; std::map local_slam_subscriptions_ GUARDED_BY(subscriptions_lock_); diff --git a/cartographer/common/blocking_queue.h b/cartographer/common/blocking_queue.h index ad2a641..cba91c0 100644 --- a/cartographer/common/blocking_queue.h +++ b/cartographer/common/blocking_queue.h @@ -21,7 +21,7 @@ #include #include -#include "cartographer/common/mutex.h" +#include "absl/synchronization/mutex.h" #include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "glog/logging.h" @@ -47,17 +47,22 @@ class BlockingQueue { // Pushes a value onto the queue. Blocks if the queue is full. void Push(T t) { - MutexLocker lock(&mutex_); - lock.Await([this]() REQUIRES(mutex_) { return QueueNotFullCondition(); }); + const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return QueueNotFullCondition(); + }; + absl::MutexLock lock(&mutex_); + mutex_.Await(absl::Condition(&predicate)); deque_.push_back(std::move(t)); } // Like push, but returns false if 'timeout' is reached. bool PushWithTimeout(T t, const common::Duration timeout) { - MutexLocker lock(&mutex_); - if (!lock.AwaitWithTimeout( - [this]() REQUIRES(mutex_) { return QueueNotFullCondition(); }, - timeout)) { + const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return QueueNotFullCondition(); + }; + absl::MutexLock lock(&mutex_); + if (!mutex_.AwaitWithTimeout(absl::Condition(&predicate), + absl::FromChrono(timeout))) { return false; } deque_.push_back(std::move(t)); @@ -66,8 +71,11 @@ class BlockingQueue { // Pops the next value from the queue. Blocks until a value is available. T Pop() { - MutexLocker lock(&mutex_); - lock.Await([this]() REQUIRES(mutex_) { return !QueueEmptyCondition(); }); + const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return !QueueEmptyCondition(); + }; + absl::MutexLock lock(&mutex_); + mutex_.Await(absl::Condition(&predicate)); T t = std::move(deque_.front()); deque_.pop_front(); @@ -76,10 +84,12 @@ class BlockingQueue { // Like Pop, but can timeout. Returns nullptr in this case. T PopWithTimeout(const common::Duration timeout) { - MutexLocker lock(&mutex_); - if (!lock.AwaitWithTimeout( - [this]() REQUIRES(mutex_) { return !QueueEmptyCondition(); }, - timeout)) { + const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return !QueueEmptyCondition(); + }; + absl::MutexLock lock(&mutex_); + if (!mutex_.AwaitWithTimeout(absl::Condition(&predicate), + absl::FromChrono(timeout))) { return nullptr; } T t = std::move(deque_.front()); @@ -90,10 +100,12 @@ class BlockingQueue { // Like Peek, but can timeout. Returns nullptr in this case. template R* PeekWithTimeout(const common::Duration timeout) { - MutexLocker lock(&mutex_); - if (!lock.AwaitWithTimeout( - [this]() REQUIRES(mutex_) { return !QueueEmptyCondition(); }, - timeout)) { + const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return !QueueEmptyCondition(); + }; + absl::MutexLock lock(&mutex_); + if (!mutex_.AwaitWithTimeout(absl::Condition(&predicate), + absl::FromChrono(timeout))) { return nullptr; } return deque_.front().get(); @@ -104,7 +116,7 @@ class BlockingQueue { // a pointer to the given type R. template const R* Peek() { - MutexLocker lock(&mutex_); + absl::MutexLock lock(&mutex_); if (deque_.empty()) { return nullptr; } @@ -113,26 +125,31 @@ class BlockingQueue { // Returns the number of items currently in the queue. size_t Size() { - MutexLocker lock(&mutex_); + absl::MutexLock lock(&mutex_); return deque_.size(); } // Blocks until the queue is empty. void WaitUntilEmpty() { - MutexLocker lock(&mutex_); - lock.Await([this]() REQUIRES(mutex_) { return QueueEmptyCondition(); }); + const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return QueueEmptyCondition(); + }; + absl::MutexLock lock(&mutex_); + mutex_.Await(absl::Condition(&predicate)); } private: // Returns true iff the queue is empty. - bool QueueEmptyCondition() REQUIRES(mutex_) { return deque_.empty(); } + bool QueueEmptyCondition() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return deque_.empty(); + } // Returns true iff the queue is not full. - bool QueueNotFullCondition() REQUIRES(mutex_) { + bool QueueNotFullCondition() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { return queue_size_ == kInfiniteQueueSize || deque_.size() < queue_size_; } - Mutex mutex_; + absl::Mutex mutex_; const size_t queue_size_ GUARDED_BY(mutex_); std::deque deque_ GUARDED_BY(mutex_); }; diff --git a/cartographer/common/internal/testing/thread_pool_for_testing.cc b/cartographer/common/internal/testing/thread_pool_for_testing.cc index 771c44a..281788c 100644 --- a/cartographer/common/internal/testing/thread_pool_for_testing.cc +++ b/cartographer/common/internal/testing/thread_pool_for_testing.cc @@ -35,7 +35,7 @@ ThreadPoolForTesting::ThreadPoolForTesting() ThreadPoolForTesting::~ThreadPoolForTesting() { { - MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK(running_); running_ = false; CHECK_EQ(task_queue_.size(), 0); @@ -45,7 +45,7 @@ ThreadPoolForTesting::~ThreadPoolForTesting() { } void ThreadPoolForTesting::NotifyDependenciesCompleted(Task* task) { - MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK(running_); auto it = tasks_not_ready_.find(task); CHECK(it != tasks_not_ready_.end()); @@ -56,7 +56,7 @@ void ThreadPoolForTesting::NotifyDependenciesCompleted(Task* task) { std::weak_ptr ThreadPoolForTesting::Schedule(std::unique_ptr task) { std::shared_ptr shared_task; { - MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); idle_ = false; CHECK(running_); auto insert_result = @@ -69,11 +69,13 @@ std::weak_ptr ThreadPoolForTesting::Schedule(std::unique_ptr task) { } void ThreadPoolForTesting::WaitUntilIdle() { + const auto predicate = [this]() + EXCLUSIVE_LOCKS_REQUIRED(mutex_) { return idle_; }; for (;;) { { - common::MutexLocker locker(&mutex_); - if (locker.AwaitWithTimeout([this]() REQUIRES(mutex_) { return idle_; }, - common::FromSeconds(0.1))) { + absl::MutexLock locker(&mutex_); + if (mutex_.AwaitWithTimeout(absl::Condition(&predicate), + absl::FromChrono(common::FromSeconds(0.1)))) { return; } } @@ -81,14 +83,15 @@ void ThreadPoolForTesting::WaitUntilIdle() { } void ThreadPoolForTesting::DoWork() { + const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return !task_queue_.empty() || !running_; + }; for (;;) { std::shared_ptr task; { - MutexLocker locker(&mutex_); - locker.AwaitWithTimeout( - [this]() - REQUIRES(mutex_) { return !task_queue_.empty() || !running_; }, - common::FromSeconds(0.1)); + absl::MutexLock locker(&mutex_); + mutex_.AwaitWithTimeout(absl::Condition(&predicate), + absl::FromChrono(common::FromSeconds(0.1))); if (!task_queue_.empty()) { task = task_queue_.front(); task_queue_.pop_front(); diff --git a/cartographer/common/internal/testing/thread_pool_for_testing.h b/cartographer/common/internal/testing/thread_pool_for_testing.h index 53bccb3..f733d0f 100644 --- a/cartographer/common/internal/testing/thread_pool_for_testing.h +++ b/cartographer/common/internal/testing/thread_pool_for_testing.h @@ -22,7 +22,7 @@ #include #include -#include "cartographer/common/mutex.h" +#include "absl/synchronization/mutex.h" #include "cartographer/common/thread_pool.h" namespace cartographer { @@ -35,7 +35,7 @@ class ThreadPoolForTesting : public ThreadPoolInterface { ~ThreadPoolForTesting(); std::weak_ptr Schedule(std::unique_ptr task) - EXCLUDES(mutex_) override; + LOCKS_EXCLUDED(mutex_) override; void WaitUntilIdle(); @@ -44,9 +44,9 @@ class ThreadPoolForTesting : public ThreadPoolInterface { void DoWork(); - void NotifyDependenciesCompleted(Task* task) EXCLUDES(mutex_) override; + void NotifyDependenciesCompleted(Task* task) LOCKS_EXCLUDED(mutex_) override; - Mutex mutex_; + absl::Mutex mutex_; bool running_ GUARDED_BY(mutex_) = true; bool idle_ GUARDED_BY(mutex_) = true; std::deque> task_queue_ GUARDED_BY(mutex_); diff --git a/cartographer/common/mutex.h b/cartographer/common/mutex.h deleted file mode 100644 index c064bba..0000000 --- a/cartographer/common/mutex.h +++ /dev/null @@ -1,67 +0,0 @@ -/* - * Copyright 2016 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_COMMON_MUTEX_H_ -#define CARTOGRAPHER_COMMON_MUTEX_H_ - -#include "absl/base/thread_annotations.h" -#include "absl/synchronization/mutex.h" -#include "cartographer/common/time.h" - -namespace cartographer { -namespace common { - -#define REQUIRES(...) \ - THREAD_ANNOTATION_ATTRIBUTE__(requires_capability(__VA_ARGS__)) - -#define EXCLUDES(...) THREAD_ANNOTATION_ATTRIBUTE__(locks_excluded(__VA_ARGS__)) - -// TODO(CodeArno): Replace references in code to absl::Mutex directly. -using Mutex = absl::Mutex; - -// A RAII class that acquires a mutex in its constructor, and -// releases it in its destructor. It also implements waiting functionality on -// conditions that get checked whenever the mutex is released. -// TODO(CodeArno): Replace MutexLocker instantiations in the codebase with -// absl::MutexLock. -class SCOPED_LOCKABLE MutexLocker { - public: - MutexLocker(Mutex* mutex) EXCLUSIVE_LOCK_FUNCTION(mutex) - : mutex_(mutex), lock_(mutex) {} - - ~MutexLocker() UNLOCK_FUNCTION() {} - - template - void Await(Predicate predicate) REQUIRES(this) { - mutex_->Await(absl::Condition(&predicate)); - } - - template - bool AwaitWithTimeout(Predicate predicate, common::Duration timeout) - REQUIRES(this) { - return mutex_->AwaitWithTimeout(absl::Condition(&predicate), - absl::FromChrono(timeout)); - } - - private: - absl::Mutex* mutex_; - absl::MutexLock lock_; -}; - -} // namespace common -} // namespace cartographer - -#endif // CARTOGRAPHER_COMMON_MUTEX_H_ diff --git a/cartographer/common/task.cc b/cartographer/common/task.cc index dc3d28b..4085c97 100644 --- a/cartographer/common/task.cc +++ b/cartographer/common/task.cc @@ -27,12 +27,12 @@ Task::~Task() { } Task::State Task::GetState() { - MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return state_; } void Task::SetWorkItem(const WorkItem& work_item) { - MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK_EQ(state_, NEW); work_item_ = work_item; } @@ -40,7 +40,7 @@ void Task::SetWorkItem(const WorkItem& work_item) { void Task::AddDependency(std::weak_ptr dependency) { std::shared_ptr shared_dependency; { - MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK_EQ(state_, NEW); if ((shared_dependency = dependency.lock())) { ++uncompleted_dependencies_; @@ -52,7 +52,7 @@ void Task::AddDependency(std::weak_ptr dependency) { } void Task::SetThreadPool(ThreadPoolInterface* thread_pool) { - MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK_EQ(state_, NEW); state_ = DISPATCHED; thread_pool_to_notify_ = thread_pool; @@ -64,7 +64,7 @@ void Task::SetThreadPool(ThreadPoolInterface* thread_pool) { } void Task::AddDependentTask(Task* dependent_task) { - MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); if (state_ == COMPLETED) { dependent_task->OnDependenyCompleted(); return; @@ -74,7 +74,7 @@ void Task::AddDependentTask(Task* dependent_task) { } void Task::OnDependenyCompleted() { - MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK(state_ == NEW || state_ == DISPATCHED); --uncompleted_dependencies_; if (uncompleted_dependencies_ == 0 && state_ == DISPATCHED) { @@ -86,7 +86,7 @@ void Task::OnDependenyCompleted() { void Task::Execute() { { - MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK_EQ(state_, DEPENDENCIES_COMPLETED); state_ = RUNNING; } @@ -96,7 +96,7 @@ void Task::Execute() { work_item_(); } - MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); state_ = COMPLETED; for (Task* dependent_task : dependent_tasks_) { dependent_task->OnDependenyCompleted(); diff --git a/cartographer/common/task.h b/cartographer/common/task.h index bd84699..ae44fb1 100644 --- a/cartographer/common/task.h +++ b/cartographer/common/task.h @@ -19,7 +19,7 @@ #include -#include "cartographer/common/mutex.h" +#include "absl/synchronization/mutex.h" #include "glog/logging.h" #include "thread_pool.h" @@ -38,24 +38,24 @@ class Task { Task() = default; ~Task(); - State GetState() EXCLUDES(mutex_); + State GetState() LOCKS_EXCLUDED(mutex_); // State must be 'NEW'. - void SetWorkItem(const WorkItem& work_item) EXCLUDES(mutex_); + void SetWorkItem(const WorkItem& work_item) LOCKS_EXCLUDED(mutex_); // State must be 'NEW'. 'dependency' may be nullptr, in which case it is // assumed completed. - void AddDependency(std::weak_ptr dependency) EXCLUDES(mutex_); + void AddDependency(std::weak_ptr dependency) LOCKS_EXCLUDED(mutex_); private: // Allowed in all states. void AddDependentTask(Task* dependent_task); // State must be 'DEPENDENCIES_COMPLETED' and becomes 'COMPLETED'. - void Execute() EXCLUDES(mutex_); + void Execute() LOCKS_EXCLUDED(mutex_); // State must be 'NEW' and becomes 'DISPATCHED' or 'DEPENDENCIES_COMPLETED'. - void SetThreadPool(ThreadPoolInterface* thread_pool) EXCLUDES(mutex_); + void SetThreadPool(ThreadPoolInterface* thread_pool) LOCKS_EXCLUDED(mutex_); // State must be 'NEW' or 'DISPATCHED'. If 'DISPATCHED', may become // 'DEPENDENCIES_COMPLETED'. @@ -67,7 +67,7 @@ class Task { unsigned int uncompleted_dependencies_ GUARDED_BY(mutex_) = 0; std::set dependent_tasks_ GUARDED_BY(mutex_); - Mutex mutex_; + absl::Mutex mutex_; }; } // namespace common diff --git a/cartographer/common/thread_pool.cc b/cartographer/common/thread_pool.cc index 21ad1a1..7973ae2 100644 --- a/cartographer/common/thread_pool.cc +++ b/cartographer/common/thread_pool.cc @@ -35,7 +35,7 @@ void ThreadPoolInterface::SetThreadPool(Task* task) { } ThreadPool::ThreadPool(int num_threads) { - MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); for (int i = 0; i != num_threads; ++i) { pool_.emplace_back([this]() { ThreadPool::DoWork(); }); } @@ -43,7 +43,7 @@ ThreadPool::ThreadPool(int num_threads) { ThreadPool::~ThreadPool() { { - MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK(running_); running_ = false; } @@ -53,7 +53,7 @@ ThreadPool::~ThreadPool() { } void ThreadPool::NotifyDependenciesCompleted(Task* task) { - MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); auto it = tasks_not_ready_.find(task); CHECK(it != tasks_not_ready_.end()); task_queue_.push_back(it->second); @@ -63,7 +63,7 @@ void ThreadPool::NotifyDependenciesCompleted(Task* task) { std::weak_ptr ThreadPool::Schedule(std::unique_ptr task) { std::shared_ptr shared_task; { - MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); auto insert_result = tasks_not_ready_.insert(std::make_pair(task.get(), std::move(task))); CHECK(insert_result.second) << "Schedule called twice"; @@ -80,13 +80,14 @@ void ThreadPool::DoWork() { // away CPU resources from more important foreground threads. CHECK_NE(nice(10), -1); #endif + const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return !task_queue_.empty() || !running_; + }; for (;;) { std::shared_ptr task; { - MutexLocker locker(&mutex_); - locker.Await([this]() REQUIRES(mutex_) { - return !task_queue_.empty() || !running_; - }); + absl::MutexLock locker(&mutex_); + mutex_.Await(absl::Condition(&predicate)); if (!task_queue_.empty()) { task = std::move(task_queue_.front()); task_queue_.pop_front(); diff --git a/cartographer/common/thread_pool.h b/cartographer/common/thread_pool.h index b3bb595..1c7c352 100644 --- a/cartographer/common/thread_pool.h +++ b/cartographer/common/thread_pool.h @@ -24,7 +24,7 @@ #include #include -#include "cartographer/common/mutex.h" +#include "absl/synchronization/mutex.h" #include "cartographer/common/task.h" namespace cartographer { @@ -65,14 +65,14 @@ class ThreadPool : public ThreadPoolInterface { // When the returned weak pointer is expired, 'task' has certainly completed, // so dependants no longer need to add it as a dependency. std::weak_ptr Schedule(std::unique_ptr task) - EXCLUDES(mutex_) override; + LOCKS_EXCLUDED(mutex_) override; private: void DoWork(); - void NotifyDependenciesCompleted(Task* task) EXCLUDES(mutex_) override; + void NotifyDependenciesCompleted(Task* task) LOCKS_EXCLUDED(mutex_) override; - Mutex mutex_; + absl::Mutex mutex_; bool running_ GUARDED_BY(mutex_) = true; std::vector pool_ GUARDED_BY(mutex_); std::deque> task_queue_ GUARDED_BY(mutex_); diff --git a/cartographer/common/thread_pool_test.cc b/cartographer/common/thread_pool_test.cc index 2a3b816..fec82ee 100644 --- a/cartographer/common/thread_pool_test.cc +++ b/cartographer/common/thread_pool_test.cc @@ -28,19 +28,21 @@ namespace { class Receiver { public: void Receive(int number) { - MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); received_numbers_.push_back(number); } void WaitForNumberSequence(const std::vector& expected_numbers) { - common::MutexLocker locker(&mutex_); - locker.Await([this, &expected_numbers]() REQUIRES(mutex_) { - return (received_numbers_.size() >= expected_numbers.size()); - }); + const auto predicate = + [this, &expected_numbers]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return (received_numbers_.size() >= expected_numbers.size()); + }; + absl::MutexLock locker(&mutex_); + mutex_.Await(absl::Condition(&predicate)); EXPECT_EQ(expected_numbers, received_numbers_); } - Mutex mutex_; + absl::Mutex mutex_; std::vector received_numbers_ GUARDED_BY(mutex_); }; diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 5518c27..644c210 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -53,7 +53,7 @@ PoseGraph2D::PoseGraph2D( PoseGraph2D::~PoseGraph2D() { WaitForAllComputations(); - common::MutexLocker locker(&work_queue_mutex_); + absl::MutexLock locker(&work_queue_mutex_); CHECK(work_queue_ == nullptr); } @@ -113,7 +113,7 @@ NodeId PoseGraph2D::AppendNode( const int trajectory_id, const std::vector>& insertion_submaps, const transform::Rigid3d& optimized_pose) { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); AddTrajectoryIfNeeded(trajectory_id); if (!CanAddWorkItemModifying(trajectory_id)) { LOG(WARNING) << "AddNode was called for finished or deleted trajectory."; @@ -147,7 +147,7 @@ NodeId PoseGraph2D::AddNode( // We have to check this here, because it might have changed by the time we // execute the lambda. const bool newly_finished_submap = insertion_submaps.front()->finished(); - AddWorkItem([=]() EXCLUDES(mutex_) { + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { return ComputeConstraintsForNode(node_id, insertion_submaps, newly_finished_submap); }); @@ -156,7 +156,7 @@ NodeId PoseGraph2D::AddNode( void PoseGraph2D::AddWorkItem( const std::function& work_item) { - common::MutexLocker locker(&work_queue_mutex_); + absl::MutexLock locker(&work_queue_mutex_); if (work_queue_ == nullptr) { work_queue_ = absl::make_unique(); auto task = absl::make_unique(); @@ -190,8 +190,8 @@ void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) { void PoseGraph2D::AddImuData(const int trajectory_id, const sensor::ImuData& imu_data) { - AddWorkItem([=]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); if (CanAddWorkItemModifying(trajectory_id)) { optimization_problem_->AddImuData(trajectory_id, imu_data); } @@ -201,8 +201,8 @@ void PoseGraph2D::AddImuData(const int trajectory_id, void PoseGraph2D::AddOdometryData(const int trajectory_id, const sensor::OdometryData& odometry_data) { - AddWorkItem([=]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); if (CanAddWorkItemModifying(trajectory_id)) { optimization_problem_->AddOdometryData(trajectory_id, odometry_data); } @@ -218,8 +218,8 @@ void PoseGraph2D::AddFixedFramePoseData( void PoseGraph2D::AddLandmarkData(int trajectory_id, const sensor::LandmarkData& landmark_data) { - AddWorkItem([=]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); if (CanAddWorkItemModifying(trajectory_id)) { for (const auto& observation : landmark_data.landmark_observations) { data_.landmark_nodes[observation.id].landmark_observations.emplace_back( @@ -240,7 +240,7 @@ void PoseGraph2D::ComputeConstraint(const NodeId& node_id, const TrajectoryNode::Data* constant_data; const Submap2D* submap; { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished); if (!data_.submap_data.at(submap_id).submap->finished()) { // Uplink server only receives grids when they are finished, so skip @@ -292,7 +292,7 @@ WorkItem::Result PoseGraph2D::ComputeConstraintsForNode( std::vector finished_submap_ids; std::set newly_finished_submap_node_ids; { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); const auto& constant_data = data_.trajectory_nodes.at(node_id).constant_data; submap_ids = InitializeGlobalSubmapPoses( @@ -417,7 +417,7 @@ void PoseGraph2D::DeleteTrajectoriesIfNeeded() { void PoseGraph2D::HandleWorkQueue( const constraints::ConstraintBuilder2D::Result& result) { { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); data_.constraints.insert(data_.constraints.end(), result.begin(), result.end()); } @@ -427,7 +427,7 @@ void PoseGraph2D::HandleWorkQueue( std::map trajectory_id_to_last_optimized_node_id; std::map trajectory_id_to_last_optimized_submap_id; { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); const auto& submap_data = optimization_problem_->submap_data(); const auto& node_data = optimization_problem_->node_data(); for (const int trajectory_id : node_data.trajectory_ids()) { @@ -449,7 +449,7 @@ void PoseGraph2D::HandleWorkQueue( } { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); for (const Constraint& constraint : result) { UpdateTrajectoryConnectivity(constraint); } @@ -477,7 +477,7 @@ void PoseGraph2D::DrainWorkQueue() { while (process_work_queue) { std::function work_item; { - common::MutexLocker locker(&work_queue_mutex_); + absl::MutexLock locker(&work_queue_mutex_); if (work_queue_->empty()) { work_queue_.reset(); return; @@ -499,7 +499,7 @@ void PoseGraph2D::DrainWorkQueue() { void PoseGraph2D::WaitForAllComputations() { int num_trajectory_nodes; { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); num_trajectory_nodes = data_.num_trajectory_nodes; } @@ -524,29 +524,35 @@ void PoseGraph2D::WaitForAllComputations() { // First wait for the work queue to drain so that it's safe to schedule // a WhenDone() callback. { - common::MutexLocker locker(&work_queue_mutex_); - while (!locker.AwaitWithTimeout( - [this]() REQUIRES(work_queue_mutex_) { return work_queue_ == nullptr; }, - common::FromSeconds(1.))) { + const auto predicate = [this]() + EXCLUSIVE_LOCKS_REQUIRED(work_queue_mutex_) { + return work_queue_ == nullptr; + }; + absl::MutexLock locker(&work_queue_mutex_); + while (!work_queue_mutex_.AwaitWithTimeout( + absl::Condition(&predicate), + absl::FromChrono(common::FromSeconds(1.)))) { report_progress(); } } // Now wait for any pending constraint computations to finish. - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); bool notification = false; constraint_builder_.WhenDone( [this, ¬ification](const constraints::ConstraintBuilder2D::Result& result) - EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); data_.constraints.insert(data_.constraints.end(), result.begin(), result.end()); notification = true; }); - while (!locker.AwaitWithTimeout([¬ification]() - REQUIRES(mutex_) { return notification; }, - common::FromSeconds(1.))) { + const auto predicate = [¬ification]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return notification; + }; + while (!mutex_.AwaitWithTimeout(absl::Condition(&predicate), + absl::FromChrono(common::FromSeconds(1.)))) { report_progress(); } CHECK_EQ(constraint_builder_.GetNumFinishedNodes(), num_trajectory_nodes); @@ -555,7 +561,7 @@ void PoseGraph2D::WaitForAllComputations() { void PoseGraph2D::DeleteTrajectory(const int trajectory_id) { { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); auto it = data_.trajectories_state.find(trajectory_id); if (it == data_.trajectories_state.end()) { LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: " @@ -565,8 +571,8 @@ void PoseGraph2D::DeleteTrajectory(const int trajectory_id) { it->second.deletion_state = InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION; } - AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); CHECK(data_.trajectories_state.at(trajectory_id).state != TrajectoryState::ACTIVE); CHECK(data_.trajectories_state.at(trajectory_id).state != @@ -580,8 +586,8 @@ void PoseGraph2D::DeleteTrajectory(const int trajectory_id) { } void PoseGraph2D::FinishTrajectory(const int trajectory_id) { - AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); CHECK(!IsTrajectoryFinished(trajectory_id)); data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED; @@ -600,11 +606,11 @@ bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) const { void PoseGraph2D::FreezeTrajectory(const int trajectory_id) { { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); data_.trajectory_connectivity_state.Add(trajectory_id); } - AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); CHECK(!IsTrajectoryFrozen(trajectory_id)); data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN; return WorkItem::Result::kDoNotRunOptimization; @@ -629,7 +635,7 @@ void PoseGraph2D::AddSubmapFromProto( const transform::Rigid2d global_submap_pose_2d = transform::Project2D(global_submap_pose); { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); const std::shared_ptr submap_ptr = std::make_shared(submap.submap_2d(), &conversion_tables_); @@ -641,12 +647,13 @@ void PoseGraph2D::AddSubmapFromProto( data_.global_submap_poses_2d.Insert( submap_id, optimization::SubmapSpec2D{global_submap_pose_2d}); } - AddWorkItem([this, submap_id, global_submap_pose_2d]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); - data_.submap_data.at(submap_id).state = SubmapState::kFinished; - optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d); - return WorkItem::Result::kDoNotRunOptimization; - }); + AddWorkItem( + [this, submap_id, global_submap_pose_2d]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + data_.submap_data.at(submap_id).state = SubmapState::kFinished; + optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d); + return WorkItem::Result::kDoNotRunOptimization; + }); } void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose, @@ -657,15 +664,15 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose, std::make_shared(FromProto(node.node_data())); { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); AddTrajectoryIfNeeded(node_id.trajectory_id); if (!CanAddWorkItemModifying(node_id.trajectory_id)) return; data_.trajectory_nodes.Insert(node_id, TrajectoryNode{constant_data, global_pose}); } - AddWorkItem([this, node_id, global_pose]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this, node_id, global_pose]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); const auto& constant_data = data_.trajectory_nodes.at(node_id).constant_data; const auto gravity_alignment_inverse = transform::Rigid3d::Rotation( @@ -689,8 +696,8 @@ void PoseGraph2D::SetTrajectoryDataFromProto( void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id, const SubmapId& submap_id) { - AddWorkItem([this, node_id, submap_id]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this, node_id, submap_id]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); if (CanAddWorkItemModifying(submap_id.trajectory_id)) { data_.submap_data.at(submap_id).node_ids.insert(node_id); } @@ -700,8 +707,8 @@ void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id, void PoseGraph2D::AddSerializedConstraints( const std::vector& constraints) { - AddWorkItem([this, constraints]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this, constraints]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); for (const auto& constraint : constraints) { CHECK(data_.trajectory_nodes.Contains(constraint.node_id)); CHECK(data_.submap_data.Contains(constraint.submap_id)); @@ -735,8 +742,8 @@ void PoseGraph2D::AddSerializedConstraints( void PoseGraph2D::AddTrimmer(std::unique_ptr trimmer) { // C++11 does not allow us to move a unique_ptr into a lambda. PoseGraphTrimmer* const trimmer_ptr = trimmer.release(); - AddWorkItem([this, trimmer_ptr]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); trimmers_.emplace_back(trimmer_ptr); return WorkItem::Result::kDoNotRunOptimization; }); @@ -744,14 +751,14 @@ void PoseGraph2D::AddTrimmer(std::unique_ptr trimmer) { void PoseGraph2D::RunFinalOptimization() { { - AddWorkItem([this]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); optimization_problem_->SetMaxNumIterations( options_.max_num_final_iterations()); return WorkItem::Result::kRunOptimization; }); - AddWorkItem([this]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); optimization_problem_->SetMaxNumIterations( options_.optimization_problem_options() .ceres_solver_options() @@ -773,7 +780,7 @@ void PoseGraph2D::RunOptimization() { // before Solve to avoid blocking foreground processing. optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(), data_.landmark_nodes); - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); const auto& submap_data = optimization_problem_->submap_data(); const auto& node_data = optimization_problem_->node_data(); @@ -841,14 +848,14 @@ bool PoseGraph2D::CanAddWorkItemModifying(int trajectory_id) { } MapById PoseGraph2D::GetTrajectoryNodes() const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return data_.trajectory_nodes; } MapById PoseGraph2D::GetTrajectoryNodePoses() const { MapById node_poses; - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); for (const auto& node_id_data : data_.trajectory_nodes) { absl::optional constant_pose_data; if (node_id_data.data.constant_data != nullptr) { @@ -866,7 +873,7 @@ MapById PoseGraph2D::GetTrajectoryNodePoses() std::map PoseGraph2D::GetTrajectoryStates() const { std::map trajectories_state; - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); for (const auto& it : data_.trajectories_state) { trajectories_state[it.first] = it.second.state; } @@ -876,7 +883,7 @@ PoseGraph2D::GetTrajectoryStates() const { std::map PoseGraph2D::GetLandmarkPoses() const { std::map landmark_poses; - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); for (const auto& landmark : data_.landmark_nodes) { // Landmark without value has not been optimized yet. if (!landmark.second.global_landmark_pose.has_value()) continue; @@ -888,26 +895,26 @@ std::map PoseGraph2D::GetLandmarkPoses() void PoseGraph2D::SetLandmarkPose(const std::string& landmark_id, const transform::Rigid3d& global_pose) { - AddWorkItem([=]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose; return WorkItem::Result::kDoNotRunOptimization; }); } sensor::MapByTime PoseGraph2D::GetImuData() const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return optimization_problem_->imu_data(); } sensor::MapByTime PoseGraph2D::GetOdometryData() const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return optimization_problem_->odometry_data(); } std::map PoseGraph2D::GetLandmarkNodes() const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return data_.landmark_nodes; } @@ -926,7 +933,7 @@ PoseGraph2D::GetFixedFramePoseData() const { std::vector PoseGraph2D::constraints() const { std::vector result; - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); for (const Constraint& constraint : data_.constraints) { result.push_back(Constraint{ constraint.submap_id, constraint.node_id, @@ -945,7 +952,7 @@ void PoseGraph2D::SetInitialTrajectoryPose(const int from_trajectory_id, const int to_trajectory_id, const transform::Rigid3d& pose, const common::Time time) { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); data_.initial_trajectory_poses[from_trajectory_id] = InitialTrajectoryPose{to_trajectory_id, pose, time}; } @@ -973,7 +980,7 @@ transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose( transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform( const int trajectory_id) const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return ComputeLocalToGlobalTransform(data_.global_submap_poses_2d, trajectory_id); } @@ -984,19 +991,19 @@ std::vector> PoseGraph2D::GetConnectedTrajectories() const { PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapData( const SubmapId& submap_id) const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return GetSubmapDataUnderLock(submap_id); } MapById PoseGraph2D::GetAllSubmapData() const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return GetSubmapDataUnderLock(); } MapById PoseGraph2D::GetAllSubmapPoses() const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); MapById submap_poses; for (const auto& submap_id_data : data_.submap_data) { auto submap_data = GetSubmapDataUnderLock(submap_id_data.id); diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index 55f1cfc..2fcca2c 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -28,8 +28,8 @@ #include "Eigen/Core" #include "Eigen/Geometry" +#include "absl/synchronization/mutex.h" #include "cartographer/common/fixed_ratio_sampler.h" -#include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" #include "cartographer/common/time.h" #include "cartographer/mapping/2d/submap_2d.h" @@ -81,26 +81,28 @@ class PoseGraph2D : public PoseGraph { std::shared_ptr constant_data, int trajectory_id, const std::vector>& insertion_submaps) - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); void AddOdometryData(int trajectory_id, const sensor::OdometryData& odometry_data) override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); void AddFixedFramePoseData( int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); void AddLandmarkData(int trajectory_id, const sensor::LandmarkData& landmark_data) override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); void DeleteTrajectory(int trajectory_id) override; void FinishTrajectory(int trajectory_id) override; - bool IsTrajectoryFinished(int trajectory_id) const override REQUIRES(mutex_); + bool IsTrajectoryFinished(int trajectory_id) const override + EXCLUSIVE_LOCKS_REQUIRED(mutex_); void FreezeTrajectory(int trajectory_id) override; - bool IsTrajectoryFrozen(int trajectory_id) const override REQUIRES(mutex_); + bool IsTrajectoryFrozen(int trajectory_id) const override + EXCLUSIVE_LOCKS_REQUIRED(mutex_); void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, const proto::Submap& submap) override; void AddNodeFromProto(const transform::Rigid3d& global_pose, @@ -114,56 +116,58 @@ class PoseGraph2D : public PoseGraph { void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() const override; PoseGraphInterface::SubmapData GetSubmapData(const SubmapId& submap_id) const - EXCLUDES(mutex_) override; + LOCKS_EXCLUDED(mutex_) override; MapById GetAllSubmapData() const - EXCLUDES(mutex_) override; + LOCKS_EXCLUDED(mutex_) override; MapById GetAllSubmapPoses() const - EXCLUDES(mutex_) override; + LOCKS_EXCLUDED(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) const - EXCLUDES(mutex_) override; + LOCKS_EXCLUDED(mutex_) override; MapById GetTrajectoryNodes() const override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); MapById GetTrajectoryNodePoses() const override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); std::map GetTrajectoryStates() const override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); std::map GetLandmarkPoses() const override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); void SetLandmarkPose(const std::string& landmark_id, const transform::Rigid3d& global_pose) override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); sensor::MapByTime GetImuData() const override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); sensor::MapByTime GetOdometryData() const override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); sensor::MapByTime GetFixedFramePoseData() - const override EXCLUDES(mutex_); + const override LOCKS_EXCLUDED(mutex_); std::map - GetLandmarkNodes() const override EXCLUDES(mutex_); + GetLandmarkNodes() const override LOCKS_EXCLUDED(mutex_); std::map GetTrajectoryData() const override - EXCLUDES(mutex_); - std::vector constraints() const override EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); + std::vector constraints() const override LOCKS_EXCLUDED(mutex_); void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id, const transform::Rigid3d& pose, const common::Time time) override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); void SetGlobalSlamOptimizationCallback( PoseGraphInterface::GlobalSlamOptimizationCallback callback) override; transform::Rigid3d GetInterpolatedGlobalTrajectoryPose( - int trajectory_id, const common::Time time) const REQUIRES(mutex_); + int trajectory_id, const common::Time time) const + EXCLUSIVE_LOCKS_REQUIRED(mutex_); static void RegisterMetrics(metrics::FamilyFactory* family_factory); private: MapById GetSubmapDataUnderLock() - const REQUIRES(mutex_); + const EXCLUSIVE_LOCKS_REQUIRED(mutex_); // Handles a new work item. void AddWorkItem(const std::function& work_item) - EXCLUDES(mutex_) EXCLUDES(work_queue_mutex_); + LOCKS_EXCLUDED(mutex_) LOCKS_EXCLUDED(work_queue_mutex_); // Adds connectivity and sampler for a trajectory if it does not exist. - void AddTrajectoryIfNeeded(int trajectory_id) REQUIRES(mutex_); + void AddTrajectoryIfNeeded(int trajectory_id) + EXCLUSIVE_LOCKS_REQUIRED(mutex_); // Appends the new node and submap (if needed) to the internal data // structures. @@ -171,68 +175,71 @@ class PoseGraph2D : public PoseGraph { std::shared_ptr constant_data, int trajectory_id, const std::vector>& insertion_submaps, - const transform::Rigid3d& optimized_pose) EXCLUDES(mutex_); + const transform::Rigid3d& optimized_pose) LOCKS_EXCLUDED(mutex_); // Grows the optimization problem to have an entry for every element of // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. std::vector InitializeGlobalSubmapPoses( int trajectory_id, const common::Time time, const std::vector>& insertion_submaps) - REQUIRES(mutex_); + EXCLUSIVE_LOCKS_REQUIRED(mutex_); // Adds constraints for a node, and starts scan matching in the background. WorkItem::Result ComputeConstraintsForNode( const NodeId& node_id, std::vector> insertion_submaps, - bool newly_finished_submap) EXCLUDES(mutex_); + bool newly_finished_submap) LOCKS_EXCLUDED(mutex_); // Computes constraints for a node and submap pair. void ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id) - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); // Deletes trajectories waiting for deletion. Must not be called during // constraint search. - void DeleteTrajectoriesIfNeeded() REQUIRES(mutex_); + void DeleteTrajectoriesIfNeeded() EXCLUSIVE_LOCKS_REQUIRED(mutex_); // Runs the optimization, executes the trimmers and processes the work queue. void HandleWorkQueue(const constraints::ConstraintBuilder2D::Result& result) - EXCLUDES(mutex_) EXCLUDES(work_queue_mutex_); + LOCKS_EXCLUDED(mutex_) LOCKS_EXCLUDED(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_); + void DrainWorkQueue() LOCKS_EXCLUDED(mutex_) + LOCKS_EXCLUDED(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_); + void WaitForAllComputations() LOCKS_EXCLUDED(mutex_) + LOCKS_EXCLUDED(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_); + void RunOptimization() LOCKS_EXCLUDED(mutex_); - bool CanAddWorkItemModifying(int trajectory_id) REQUIRES(mutex_); + bool CanAddWorkItemModifying(int trajectory_id) + EXCLUSIVE_LOCKS_REQUIRED(mutex_); // Computes the local to global map frame transform based on the given // 'global_submap_poses'. transform::Rigid3d ComputeLocalToGlobalTransform( const MapById& global_submap_poses, - int trajectory_id) const REQUIRES(mutex_); + int trajectory_id) const EXCLUSIVE_LOCKS_REQUIRED(mutex_); SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) const - REQUIRES(mutex_); + EXCLUSIVE_LOCKS_REQUIRED(mutex_); common::Time GetLatestNodeTime(const NodeId& node_id, const SubmapId& submap_id) const - REQUIRES(mutex_); + EXCLUSIVE_LOCKS_REQUIRED(mutex_); // Updates the trajectory connectivity structure with a new constraint. void UpdateTrajectoryConnectivity(const Constraint& constraint) - REQUIRES(mutex_); + EXCLUSIVE_LOCKS_REQUIRED(mutex_); const proto::PoseGraphOptions options_; GlobalSlamOptimizationCallback global_slam_optimization_callback_; - mutable common::Mutex mutex_; - common::Mutex work_queue_mutex_; + mutable absl::Mutex mutex_; + absl::Mutex work_queue_mutex_; // If it exists, further work items must be added to this queue, and will be // considered later. @@ -269,16 +276,17 @@ class PoseGraph2D : public PoseGraph { int num_submaps(int trajectory_id) const override; std::vector GetSubmapIds(int trajectory_id) const override; MapById GetOptimizedSubmapData() const override - REQUIRES(parent_->mutex_); + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); const MapById& GetTrajectoryNodes() const override - REQUIRES(parent_->mutex_); + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); const std::vector& GetConstraints() const override - REQUIRES(parent_->mutex_); + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); void TrimSubmap(const SubmapId& submap_id) - REQUIRES(parent_->mutex_) override; - bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_); + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_) override; + bool IsFinished(int trajectory_id) const override + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); void SetTrajectoryState(int trajectory_id, TrajectoryState state) override - REQUIRES(parent_->mutex_); + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); private: PoseGraph2D* const parent_; diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index d29562f..151afe9 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -52,7 +52,7 @@ PoseGraph3D::PoseGraph3D( PoseGraph3D::~PoseGraph3D() { WaitForAllComputations(); - common::MutexLocker locker(&work_queue_mutex_); + absl::MutexLock locker(&work_queue_mutex_); CHECK(work_queue_ == nullptr); } @@ -110,7 +110,7 @@ NodeId PoseGraph3D::AppendNode( const int trajectory_id, const std::vector>& insertion_submaps, const transform::Rigid3d& optimized_pose) { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); AddTrajectoryIfNeeded(trajectory_id); if (!CanAddWorkItemModifying(trajectory_id)) { LOG(WARNING) << "AddNode was called for finished or deleted trajectory."; @@ -144,7 +144,7 @@ NodeId PoseGraph3D::AddNode( // We have to check this here, because it might have changed by the time we // execute the lambda. const bool newly_finished_submap = insertion_submaps.front()->finished(); - AddWorkItem([=]() EXCLUDES(mutex_) { + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { return ComputeConstraintsForNode(node_id, insertion_submaps, newly_finished_submap); }); @@ -153,7 +153,7 @@ NodeId PoseGraph3D::AddNode( void PoseGraph3D::AddWorkItem( const std::function& work_item) { - common::MutexLocker locker(&work_queue_mutex_); + absl::MutexLock locker(&work_queue_mutex_); if (work_queue_ == nullptr) { work_queue_ = absl::make_unique(); auto task = absl::make_unique(); @@ -187,8 +187,8 @@ void PoseGraph3D::AddTrajectoryIfNeeded(const int trajectory_id) { void PoseGraph3D::AddImuData(const int trajectory_id, const sensor::ImuData& imu_data) { - AddWorkItem([=]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); if (CanAddWorkItemModifying(trajectory_id)) { optimization_problem_->AddImuData(trajectory_id, imu_data); } @@ -198,8 +198,8 @@ void PoseGraph3D::AddImuData(const int trajectory_id, void PoseGraph3D::AddOdometryData(const int trajectory_id, const sensor::OdometryData& odometry_data) { - AddWorkItem([=]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); if (CanAddWorkItemModifying(trajectory_id)) { optimization_problem_->AddOdometryData(trajectory_id, odometry_data); } @@ -210,8 +210,8 @@ void PoseGraph3D::AddOdometryData(const int trajectory_id, void PoseGraph3D::AddFixedFramePoseData( const int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) { - AddWorkItem([=]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); if (CanAddWorkItemModifying(trajectory_id)) { optimization_problem_->AddFixedFramePoseData(trajectory_id, fixed_frame_pose_data); @@ -222,8 +222,8 @@ void PoseGraph3D::AddFixedFramePoseData( void PoseGraph3D::AddLandmarkData(int trajectory_id, const sensor::LandmarkData& landmark_data) { - AddWorkItem([=]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); if (CanAddWorkItemModifying(trajectory_id)) { for (const auto& observation : landmark_data.landmark_observations) { data_.landmark_nodes[observation.id].landmark_observations.emplace_back( @@ -254,7 +254,7 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id, const Submap3D* submap; std::vector submap_nodes; { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished); if (!data_.submap_data.at(submap_id).submap->finished()) { // Uplink server only receives grids when they are finished, so skip @@ -316,7 +316,7 @@ WorkItem::Result PoseGraph3D::ComputeConstraintsForNode( std::vector finished_submap_ids; std::set newly_finished_submap_node_ids; { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); const auto& constant_data = data_.trajectory_nodes.at(node_id).constant_data; submap_ids = InitializeGlobalSubmapPoses( @@ -432,7 +432,7 @@ void PoseGraph3D::DeleteTrajectoriesIfNeeded() { void PoseGraph3D::HandleWorkQueue( const constraints::ConstraintBuilder3D::Result& result) { { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); data_.constraints.insert(data_.constraints.end(), result.begin(), result.end()); } @@ -442,7 +442,7 @@ void PoseGraph3D::HandleWorkQueue( std::map trajectory_id_to_last_optimized_node_id; std::map trajectory_id_to_last_optimized_submap_id; { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); const auto& submap_data = optimization_problem_->submap_data(); const auto& node_data = optimization_problem_->node_data(); for (const int trajectory_id : node_data.trajectory_ids()) { @@ -464,7 +464,7 @@ void PoseGraph3D::HandleWorkQueue( } { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); for (const Constraint& constraint : result) { UpdateTrajectoryConnectivity(constraint); } @@ -492,7 +492,7 @@ void PoseGraph3D::DrainWorkQueue() { while (process_work_queue) { std::function work_item; { - common::MutexLocker locker(&work_queue_mutex_); + absl::MutexLock locker(&work_queue_mutex_); if (work_queue_->empty()) { work_queue_.reset(); return; @@ -514,7 +514,7 @@ void PoseGraph3D::DrainWorkQueue() { void PoseGraph3D::WaitForAllComputations() { int num_trajectory_nodes; { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); num_trajectory_nodes = data_.num_trajectory_nodes; } @@ -539,29 +539,35 @@ void PoseGraph3D::WaitForAllComputations() { // First wait for the work queue to drain so that it's safe to schedule // a WhenDone() callback. { - common::MutexLocker locker(&work_queue_mutex_); - while (!locker.AwaitWithTimeout( - [this]() REQUIRES(work_queue_mutex_) { return work_queue_ == nullptr; }, - common::FromSeconds(1.))) { + const auto predicate = [this]() + EXCLUSIVE_LOCKS_REQUIRED(work_queue_mutex_) { + return work_queue_ == nullptr; + }; + absl::MutexLock locker(&work_queue_mutex_); + while (!work_queue_mutex_.AwaitWithTimeout( + absl::Condition(&predicate), + absl::FromChrono(common::FromSeconds(1.)))) { report_progress(); } } // Now wait for any pending constraint computations to finish. - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); bool notification = false; constraint_builder_.WhenDone( [this, ¬ification](const constraints::ConstraintBuilder3D::Result& result) - EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); data_.constraints.insert(data_.constraints.end(), result.begin(), result.end()); notification = true; }); - while (!locker.AwaitWithTimeout([¬ification]() - REQUIRES(mutex_) { return notification; }, - common::FromSeconds(1.))) { + const auto predicate = [¬ification]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { + return notification; + }; + while (!mutex_.AwaitWithTimeout(absl::Condition(&predicate), + absl::FromChrono(common::FromSeconds(1.)))) { report_progress(); } CHECK_EQ(constraint_builder_.GetNumFinishedNodes(), num_trajectory_nodes); @@ -570,7 +576,7 @@ void PoseGraph3D::WaitForAllComputations() { void PoseGraph3D::DeleteTrajectory(const int trajectory_id) { { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); auto it = data_.trajectories_state.find(trajectory_id); if (it == data_.trajectories_state.end()) { LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: " @@ -580,8 +586,8 @@ void PoseGraph3D::DeleteTrajectory(const int trajectory_id) { it->second.deletion_state = InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION; } - AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); CHECK(data_.trajectories_state.at(trajectory_id).state != TrajectoryState::ACTIVE); CHECK(data_.trajectories_state.at(trajectory_id).state != @@ -595,8 +601,8 @@ void PoseGraph3D::DeleteTrajectory(const int trajectory_id) { } void PoseGraph3D::FinishTrajectory(const int trajectory_id) { - AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); CHECK(!IsTrajectoryFinished(trajectory_id)); data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED; @@ -615,11 +621,11 @@ bool PoseGraph3D::IsTrajectoryFinished(const int trajectory_id) const { void PoseGraph3D::FreezeTrajectory(const int trajectory_id) { { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); data_.trajectory_connectivity_state.Add(trajectory_id); } - AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); CHECK(!IsTrajectoryFrozen(trajectory_id)); data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN; return WorkItem::Result::kDoNotRunOptimization; @@ -644,7 +650,7 @@ void PoseGraph3D::AddSubmapFromProto( std::make_shared(submap.submap_3d()); { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); AddTrajectoryIfNeeded(submap_id.trajectory_id); if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return; data_.submap_data.Insert(submap_id, InternalSubmapData()); @@ -653,8 +659,8 @@ void PoseGraph3D::AddSubmapFromProto( data_.global_submap_poses_3d.Insert( submap_id, optimization::SubmapSpec3D{global_submap_pose}); } - AddWorkItem([this, submap_id, global_submap_pose]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this, submap_id, global_submap_pose]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); data_.submap_data.at(submap_id).state = SubmapState::kFinished; optimization_problem_->InsertSubmap(submap_id, global_submap_pose); return WorkItem::Result::kDoNotRunOptimization; @@ -669,15 +675,15 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose, std::make_shared(FromProto(node.node_data())); { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); AddTrajectoryIfNeeded(node_id.trajectory_id); if (!CanAddWorkItemModifying(node_id.trajectory_id)) return; data_.trajectory_nodes.Insert(node_id, TrajectoryNode{constant_data, global_pose}); } - AddWorkItem([this, node_id, global_pose]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this, node_id, global_pose]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); const auto& constant_data = data_.trajectory_nodes.at(node_id).constant_data; optimization_problem_->InsertTrajectoryNode( @@ -701,8 +707,8 @@ void PoseGraph3D::SetTrajectoryDataFromProto( } const int trajectory_id = data.trajectory_id(); - AddWorkItem([this, trajectory_id, trajectory_data]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this, trajectory_id, trajectory_data]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); if (CanAddWorkItemModifying(trajectory_id)) { optimization_problem_->SetTrajectoryData(trajectory_id, trajectory_data); } @@ -712,8 +718,8 @@ void PoseGraph3D::SetTrajectoryDataFromProto( void PoseGraph3D::AddNodeToSubmap(const NodeId& node_id, const SubmapId& submap_id) { - AddWorkItem([this, node_id, submap_id]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this, node_id, submap_id]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); if (CanAddWorkItemModifying(submap_id.trajectory_id)) { data_.submap_data.at(submap_id).node_ids.insert(node_id); } @@ -723,8 +729,8 @@ void PoseGraph3D::AddNodeToSubmap(const NodeId& node_id, void PoseGraph3D::AddSerializedConstraints( const std::vector& constraints) { - AddWorkItem([this, constraints]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this, constraints]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); for (const auto& constraint : constraints) { CHECK(data_.trajectory_nodes.Contains(constraint.node_id)); CHECK(data_.submap_data.Contains(constraint.submap_id)); @@ -751,8 +757,8 @@ void PoseGraph3D::AddSerializedConstraints( void PoseGraph3D::AddTrimmer(std::unique_ptr trimmer) { // C++11 does not allow us to move a unique_ptr into a lambda. PoseGraphTrimmer* const trimmer_ptr = trimmer.release(); - AddWorkItem([this, trimmer_ptr]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); trimmers_.emplace_back(trimmer_ptr); return WorkItem::Result::kDoNotRunOptimization; }); @@ -760,14 +766,14 @@ void PoseGraph3D::AddTrimmer(std::unique_ptr trimmer) { void PoseGraph3D::RunFinalOptimization() { { - AddWorkItem([this]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); optimization_problem_->SetMaxNumIterations( options_.max_num_final_iterations()); return WorkItem::Result::kRunOptimization; }); - AddWorkItem([this]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); optimization_problem_->SetMaxNumIterations( options_.optimization_problem_options() .ceres_solver_options() @@ -815,7 +821,7 @@ void PoseGraph3D::RunOptimization() { // avoid blocking foreground processing. optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(), data_.landmark_nodes); - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); const auto& submap_data = optimization_problem_->submap_data(); const auto& node_data = optimization_problem_->node_data(); @@ -884,14 +890,14 @@ bool PoseGraph3D::CanAddWorkItemModifying(int trajectory_id) { } MapById PoseGraph3D::GetTrajectoryNodes() const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return data_.trajectory_nodes; } MapById PoseGraph3D::GetTrajectoryNodePoses() const { MapById node_poses; - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); for (const auto& node_id_data : data_.trajectory_nodes) { absl::optional constant_pose_data; if (node_id_data.data.constant_data != nullptr) { @@ -909,7 +915,7 @@ MapById PoseGraph3D::GetTrajectoryNodePoses() std::map PoseGraph3D::GetTrajectoryStates() const { std::map trajectories_state; - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); for (const auto& it : data_.trajectories_state) { trajectories_state[it.first] = it.second.state; } @@ -919,7 +925,7 @@ PoseGraph3D::GetTrajectoryStates() const { std::map PoseGraph3D::GetLandmarkPoses() const { std::map landmark_poses; - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); for (const auto& landmark : data_.landmark_nodes) { // Landmark without value has not been optimized yet. if (!landmark.second.global_landmark_pose.has_value()) continue; @@ -931,43 +937,43 @@ std::map PoseGraph3D::GetLandmarkPoses() void PoseGraph3D::SetLandmarkPose(const std::string& landmark_id, const transform::Rigid3d& global_pose) { - AddWorkItem([=]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose; return WorkItem::Result::kDoNotRunOptimization; }); } sensor::MapByTime PoseGraph3D::GetImuData() const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return optimization_problem_->imu_data(); } sensor::MapByTime PoseGraph3D::GetOdometryData() const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return optimization_problem_->odometry_data(); } sensor::MapByTime PoseGraph3D::GetFixedFramePoseData() const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return optimization_problem_->fixed_frame_pose_data(); } std::map PoseGraph3D::GetLandmarkNodes() const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return data_.landmark_nodes; } std::map PoseGraph3D::GetTrajectoryData() const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return optimization_problem_->trajectory_data(); } std::vector PoseGraph3D::constraints() const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return data_.constraints; } @@ -975,7 +981,7 @@ void PoseGraph3D::SetInitialTrajectoryPose(const int from_trajectory_id, const int to_trajectory_id, const transform::Rigid3d& pose, const common::Time time) { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); data_.initial_trajectory_poses[from_trajectory_id] = InitialTrajectoryPose{to_trajectory_id, pose, time}; } @@ -1003,7 +1009,7 @@ transform::Rigid3d PoseGraph3D::GetInterpolatedGlobalTrajectoryPose( transform::Rigid3d PoseGraph3D::GetLocalToGlobalTransform( const int trajectory_id) const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return ComputeLocalToGlobalTransform(data_.global_submap_poses_3d, trajectory_id); } @@ -1014,19 +1020,19 @@ std::vector> PoseGraph3D::GetConnectedTrajectories() const { PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapData( const SubmapId& submap_id) const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return GetSubmapDataUnderLock(submap_id); } MapById PoseGraph3D::GetAllSubmapData() const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return GetSubmapDataUnderLock(); } MapById PoseGraph3D::GetAllSubmapPoses() const { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); MapById submap_poses; for (const auto& submap_id_data : data_.submap_data) { auto submap_data = GetSubmapDataUnderLock(submap_id_data.id); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index 7814a86..0195c84 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -28,8 +28,8 @@ #include "Eigen/Core" #include "Eigen/Geometry" +#include "absl/synchronization/mutex.h" #include "cartographer/common/fixed_ratio_sampler.h" -#include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" #include "cartographer/common/time.h" #include "cartographer/mapping/3d/submap_3d.h" @@ -79,26 +79,28 @@ class PoseGraph3D : public PoseGraph { std::shared_ptr constant_data, int trajectory_id, const std::vector>& insertion_submaps) - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); void AddOdometryData(int trajectory_id, const sensor::OdometryData& odometry_data) override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); void AddFixedFramePoseData( int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); void AddLandmarkData(int trajectory_id, const sensor::LandmarkData& landmark_data) override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); void DeleteTrajectory(int trajectory_id) override; void FinishTrajectory(int trajectory_id) override; - bool IsTrajectoryFinished(int trajectory_id) const override REQUIRES(mutex_); + bool IsTrajectoryFinished(int trajectory_id) const override + EXCLUSIVE_LOCKS_REQUIRED(mutex_); void FreezeTrajectory(int trajectory_id) override; - bool IsTrajectoryFrozen(int trajectory_id) const override REQUIRES(mutex_); + bool IsTrajectoryFrozen(int trajectory_id) const override + EXCLUSIVE_LOCKS_REQUIRED(mutex_); void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, const proto::Submap& submap) override; void AddNodeFromProto(const transform::Rigid3d& global_pose, @@ -112,128 +114,134 @@ class PoseGraph3D : public PoseGraph { void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() const override; PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id) const - EXCLUDES(mutex_) override; + LOCKS_EXCLUDED(mutex_) override; MapById GetAllSubmapData() const - EXCLUDES(mutex_) override; + LOCKS_EXCLUDED(mutex_) override; MapById GetAllSubmapPoses() const - EXCLUDES(mutex_) override; + LOCKS_EXCLUDED(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) const - EXCLUDES(mutex_) override; + LOCKS_EXCLUDED(mutex_) override; MapById GetTrajectoryNodes() const override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); MapById GetTrajectoryNodePoses() const override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); std::map GetTrajectoryStates() const override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); std::map GetLandmarkPoses() const override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); void SetLandmarkPose(const std::string& landmark_id, const transform::Rigid3d& global_pose) override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); sensor::MapByTime GetImuData() const override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); sensor::MapByTime GetOdometryData() const override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); sensor::MapByTime GetFixedFramePoseData() - const override EXCLUDES(mutex_); + const override LOCKS_EXCLUDED(mutex_); std::map - GetLandmarkNodes() const override EXCLUDES(mutex_); + GetLandmarkNodes() const override LOCKS_EXCLUDED(mutex_); std::map GetTrajectoryData() const override; - std::vector constraints() const override EXCLUDES(mutex_); + std::vector constraints() const override LOCKS_EXCLUDED(mutex_); void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id, const transform::Rigid3d& pose, const common::Time time) override - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); void SetGlobalSlamOptimizationCallback( PoseGraphInterface::GlobalSlamOptimizationCallback callback) override; transform::Rigid3d GetInterpolatedGlobalTrajectoryPose( - int trajectory_id, const common::Time time) const REQUIRES(mutex_); + int trajectory_id, const common::Time time) const + EXCLUSIVE_LOCKS_REQUIRED(mutex_); static void RegisterMetrics(metrics::FamilyFactory* family_factory); protected: // 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_); + void WaitForAllComputations() LOCKS_EXCLUDED(mutex_) + LOCKS_EXCLUDED(work_queue_mutex_); private: - MapById GetSubmapDataUnderLock() const REQUIRES(mutex_); + MapById GetSubmapDataUnderLock() const + EXCLUSIVE_LOCKS_REQUIRED(mutex_); // Handles a new work item. void AddWorkItem(const std::function& work_item) - EXCLUDES(mutex_) EXCLUDES(work_queue_mutex_); + LOCKS_EXCLUDED(mutex_) LOCKS_EXCLUDED(work_queue_mutex_); // Adds connectivity and sampler for a trajectory if it does not exist. - void AddTrajectoryIfNeeded(int trajectory_id) REQUIRES(mutex_); + void AddTrajectoryIfNeeded(int trajectory_id) + EXCLUSIVE_LOCKS_REQUIRED(mutex_); // Appends the new node and submap (if needed) to the internal data stuctures. NodeId AppendNode( std::shared_ptr constant_data, int trajectory_id, const std::vector>& insertion_submaps, - const transform::Rigid3d& optimized_pose) EXCLUDES(mutex_); + const transform::Rigid3d& optimized_pose) LOCKS_EXCLUDED(mutex_); // Grows the optimization problem to have an entry for every element of // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. std::vector InitializeGlobalSubmapPoses( int trajectory_id, const common::Time time, const std::vector>& insertion_submaps) - REQUIRES(mutex_); + EXCLUSIVE_LOCKS_REQUIRED(mutex_); // Adds constraints for a node, and starts scan matching in the background. WorkItem::Result ComputeConstraintsForNode( const NodeId& node_id, std::vector> insertion_submaps, - bool newly_finished_submap) EXCLUDES(mutex_); + bool newly_finished_submap) LOCKS_EXCLUDED(mutex_); // Computes constraints for a node and submap pair. void ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id) - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); // Deletes trajectories waiting for deletion. Must not be called during // constraint search. - void DeleteTrajectoriesIfNeeded() REQUIRES(mutex_); + void DeleteTrajectoriesIfNeeded() EXCLUSIVE_LOCKS_REQUIRED(mutex_); // Runs the optimization, executes the trimmers and processes the work queue. void HandleWorkQueue(const constraints::ConstraintBuilder3D::Result& result) - EXCLUDES(mutex_) EXCLUDES(work_queue_mutex_); + LOCKS_EXCLUDED(mutex_) LOCKS_EXCLUDED(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_); + void DrainWorkQueue() LOCKS_EXCLUDED(mutex_) + LOCKS_EXCLUDED(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_); + void RunOptimization() LOCKS_EXCLUDED(mutex_); - bool CanAddWorkItemModifying(int trajectory_id) REQUIRES(mutex_); + bool CanAddWorkItemModifying(int trajectory_id) + EXCLUSIVE_LOCKS_REQUIRED(mutex_); // Computes the local to global map frame transform based on the given // 'global_submap_poses'. transform::Rigid3d ComputeLocalToGlobalTransform( const MapById& global_submap_poses, - int trajectory_id) const REQUIRES(mutex_); + int trajectory_id) const EXCLUSIVE_LOCKS_REQUIRED(mutex_); PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) const - REQUIRES(mutex_); + EXCLUSIVE_LOCKS_REQUIRED(mutex_); common::Time GetLatestNodeTime(const NodeId& node_id, const SubmapId& submap_id) const - REQUIRES(mutex_); + EXCLUSIVE_LOCKS_REQUIRED(mutex_); // Logs histograms for the translational and rotational residual of node // poses. - void LogResidualHistograms() const REQUIRES(mutex_); + void LogResidualHistograms() const EXCLUSIVE_LOCKS_REQUIRED(mutex_); // Updates the trajectory connectivity structure with a new constraint. void UpdateTrajectoryConnectivity(const Constraint& constraint) - REQUIRES(mutex_); + EXCLUSIVE_LOCKS_REQUIRED(mutex_); const proto::PoseGraphOptions options_; GlobalSlamOptimizationCallback global_slam_optimization_callback_; - mutable common::Mutex mutex_; - common::Mutex work_queue_mutex_; + mutable absl::Mutex mutex_; + absl::Mutex work_queue_mutex_; // If it exists, further work items must be added to this queue, and will be // considered later. @@ -268,17 +276,18 @@ class PoseGraph3D : public PoseGraph { int num_submaps(int trajectory_id) const override; std::vector GetSubmapIds(int trajectory_id) const override; MapById GetOptimizedSubmapData() const override - REQUIRES(parent_->mutex_); + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); const MapById& GetTrajectoryNodes() const override - REQUIRES(parent_->mutex_); + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); const std::vector& GetConstraints() const override - REQUIRES(parent_->mutex_); + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); void TrimSubmap(const SubmapId& submap_id) - REQUIRES(parent_->mutex_) override; - bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_); + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_) override; + bool IsFinished(int trajectory_id) const override + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); void SetTrajectoryState(int trajectory_id, TrajectoryState state) override - REQUIRES(parent_->mutex_); + EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_); private: PoseGraph3D* const parent_; diff --git a/cartographer/mapping/internal/connected_components.cc b/cartographer/mapping/internal/connected_components.cc index dca563a..4e00bf4 100644 --- a/cartographer/mapping/internal/connected_components.cc +++ b/cartographer/mapping/internal/connected_components.cc @@ -29,13 +29,13 @@ ConnectedComponents::ConnectedComponents() : lock_(), forest_(), connection_map_() {} void ConnectedComponents::Add(const int trajectory_id) { - common::MutexLocker locker(&lock_); + absl::MutexLock locker(&lock_); forest_.emplace(trajectory_id, trajectory_id); } void ConnectedComponents::Connect(const int trajectory_id_a, const int trajectory_id_b) { - common::MutexLocker locker(&lock_); + absl::MutexLock locker(&lock_); Union(trajectory_id_a, trajectory_id_b); auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b); ++connection_map_[sorted_pair]; @@ -66,7 +66,7 @@ bool ConnectedComponents::TransitivelyConnected(const int trajectory_id_a, return true; } - common::MutexLocker locker(&lock_); + absl::MutexLock locker(&lock_); if (forest_.count(trajectory_id_a) == 0 || forest_.count(trajectory_id_b) == 0) { @@ -78,7 +78,7 @@ bool ConnectedComponents::TransitivelyConnected(const int trajectory_id_a, std::vector> ConnectedComponents::Components() { // Map from cluster exemplar -> growing cluster. std::unordered_map> map; - common::MutexLocker locker(&lock_); + absl::MutexLock locker(&lock_); for (const auto& trajectory_id_entry : forest_) { map[FindSet(trajectory_id_entry.first)].push_back( trajectory_id_entry.first); @@ -93,7 +93,7 @@ std::vector> ConnectedComponents::Components() { } std::vector ConnectedComponents::GetComponent(const int trajectory_id) { - common::MutexLocker locker(&lock_); + absl::MutexLock locker(&lock_); const int set_id = FindSet(trajectory_id); std::vector trajectory_ids; for (const auto& entry : forest_) { @@ -106,7 +106,7 @@ std::vector ConnectedComponents::GetComponent(const int trajectory_id) { int ConnectedComponents::ConnectionCount(const int trajectory_id_a, const int trajectory_id_b) { - common::MutexLocker locker(&lock_); + absl::MutexLock locker(&lock_); const auto it = connection_map_.find(std::minmax(trajectory_id_a, trajectory_id_b)); return it != connection_map_.end() ? it->second : 0; diff --git a/cartographer/mapping/internal/connected_components.h b/cartographer/mapping/internal/connected_components.h index 44e7e64..cb5df96 100644 --- a/cartographer/mapping/internal/connected_components.h +++ b/cartographer/mapping/internal/connected_components.h @@ -20,7 +20,7 @@ #include #include -#include "cartographer/common/mutex.h" +#include "absl/synchronization/mutex.h" #include "cartographer/mapping/proto/connected_components.pb.h" #include "cartographer/mapping/submaps.h" @@ -41,38 +41,40 @@ class ConnectedComponents { ConnectedComponents& operator=(const ConnectedComponents&) = delete; // Add a trajectory which is initially connected to only itself. - void Add(int trajectory_id) EXCLUDES(lock_); + void Add(int trajectory_id) LOCKS_EXCLUDED(lock_); // Connect two trajectories. If either trajectory is untracked, it will be // tracked. This function is invariant to the order of its arguments. Repeated // calls to Connect increment the connectivity count. - void Connect(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_); + void Connect(int trajectory_id_a, int trajectory_id_b) LOCKS_EXCLUDED(lock_); // Determines if two trajectories have been (transitively) connected. If // either trajectory is not being tracked, returns false, except when it is // the same trajectory, where it returns true. This function is invariant to // the order of its arguments. bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b) - EXCLUDES(lock_); + LOCKS_EXCLUDED(lock_); // Return the number of _direct_ connections between 'trajectory_id_a' and // 'trajectory_id_b'. If either trajectory is not being tracked, returns 0. // This function is invariant to the order of its arguments. - int ConnectionCount(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_); + int ConnectionCount(int trajectory_id_a, int trajectory_id_b) + LOCKS_EXCLUDED(lock_); // The trajectory IDs, grouped by connectivity. - std::vector> Components() EXCLUDES(lock_); + std::vector> Components() LOCKS_EXCLUDED(lock_); // The list of trajectory IDs that belong to the same connected component as // 'trajectory_id'. - std::vector GetComponent(int trajectory_id) EXCLUDES(lock_); + std::vector GetComponent(int trajectory_id) LOCKS_EXCLUDED(lock_); private: // Find the representative and compresses the path to it. - int FindSet(int trajectory_id) REQUIRES(lock_); - void Union(int trajectory_id_a, int trajectory_id_b) REQUIRES(lock_); + int FindSet(int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(lock_); + void Union(int trajectory_id_a, int trajectory_id_b) + EXCLUSIVE_LOCKS_REQUIRED(lock_); - common::Mutex lock_; + absl::Mutex lock_; // Tracks transitive connectivity using a disjoint set forest, i.e. each // entry points towards the representative for the given trajectory. std::map forest_ GUARDED_BY(lock_); diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc index c27e3f5..58e830c 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc @@ -64,7 +64,7 @@ ConstraintBuilder2D::ConstraintBuilder2D( ceres_scan_matcher_(options.ceres_scan_matcher_options()) {} ConstraintBuilder2D::~ConstraintBuilder2D() { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK_EQ(finish_node_task_->GetState(), common::Task::NEW); CHECK_EQ(when_done_task_->GetState(), common::Task::NEW); CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called"; @@ -82,7 +82,7 @@ void ConstraintBuilder2D::MaybeAddConstraint( } if (!sampler_.Pulse()) return; - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); if (when_done_) { LOG(WARNING) << "MaybeAddConstraint was called while WhenDone was scheduled."; @@ -93,7 +93,7 @@ void ConstraintBuilder2D::MaybeAddConstraint( const auto* scan_matcher = DispatchScanMatcherConstruction(submap_id, submap->grid()); auto constraint_task = absl::make_unique(); - constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) { + constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) { ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */ constant_data, initial_relative_pose, *scan_matcher, constraint); @@ -107,7 +107,7 @@ void ConstraintBuilder2D::MaybeAddConstraint( void ConstraintBuilder2D::MaybeAddGlobalConstraint( const SubmapId& submap_id, const Submap2D* const submap, const NodeId& node_id, const TrajectoryNode::Data* const constant_data) { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); if (when_done_) { LOG(WARNING) << "MaybeAddGlobalConstraint was called while WhenDone was scheduled."; @@ -118,7 +118,7 @@ void ConstraintBuilder2D::MaybeAddGlobalConstraint( const auto* scan_matcher = DispatchScanMatcherConstruction(submap_id, submap->grid()); auto constraint_task = absl::make_unique(); - constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) { + constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) { ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */ constant_data, transform::Rigid2d::Identity(), *scan_matcher, constraint); @@ -130,10 +130,10 @@ void ConstraintBuilder2D::MaybeAddGlobalConstraint( } void ConstraintBuilder2D::NotifyEndOfNode() { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK(finish_node_task_ != nullptr); finish_node_task_->SetWorkItem([this] { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); ++num_finished_nodes_; }); auto finish_node_task_handle = @@ -145,7 +145,7 @@ void ConstraintBuilder2D::NotifyEndOfNode() { void ConstraintBuilder2D::WhenDone( const std::function& callback) { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK(when_done_ == nullptr); // TODO(gaschler): Consider using just std::function, it can also be empty. when_done_ = absl::make_unique>(callback); @@ -227,7 +227,7 @@ void ConstraintBuilder2D::ComputeConstraint( } } { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); score_histogram_.Add(score); } @@ -272,7 +272,7 @@ void ConstraintBuilder2D::RunWhenDoneCallback() { Result result; std::unique_ptr> callback; { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK(when_done_ != nullptr); for (const std::unique_ptr& constraint : constraints_) { if (constraint == nullptr) continue; @@ -292,12 +292,12 @@ void ConstraintBuilder2D::RunWhenDoneCallback() { } int ConstraintBuilder2D::GetNumFinishedNodes() { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return num_finished_nodes_; } void ConstraintBuilder2D::DeleteScanMatcher(const SubmapId& submap_id) { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); if (when_done_) { LOG(WARNING) << "DeleteScanMatcher was called while WhenDone was scheduled."; diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d.h b/cartographer/mapping/internal/constraints/constraint_builder_2d.h index 9d2520d..272f7dc 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d.h +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.h @@ -25,10 +25,10 @@ #include "Eigen/Core" #include "Eigen/Geometry" +#include "absl/synchronization/mutex.h" #include "cartographer/common/fixed_ratio_sampler.h" #include "cartographer/common/histogram.h" #include "cartographer/common/math.h" -#include "cartographer/common/mutex.h" #include "cartographer/common/task.h" #include "cartographer/common/thread_pool.h" #include "cartographer/mapping/2d/submap_2d.h" @@ -116,7 +116,8 @@ class ConstraintBuilder2D { // The returned 'grid' and 'fast_correlative_scan_matcher' must only be // accessed after 'creation_task_handle' has completed. const SubmapScanMatcher* DispatchScanMatcherConstruction( - const SubmapId& submap_id, const Grid2D* grid) REQUIRES(mutex_); + const SubmapId& submap_id, const Grid2D* grid) + EXCLUSIVE_LOCKS_REQUIRED(mutex_); // Runs in a background thread and does computations for an additional // constraint, assuming 'submap' and 'compressed_point_cloud' do not change @@ -127,13 +128,13 @@ class ConstraintBuilder2D { const transform::Rigid2d& initial_relative_pose, const SubmapScanMatcher& submap_scan_matcher, std::unique_ptr* constraint) - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); - void RunWhenDoneCallback() EXCLUDES(mutex_); + void RunWhenDoneCallback() LOCKS_EXCLUDED(mutex_); const constraints::proto::ConstraintBuilderOptions options_; common::ThreadPoolInterface* thread_pool_; - common::Mutex mutex_; + absl::Mutex mutex_; // 'callback' set by WhenDone(). std::unique_ptr> when_done_ diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc index 6aef0b0..e2f333f 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc @@ -66,7 +66,7 @@ ConstraintBuilder3D::ConstraintBuilder3D( ceres_scan_matcher_(options.ceres_scan_matcher_options_3d()) {} ConstraintBuilder3D::~ConstraintBuilder3D() { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK_EQ(finish_node_task_->GetState(), common::Task::NEW); CHECK_EQ(when_done_task_->GetState(), common::Task::NEW); CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called"; @@ -86,7 +86,7 @@ void ConstraintBuilder3D::MaybeAddConstraint( } if (!sampler_.Pulse()) return; - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); if (when_done_) { LOG(WARNING) << "MaybeAddConstraint was called while WhenDone was scheduled."; @@ -97,7 +97,7 @@ void ConstraintBuilder3D::MaybeAddConstraint( const auto* scan_matcher = DispatchScanMatcherConstruction(submap_id, submap_nodes, submap); auto constraint_task = absl::make_unique(); - constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) { + constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) { ComputeConstraint(submap_id, node_id, false, /* match_full_submap */ constant_data, global_node_pose, global_submap_pose, *scan_matcher, constraint); @@ -114,7 +114,7 @@ void ConstraintBuilder3D::MaybeAddGlobalConstraint( const std::vector& submap_nodes, const Eigen::Quaterniond& global_node_rotation, const Eigen::Quaterniond& global_submap_rotation) { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); if (when_done_) { LOG(WARNING) << "MaybeAddGlobalConstraint was called while WhenDone was scheduled."; @@ -125,7 +125,7 @@ void ConstraintBuilder3D::MaybeAddGlobalConstraint( const auto* scan_matcher = DispatchScanMatcherConstruction(submap_id, submap_nodes, submap); auto constraint_task = absl::make_unique(); - constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) { + constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) { ComputeConstraint(submap_id, node_id, true, /* match_full_submap */ constant_data, transform::Rigid3d::Rotation(global_node_rotation), @@ -139,10 +139,10 @@ void ConstraintBuilder3D::MaybeAddGlobalConstraint( } void ConstraintBuilder3D::NotifyEndOfNode() { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK(finish_node_task_ != nullptr); finish_node_task_->SetWorkItem([this] { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); ++num_finished_nodes_; }); auto finish_node_task_handle = @@ -154,7 +154,7 @@ void ConstraintBuilder3D::NotifyEndOfNode() { void ConstraintBuilder3D::WhenDone( const std::function& callback) { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK(when_done_ == nullptr); // TODO(gaschler): Consider using just std::function, it can also be empty. when_done_ = absl::make_unique>(callback); @@ -248,7 +248,7 @@ void ConstraintBuilder3D::ComputeConstraint( } } { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); score_histogram_.Add(match_result->score); rotational_score_histogram_.Add(match_result->rotational_score); low_resolution_score_histogram_.Add(match_result->low_resolution_score); @@ -301,7 +301,7 @@ void ConstraintBuilder3D::RunWhenDoneCallback() { Result result; std::unique_ptr> callback; { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); CHECK(when_done_ != nullptr); for (const std::unique_ptr& constraint : constraints_) { if (constraint == nullptr) continue; @@ -326,12 +326,12 @@ void ConstraintBuilder3D::RunWhenDoneCallback() { } int ConstraintBuilder3D::GetNumFinishedNodes() { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return num_finished_nodes_; } void ConstraintBuilder3D::DeleteScanMatcher(const SubmapId& submap_id) { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); if (when_done_) { LOG(WARNING) << "DeleteScanMatcher was called while WhenDone was scheduled."; diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d.h b/cartographer/mapping/internal/constraints/constraint_builder_3d.h index 7bb7368..12be5a6 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d.h +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d.h @@ -25,11 +25,11 @@ #include "Eigen/Core" #include "Eigen/Geometry" +#include "absl/synchronization/mutex.h" #include "cartographer/common/fixed_ratio_sampler.h" #include "cartographer/common/histogram.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/math.h" -#include "cartographer/common/mutex.h" #include "cartographer/common/task.h" #include "cartographer/common/thread_pool.h" #include "cartographer/mapping/3d/submap_3d.h" @@ -128,7 +128,7 @@ class ConstraintBuilder3D { const SubmapScanMatcher* DispatchScanMatcherConstruction( const SubmapId& submap_id, const std::vector& submap_nodes, const Submap3D* submap) - REQUIRES(mutex_); + EXCLUSIVE_LOCKS_REQUIRED(mutex_); // Runs in a background thread and does computations for an additional // constraint. @@ -140,13 +140,13 @@ class ConstraintBuilder3D { const transform::Rigid3d& global_submap_pose, const SubmapScanMatcher& submap_scan_matcher, std::unique_ptr* constraint) - EXCLUDES(mutex_); + LOCKS_EXCLUDED(mutex_); - void RunWhenDoneCallback() EXCLUDES(mutex_); + void RunWhenDoneCallback() LOCKS_EXCLUDED(mutex_); const proto::ConstraintBuilderOptions options_; common::ThreadPoolInterface* thread_pool_; - common::Mutex mutex_; + absl::Mutex mutex_; // 'callback' set by WhenDone(). std::unique_ptr> when_done_ diff --git a/cartographer/pose_graph/pose_graph_controller.cc b/cartographer/pose_graph/pose_graph_controller.cc index a342c71..b79b48c 100644 --- a/cartographer/pose_graph/pose_graph_controller.cc +++ b/cartographer/pose_graph/pose_graph_controller.cc @@ -20,17 +20,17 @@ namespace cartographer { namespace pose_graph { void PoseGraphController::AddNode(const proto::Node& node) { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); AddNodeToPoseGraphData(node, &data_); } void PoseGraphController::AddConstraint(const proto::Constraint& constraint) { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); AddConstraintToPoseGraphData(constraint, &data_); } Optimizer::SolverStatus PoseGraphController::Optimize() { - common::MutexLocker locker(&mutex_); + absl::MutexLock locker(&mutex_); return optimizer_->Solve(&data_); } diff --git a/cartographer/pose_graph/pose_graph_controller.h b/cartographer/pose_graph/pose_graph_controller.h index db847a1..fd18896 100644 --- a/cartographer/pose_graph/pose_graph_controller.h +++ b/cartographer/pose_graph/pose_graph_controller.h @@ -17,7 +17,7 @@ #ifndef CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_ #define CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_ -#include "cartographer/common/mutex.h" +#include "absl/synchronization/mutex.h" #include "cartographer/pose_graph/optimizer/optimizer.h" #include "cartographer/pose_graph/pose_graph_data.h" @@ -31,15 +31,16 @@ class PoseGraphController { PoseGraphController(const PoseGraphController&) = delete; PoseGraphController& operator=(const PoseGraphController&) = delete; - void AddNode(const proto::Node& node) EXCLUDES(mutex_); - void AddConstraint(const proto::Constraint& constraint) EXCLUDES(mutex_); + void AddNode(const proto::Node& node) LOCKS_EXCLUDED(mutex_); + void AddConstraint(const proto::Constraint& constraint) + LOCKS_EXCLUDED(mutex_); - Optimizer::SolverStatus Optimize() EXCLUDES(mutex_); + Optimizer::SolverStatus Optimize() LOCKS_EXCLUDED(mutex_); private: std::unique_ptr optimizer_; - mutable common::Mutex mutex_; + mutable absl::Mutex mutex_; PoseGraphData data_ GUARDED_BY(mutex_); };