12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

[ABSL] Purge common/mutex.h. (#1369)

master
Alexander Belyaev 2018-08-07 10:43:38 +02:00 committed by GitHub
parent 54f7f4f969
commit 8d5bf2a5dd
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
23 changed files with 439 additions and 449 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
cartographer/cloud/internal/map_builder_server.cc
View File

@ -200,7 +200,7 @@ void MapBuilderServer::OnLocalSlamResult(
->EnqueueSensorData(std::move(sensor_data)); ->EnqueueSensorData(std::move(sensor_data));
} }
common::MutexLocker locker(&subscriptions_lock_); absl::MutexLock locker(&subscriptions_lock_);
for (auto& entry : local_slam_subscriptions_[trajectory_id]) { for (auto& entry : local_slam_subscriptions_[trajectory_id]) {
auto copy_of_insertion_result = auto copy_of_insertion_result =
insertion_result insertion_result
@ -224,7 +224,7 @@ void MapBuilderServer::OnLocalSlamResult(
void MapBuilderServer::OnGlobalSlamOptimizations( void MapBuilderServer::OnGlobalSlamOptimizations(
const std::map<int, mapping::SubmapId>& last_optimized_submap_ids, const std::map<int, mapping::SubmapId>& last_optimized_submap_ids,
const std::map<int, mapping::NodeId>& last_optimized_node_ids) { const std::map<int, mapping::NodeId>& last_optimized_node_ids) {
common::MutexLocker locker(&subscriptions_lock_); absl::MutexLock locker(&subscriptions_lock_);
for (auto& entry : global_slam_subscriptions_) { for (auto& entry : global_slam_subscriptions_) {
if (!entry.second(last_optimized_submap_ids, last_optimized_node_ids)) { if (!entry.second(last_optimized_submap_ids, last_optimized_node_ids)) {
LOG(INFO) << "Removing subscription with index: " << entry.first; LOG(INFO) << "Removing subscription with index: " << entry.first;
@ -237,7 +237,7 @@ MapBuilderContextInterface::LocalSlamSubscriptionId
MapBuilderServer::SubscribeLocalSlamResults( MapBuilderServer::SubscribeLocalSlamResults(
int trajectory_id, int trajectory_id,
MapBuilderContextInterface::LocalSlamSubscriptionCallback callback) { MapBuilderContextInterface::LocalSlamSubscriptionCallback callback) {
common::MutexLocker locker(&subscriptions_lock_); absl::MutexLock locker(&subscriptions_lock_);
local_slam_subscriptions_[trajectory_id].emplace(current_subscription_index_, local_slam_subscriptions_[trajectory_id].emplace(current_subscription_index_,
callback); callback);
return MapBuilderContextInterface::LocalSlamSubscriptionId{ return MapBuilderContextInterface::LocalSlamSubscriptionId{
@ -247,7 +247,7 @@ MapBuilderServer::SubscribeLocalSlamResults(
void MapBuilderServer::UnsubscribeLocalSlamResults( void MapBuilderServer::UnsubscribeLocalSlamResults(
const MapBuilderContextInterface::LocalSlamSubscriptionId& const MapBuilderContextInterface::LocalSlamSubscriptionId&
subscription_id) { subscription_id) {
common::MutexLocker locker(&subscriptions_lock_); absl::MutexLock locker(&subscriptions_lock_);
CHECK_EQ(local_slam_subscriptions_[subscription_id.trajectory_id].erase( CHECK_EQ(local_slam_subscriptions_[subscription_id.trajectory_id].erase(
subscription_id.subscription_index), subscription_id.subscription_index),
1u); 1u);
@ -255,19 +255,19 @@ void MapBuilderServer::UnsubscribeLocalSlamResults(
int MapBuilderServer::SubscribeGlobalSlamOptimizations( int MapBuilderServer::SubscribeGlobalSlamOptimizations(
MapBuilderContextInterface::GlobalSlamOptimizationCallback callback) { MapBuilderContextInterface::GlobalSlamOptimizationCallback callback) {
common::MutexLocker locker(&subscriptions_lock_); absl::MutexLock locker(&subscriptions_lock_);
global_slam_subscriptions_.emplace(current_subscription_index_, callback); global_slam_subscriptions_.emplace(current_subscription_index_, callback);
return current_subscription_index_++; return current_subscription_index_++;
} }
void MapBuilderServer::UnsubscribeGlobalSlamOptimizations( void MapBuilderServer::UnsubscribeGlobalSlamOptimizations(
int subscription_index) { int subscription_index) {
common::MutexLocker locker(&subscriptions_lock_); absl::MutexLock locker(&subscriptions_lock_);
CHECK_EQ(global_slam_subscriptions_.erase(subscription_index), 1u); CHECK_EQ(global_slam_subscriptions_.erase(subscription_index), 1u);
} }
void MapBuilderServer::NotifyFinishTrajectory(int trajectory_id) { void MapBuilderServer::NotifyFinishTrajectory(int trajectory_id) {
common::MutexLocker locker(&subscriptions_lock_); absl::MutexLock locker(&subscriptions_lock_);
for (auto& entry : local_slam_subscriptions_[trajectory_id]) { for (auto& entry : local_slam_subscriptions_[trajectory_id]) {
MapBuilderContextInterface::LocalSlamSubscriptionCallback callback = MapBuilderContextInterface::LocalSlamSubscriptionCallback callback =
entry.second; entry.second;

2
cartographer/cloud/internal/map_builder_server.h
View File

@ -135,7 +135,7 @@ class MapBuilderServer : public MapBuilderServerInterface {
std::unique_ptr<mapping::MapBuilderInterface> map_builder_; std::unique_ptr<mapping::MapBuilderInterface> map_builder_;
common::BlockingQueue<std::unique_ptr<MapBuilderContextInterface::Data>> common::BlockingQueue<std::unique_ptr<MapBuilderContextInterface::Data>>
incoming_data_queue_; incoming_data_queue_;
common::Mutex subscriptions_lock_; absl::Mutex subscriptions_lock_;
int current_subscription_index_ = 0; int current_subscription_index_ = 0;
std::map<int /* trajectory ID */, LocalSlamResultHandlerSubscriptions> std::map<int /* trajectory ID */, LocalSlamResultHandlerSubscriptions>
local_slam_subscriptions_ GUARDED_BY(subscriptions_lock_); local_slam_subscriptions_ GUARDED_BY(subscriptions_lock_);

65
cartographer/common/blocking_queue.h
View File

@ -21,7 +21,7 @@
#include <deque> #include <deque>
#include <memory> #include <memory>
#include "cartographer/common/mutex.h" #include "absl/synchronization/mutex.h"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "glog/logging.h" #include "glog/logging.h"
@ -47,17 +47,22 @@ class BlockingQueue {
// Pushes a value onto the queue. Blocks if the queue is full. // Pushes a value onto the queue. Blocks if the queue is full.
void Push(T t) { void Push(T t) {
MutexLocker lock(&mutex_); const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
lock.Await([this]() REQUIRES(mutex_) { return QueueNotFullCondition(); }); return QueueNotFullCondition();
};
absl::MutexLock lock(&mutex_);
mutex_.Await(absl::Condition(&predicate));
deque_.push_back(std::move(t)); deque_.push_back(std::move(t));
} }
// Like push, but returns false if 'timeout' is reached. // Like push, but returns false if 'timeout' is reached.
bool PushWithTimeout(T t, const common::Duration timeout) { bool PushWithTimeout(T t, const common::Duration timeout) {
MutexLocker lock(&mutex_); const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
if (!lock.AwaitWithTimeout( return QueueNotFullCondition();
[this]() REQUIRES(mutex_) { return QueueNotFullCondition(); }, };
timeout)) { absl::MutexLock lock(&mutex_);
if (!mutex_.AwaitWithTimeout(absl::Condition(&predicate),
absl::FromChrono(timeout))) {
return false; return false;
} }
deque_.push_back(std::move(t)); 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. // Pops the next value from the queue. Blocks until a value is available.
T Pop() { T Pop() {
MutexLocker lock(&mutex_); const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
lock.Await([this]() REQUIRES(mutex_) { return !QueueEmptyCondition(); }); return !QueueEmptyCondition();
};
absl::MutexLock lock(&mutex_);
mutex_.Await(absl::Condition(&predicate));
T t = std::move(deque_.front()); T t = std::move(deque_.front());
deque_.pop_front(); deque_.pop_front();
@ -76,10 +84,12 @@ class BlockingQueue {
// Like Pop, but can timeout. Returns nullptr in this case. // Like Pop, but can timeout. Returns nullptr in this case.
T PopWithTimeout(const common::Duration timeout) { T PopWithTimeout(const common::Duration timeout) {
MutexLocker lock(&mutex_); const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
if (!lock.AwaitWithTimeout( return !QueueEmptyCondition();
[this]() REQUIRES(mutex_) { return !QueueEmptyCondition(); }, };
timeout)) { absl::MutexLock lock(&mutex_);
if (!mutex_.AwaitWithTimeout(absl::Condition(&predicate),
absl::FromChrono(timeout))) {
return nullptr; return nullptr;
} }
T t = std::move(deque_.front()); T t = std::move(deque_.front());
@ -90,10 +100,12 @@ class BlockingQueue {
// Like Peek, but can timeout. Returns nullptr in this case. // Like Peek, but can timeout. Returns nullptr in this case.
template <typename R> template <typename R>
R* PeekWithTimeout(const common::Duration timeout) { R* PeekWithTimeout(const common::Duration timeout) {
MutexLocker lock(&mutex_); const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
if (!lock.AwaitWithTimeout( return !QueueEmptyCondition();
[this]() REQUIRES(mutex_) { return !QueueEmptyCondition(); }, };
timeout)) { absl::MutexLock lock(&mutex_);
if (!mutex_.AwaitWithTimeout(absl::Condition(&predicate),
absl::FromChrono(timeout))) {
return nullptr; return nullptr;
} }
return deque_.front().get(); return deque_.front().get();
@ -104,7 +116,7 @@ class BlockingQueue {
// a pointer to the given type R. // a pointer to the given type R.
template <typename R> template <typename R>
const R* Peek() { const R* Peek() {
MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
if (deque_.empty()) { if (deque_.empty()) {
return nullptr; return nullptr;
} }
@ -113,26 +125,31 @@ class BlockingQueue {
// Returns the number of items currently in the queue. // Returns the number of items currently in the queue.
size_t Size() { size_t Size() {
MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
return deque_.size(); return deque_.size();
} }
// Blocks until the queue is empty. // Blocks until the queue is empty.
void WaitUntilEmpty() { void WaitUntilEmpty() {
MutexLocker lock(&mutex_); const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
lock.Await([this]() REQUIRES(mutex_) { return QueueEmptyCondition(); }); return QueueEmptyCondition();
};
absl::MutexLock lock(&mutex_);
mutex_.Await(absl::Condition(&predicate));
} }
private: private:
// Returns true iff the queue is empty. // 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. // 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_; return queue_size_ == kInfiniteQueueSize || deque_.size() < queue_size_;
} }
Mutex mutex_; absl::Mutex mutex_;
const size_t queue_size_ GUARDED_BY(mutex_); const size_t queue_size_ GUARDED_BY(mutex_);
std::deque<T> deque_ GUARDED_BY(mutex_); std::deque<T> deque_ GUARDED_BY(mutex_);
}; };

25
cartographer/common/internal/testing/thread_pool_for_testing.cc
View File

@ -35,7 +35,7 @@ ThreadPoolForTesting::ThreadPoolForTesting()
ThreadPoolForTesting::~ThreadPoolForTesting() { ThreadPoolForTesting::~ThreadPoolForTesting() {
{ {
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(running_); CHECK(running_);
running_ = false; running_ = false;
CHECK_EQ(task_queue_.size(), 0); CHECK_EQ(task_queue_.size(), 0);
@ -45,7 +45,7 @@ ThreadPoolForTesting::~ThreadPoolForTesting() {
} }
void ThreadPoolForTesting::NotifyDependenciesCompleted(Task* task) { void ThreadPoolForTesting::NotifyDependenciesCompleted(Task* task) {
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(running_); CHECK(running_);
auto it = tasks_not_ready_.find(task); auto it = tasks_not_ready_.find(task);
CHECK(it != tasks_not_ready_.end()); CHECK(it != tasks_not_ready_.end());
@ -56,7 +56,7 @@ void ThreadPoolForTesting::NotifyDependenciesCompleted(Task* task) {
std::weak_ptr<Task> ThreadPoolForTesting::Schedule(std::unique_ptr<Task> task) { std::weak_ptr<Task> ThreadPoolForTesting::Schedule(std::unique_ptr<Task> task) {
std::shared_ptr<Task> shared_task; std::shared_ptr<Task> shared_task;
{ {
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
idle_ = false; idle_ = false;
CHECK(running_); CHECK(running_);
auto insert_result = auto insert_result =
@ -69,11 +69,13 @@ std::weak_ptr<Task> ThreadPoolForTesting::Schedule(std::unique_ptr<Task> task) {
} }
void ThreadPoolForTesting::WaitUntilIdle() { void ThreadPoolForTesting::WaitUntilIdle() {
const auto predicate = [this]()
EXCLUSIVE_LOCKS_REQUIRED(mutex_) { return idle_; };
for (;;) { for (;;) {
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (locker.AwaitWithTimeout([this]() REQUIRES(mutex_) { return idle_; }, if (mutex_.AwaitWithTimeout(absl::Condition(&predicate),
common::FromSeconds(0.1))) { absl::FromChrono(common::FromSeconds(0.1)))) {
return; return;
} }
} }
@ -81,14 +83,15 @@ void ThreadPoolForTesting::WaitUntilIdle() {
} }
void ThreadPoolForTesting::DoWork() { void ThreadPoolForTesting::DoWork() {
const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return !task_queue_.empty() || !running_;
};
for (;;) { for (;;) {
std::shared_ptr<Task> task; std::shared_ptr<Task> task;
{ {
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
locker.AwaitWithTimeout( mutex_.AwaitWithTimeout(absl::Condition(&predicate),
[this]() absl::FromChrono(common::FromSeconds(0.1)));
REQUIRES(mutex_) { return !task_queue_.empty() || !running_; },
common::FromSeconds(0.1));
if (!task_queue_.empty()) { if (!task_queue_.empty()) {
task = task_queue_.front(); task = task_queue_.front();
task_queue_.pop_front(); task_queue_.pop_front();

8
cartographer/common/internal/testing/thread_pool_for_testing.h
View File

@ -22,7 +22,7 @@
#include <map> #include <map>
#include <thread> #include <thread>
#include "cartographer/common/mutex.h" #include "absl/synchronization/mutex.h"
#include "cartographer/common/thread_pool.h" #include "cartographer/common/thread_pool.h"
namespace cartographer { namespace cartographer {
@ -35,7 +35,7 @@ class ThreadPoolForTesting : public ThreadPoolInterface {
~ThreadPoolForTesting(); ~ThreadPoolForTesting();
std::weak_ptr<Task> Schedule(std::unique_ptr<Task> task) std::weak_ptr<Task> Schedule(std::unique_ptr<Task> task)
EXCLUDES(mutex_) override; LOCKS_EXCLUDED(mutex_) override;
void WaitUntilIdle(); void WaitUntilIdle();
@ -44,9 +44,9 @@ class ThreadPoolForTesting : public ThreadPoolInterface {
void DoWork(); 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 running_ GUARDED_BY(mutex_) = true;
bool idle_ GUARDED_BY(mutex_) = true; bool idle_ GUARDED_BY(mutex_) = true;
std::deque<std::shared_ptr<Task>> task_queue_ GUARDED_BY(mutex_); std::deque<std::shared_ptr<Task>> task_queue_ GUARDED_BY(mutex_);

67
cartographer/common/mutex.h
View File

@ -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 <typename Predicate>
void Await(Predicate predicate) REQUIRES(this) {
mutex_->Await(absl::Condition(&predicate));
}
template <typename Predicate>
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_

16
cartographer/common/task.cc
View File

@ -27,12 +27,12 @@ Task::~Task() {
} }
Task::State Task::GetState() { Task::State Task::GetState() {
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return state_; return state_;
} }
void Task::SetWorkItem(const WorkItem& work_item) { void Task::SetWorkItem(const WorkItem& work_item) {
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK_EQ(state_, NEW); CHECK_EQ(state_, NEW);
work_item_ = work_item; work_item_ = work_item;
} }
@ -40,7 +40,7 @@ void Task::SetWorkItem(const WorkItem& work_item) {
void Task::AddDependency(std::weak_ptr<Task> dependency) { void Task::AddDependency(std::weak_ptr<Task> dependency) {
std::shared_ptr<Task> shared_dependency; std::shared_ptr<Task> shared_dependency;
{ {
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK_EQ(state_, NEW); CHECK_EQ(state_, NEW);
if ((shared_dependency = dependency.lock())) { if ((shared_dependency = dependency.lock())) {
++uncompleted_dependencies_; ++uncompleted_dependencies_;
@ -52,7 +52,7 @@ void Task::AddDependency(std::weak_ptr<Task> dependency) {
} }
void Task::SetThreadPool(ThreadPoolInterface* thread_pool) { void Task::SetThreadPool(ThreadPoolInterface* thread_pool) {
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK_EQ(state_, NEW); CHECK_EQ(state_, NEW);
state_ = DISPATCHED; state_ = DISPATCHED;
thread_pool_to_notify_ = thread_pool; thread_pool_to_notify_ = thread_pool;
@ -64,7 +64,7 @@ void Task::SetThreadPool(ThreadPoolInterface* thread_pool) {
} }
void Task::AddDependentTask(Task* dependent_task) { void Task::AddDependentTask(Task* dependent_task) {
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (state_ == COMPLETED) { if (state_ == COMPLETED) {
dependent_task->OnDependenyCompleted(); dependent_task->OnDependenyCompleted();
return; return;
@ -74,7 +74,7 @@ void Task::AddDependentTask(Task* dependent_task) {
} }
void Task::OnDependenyCompleted() { void Task::OnDependenyCompleted() {
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(state_ == NEW || state_ == DISPATCHED); CHECK(state_ == NEW || state_ == DISPATCHED);
--uncompleted_dependencies_; --uncompleted_dependencies_;
if (uncompleted_dependencies_ == 0 && state_ == DISPATCHED) { if (uncompleted_dependencies_ == 0 && state_ == DISPATCHED) {
@ -86,7 +86,7 @@ void Task::OnDependenyCompleted() {
void Task::Execute() { void Task::Execute() {
{ {
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK_EQ(state_, DEPENDENCIES_COMPLETED); CHECK_EQ(state_, DEPENDENCIES_COMPLETED);
state_ = RUNNING; state_ = RUNNING;
} }
@ -96,7 +96,7 @@ void Task::Execute() {
work_item_(); work_item_();
} }
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
state_ = COMPLETED; state_ = COMPLETED;
for (Task* dependent_task : dependent_tasks_) { for (Task* dependent_task : dependent_tasks_) {
dependent_task->OnDependenyCompleted(); dependent_task->OnDependenyCompleted();

14
cartographer/common/task.h
View File

@ -19,7 +19,7 @@
#include <set> #include <set>
#include "cartographer/common/mutex.h" #include "absl/synchronization/mutex.h"
#include "glog/logging.h" #include "glog/logging.h"
#include "thread_pool.h" #include "thread_pool.h"
@ -38,24 +38,24 @@ class Task {
Task() = default; Task() = default;
~Task(); ~Task();
State GetState() EXCLUDES(mutex_); State GetState() LOCKS_EXCLUDED(mutex_);
// State must be 'NEW'. // 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 // State must be 'NEW'. 'dependency' may be nullptr, in which case it is
// assumed completed. // assumed completed.
void AddDependency(std::weak_ptr<Task> dependency) EXCLUDES(mutex_); void AddDependency(std::weak_ptr<Task> dependency) LOCKS_EXCLUDED(mutex_);
private: private:
// Allowed in all states. // Allowed in all states.
void AddDependentTask(Task* dependent_task); void AddDependentTask(Task* dependent_task);
// State must be 'DEPENDENCIES_COMPLETED' and becomes 'COMPLETED'. // 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'. // 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 // State must be 'NEW' or 'DISPATCHED'. If 'DISPATCHED', may become
// 'DEPENDENCIES_COMPLETED'. // 'DEPENDENCIES_COMPLETED'.
@ -67,7 +67,7 @@ class Task {
unsigned int uncompleted_dependencies_ GUARDED_BY(mutex_) = 0; unsigned int uncompleted_dependencies_ GUARDED_BY(mutex_) = 0;
std::set<Task*> dependent_tasks_ GUARDED_BY(mutex_); std::set<Task*> dependent_tasks_ GUARDED_BY(mutex_);
Mutex mutex_; absl::Mutex mutex_;
}; };
} // namespace common } // namespace common

17
cartographer/common/thread_pool.cc
View File

@ -35,7 +35,7 @@ void ThreadPoolInterface::SetThreadPool(Task* task) {
} }
ThreadPool::ThreadPool(int num_threads) { ThreadPool::ThreadPool(int num_threads) {
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
for (int i = 0; i != num_threads; ++i) { for (int i = 0; i != num_threads; ++i) {
pool_.emplace_back([this]() { ThreadPool::DoWork(); }); pool_.emplace_back([this]() { ThreadPool::DoWork(); });
} }
@ -43,7 +43,7 @@ ThreadPool::ThreadPool(int num_threads) {
ThreadPool::~ThreadPool() { ThreadPool::~ThreadPool() {
{ {
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(running_); CHECK(running_);
running_ = false; running_ = false;
} }
@ -53,7 +53,7 @@ ThreadPool::~ThreadPool() {
} }
void ThreadPool::NotifyDependenciesCompleted(Task* task) { void ThreadPool::NotifyDependenciesCompleted(Task* task) {
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
auto it = tasks_not_ready_.find(task); auto it = tasks_not_ready_.find(task);
CHECK(it != tasks_not_ready_.end()); CHECK(it != tasks_not_ready_.end());
task_queue_.push_back(it->second); task_queue_.push_back(it->second);
@ -63,7 +63,7 @@ void ThreadPool::NotifyDependenciesCompleted(Task* task) {
std::weak_ptr<Task> ThreadPool::Schedule(std::unique_ptr<Task> task) { std::weak_ptr<Task> ThreadPool::Schedule(std::unique_ptr<Task> task) {
std::shared_ptr<Task> shared_task; std::shared_ptr<Task> shared_task;
{ {
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
auto insert_result = auto insert_result =
tasks_not_ready_.insert(std::make_pair(task.get(), std::move(task))); tasks_not_ready_.insert(std::make_pair(task.get(), std::move(task)));
CHECK(insert_result.second) << "Schedule called twice"; CHECK(insert_result.second) << "Schedule called twice";
@ -80,13 +80,14 @@ void ThreadPool::DoWork() {
// away CPU resources from more important foreground threads. // away CPU resources from more important foreground threads.
CHECK_NE(nice(10), -1); CHECK_NE(nice(10), -1);
#endif #endif
const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return !task_queue_.empty() || !running_;
};
for (;;) { for (;;) {
std::shared_ptr<Task> task; std::shared_ptr<Task> task;
{ {
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
locker.Await([this]() REQUIRES(mutex_) { mutex_.Await(absl::Condition(&predicate));
return !task_queue_.empty() || !running_;
});
if (!task_queue_.empty()) { if (!task_queue_.empty()) {
task = std::move(task_queue_.front()); task = std::move(task_queue_.front());
task_queue_.pop_front(); task_queue_.pop_front();

8
cartographer/common/thread_pool.h
View File

@ -24,7 +24,7 @@
#include <unordered_map> #include <unordered_map>
#include <vector> #include <vector>
#include "cartographer/common/mutex.h" #include "absl/synchronization/mutex.h"
#include "cartographer/common/task.h" #include "cartographer/common/task.h"
namespace cartographer { namespace cartographer {
@ -65,14 +65,14 @@ class ThreadPool : public ThreadPoolInterface {
// When the returned weak pointer is expired, 'task' has certainly completed, // When the returned weak pointer is expired, 'task' has certainly completed,
// so dependants no longer need to add it as a dependency. // so dependants no longer need to add it as a dependency.
std::weak_ptr<Task> Schedule(std::unique_ptr<Task> task) std::weak_ptr<Task> Schedule(std::unique_ptr<Task> task)
EXCLUDES(mutex_) override; LOCKS_EXCLUDED(mutex_) override;
private: private:
void DoWork(); 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 running_ GUARDED_BY(mutex_) = true;
std::vector<std::thread> pool_ GUARDED_BY(mutex_); std::vector<std::thread> pool_ GUARDED_BY(mutex_);
std::deque<std::shared_ptr<Task>> task_queue_ GUARDED_BY(mutex_); std::deque<std::shared_ptr<Task>> task_queue_ GUARDED_BY(mutex_);

14
cartographer/common/thread_pool_test.cc
View File

@ -28,19 +28,21 @@ namespace {
class Receiver { class Receiver {
public: public:
void Receive(int number) { void Receive(int number) {
MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
received_numbers_.push_back(number); received_numbers_.push_back(number);
} }
void WaitForNumberSequence(const std::vector<int>& expected_numbers) { void WaitForNumberSequence(const std::vector<int>& expected_numbers) {
common::MutexLocker locker(&mutex_); const auto predicate =
locker.Await([this, &expected_numbers]() REQUIRES(mutex_) { [this, &expected_numbers]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return (received_numbers_.size() >= expected_numbers.size()); return (received_numbers_.size() >= expected_numbers.size());
}); };
absl::MutexLock locker(&mutex_);
mutex_.Await(absl::Condition(&predicate));
EXPECT_EQ(expected_numbers, received_numbers_); EXPECT_EQ(expected_numbers, received_numbers_);
} }
Mutex mutex_; absl::Mutex mutex_;
std::vector<int> received_numbers_ GUARDED_BY(mutex_); std::vector<int> received_numbers_ GUARDED_BY(mutex_);
}; };

149
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -53,7 +53,7 @@ PoseGraph2D::PoseGraph2D(
PoseGraph2D::~PoseGraph2D() { PoseGraph2D::~PoseGraph2D() {
WaitForAllComputations(); WaitForAllComputations();
common::MutexLocker locker(&work_queue_mutex_); absl::MutexLock locker(&work_queue_mutex_);
CHECK(work_queue_ == nullptr); CHECK(work_queue_ == nullptr);
} }
@ -113,7 +113,7 @@ NodeId PoseGraph2D::AppendNode(
const int trajectory_id, const int trajectory_id,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps, const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps,
const transform::Rigid3d& optimized_pose) { const transform::Rigid3d& optimized_pose) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
AddTrajectoryIfNeeded(trajectory_id); AddTrajectoryIfNeeded(trajectory_id);
if (!CanAddWorkItemModifying(trajectory_id)) { if (!CanAddWorkItemModifying(trajectory_id)) {
LOG(WARNING) << "AddNode was called for finished or deleted trajectory."; 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 // We have to check this here, because it might have changed by the time we
// execute the lambda. // execute the lambda.
const bool newly_finished_submap = insertion_submaps.front()->finished(); const bool newly_finished_submap = insertion_submaps.front()->finished();
AddWorkItem([=]() EXCLUDES(mutex_) { AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
return ComputeConstraintsForNode(node_id, insertion_submaps, return ComputeConstraintsForNode(node_id, insertion_submaps,
newly_finished_submap); newly_finished_submap);
}); });
@ -156,7 +156,7 @@ NodeId PoseGraph2D::AddNode(
void PoseGraph2D::AddWorkItem( void PoseGraph2D::AddWorkItem(
const std::function<WorkItem::Result()>& work_item) { const std::function<WorkItem::Result()>& work_item) {
common::MutexLocker locker(&work_queue_mutex_); absl::MutexLock locker(&work_queue_mutex_);
if (work_queue_ == nullptr) { if (work_queue_ == nullptr) {
work_queue_ = absl::make_unique<WorkQueue>(); work_queue_ = absl::make_unique<WorkQueue>();
auto task = absl::make_unique<common::Task>(); auto task = absl::make_unique<common::Task>();
@ -190,8 +190,8 @@ void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) {
void PoseGraph2D::AddImuData(const int trajectory_id, void PoseGraph2D::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) { const sensor::ImuData& imu_data) {
AddWorkItem([=]() EXCLUDES(mutex_) { AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) { if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddImuData(trajectory_id, imu_data); 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, void PoseGraph2D::AddOdometryData(const int trajectory_id,
const sensor::OdometryData& odometry_data) { const sensor::OdometryData& odometry_data) {
AddWorkItem([=]() EXCLUDES(mutex_) { AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) { if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddOdometryData(trajectory_id, odometry_data); optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
} }
@ -218,8 +218,8 @@ void PoseGraph2D::AddFixedFramePoseData(
void PoseGraph2D::AddLandmarkData(int trajectory_id, void PoseGraph2D::AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data) { const sensor::LandmarkData& landmark_data) {
AddWorkItem([=]() EXCLUDES(mutex_) { AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) { if (CanAddWorkItemModifying(trajectory_id)) {
for (const auto& observation : landmark_data.landmark_observations) { for (const auto& observation : landmark_data.landmark_observations) {
data_.landmark_nodes[observation.id].landmark_observations.emplace_back( 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 TrajectoryNode::Data* constant_data;
const Submap2D* submap; const Submap2D* submap;
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished); CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
if (!data_.submap_data.at(submap_id).submap->finished()) { if (!data_.submap_data.at(submap_id).submap->finished()) {
// Uplink server only receives grids when they are finished, so skip // Uplink server only receives grids when they are finished, so skip
@ -292,7 +292,7 @@ WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
std::vector<SubmapId> finished_submap_ids; std::vector<SubmapId> finished_submap_ids;
std::set<NodeId> newly_finished_submap_node_ids; std::set<NodeId> newly_finished_submap_node_ids;
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
const auto& constant_data = const auto& constant_data =
data_.trajectory_nodes.at(node_id).constant_data; data_.trajectory_nodes.at(node_id).constant_data;
submap_ids = InitializeGlobalSubmapPoses( submap_ids = InitializeGlobalSubmapPoses(
@ -417,7 +417,7 @@ void PoseGraph2D::DeleteTrajectoriesIfNeeded() {
void PoseGraph2D::HandleWorkQueue( void PoseGraph2D::HandleWorkQueue(
const constraints::ConstraintBuilder2D::Result& result) { const constraints::ConstraintBuilder2D::Result& result) {
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
data_.constraints.insert(data_.constraints.end(), result.begin(), data_.constraints.insert(data_.constraints.end(), result.begin(),
result.end()); result.end());
} }
@ -427,7 +427,7 @@ void PoseGraph2D::HandleWorkQueue(
std::map<int, NodeId> trajectory_id_to_last_optimized_node_id; std::map<int, NodeId> trajectory_id_to_last_optimized_node_id;
std::map<int, SubmapId> trajectory_id_to_last_optimized_submap_id; std::map<int, SubmapId> 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& submap_data = optimization_problem_->submap_data();
const auto& node_data = optimization_problem_->node_data(); const auto& node_data = optimization_problem_->node_data();
for (const int trajectory_id : node_data.trajectory_ids()) { 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) { for (const Constraint& constraint : result) {
UpdateTrajectoryConnectivity(constraint); UpdateTrajectoryConnectivity(constraint);
} }
@ -477,7 +477,7 @@ void PoseGraph2D::DrainWorkQueue() {
while (process_work_queue) { while (process_work_queue) {
std::function<WorkItem::Result()> work_item; std::function<WorkItem::Result()> work_item;
{ {
common::MutexLocker locker(&work_queue_mutex_); absl::MutexLock locker(&work_queue_mutex_);
if (work_queue_->empty()) { if (work_queue_->empty()) {
work_queue_.reset(); work_queue_.reset();
return; return;
@ -499,7 +499,7 @@ void PoseGraph2D::DrainWorkQueue() {
void PoseGraph2D::WaitForAllComputations() { void PoseGraph2D::WaitForAllComputations() {
int num_trajectory_nodes; int num_trajectory_nodes;
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
num_trajectory_nodes = data_.num_trajectory_nodes; 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 // First wait for the work queue to drain so that it's safe to schedule
// a WhenDone() callback. // a WhenDone() callback.
{ {
common::MutexLocker locker(&work_queue_mutex_); const auto predicate = [this]()
while (!locker.AwaitWithTimeout( EXCLUSIVE_LOCKS_REQUIRED(work_queue_mutex_) {
[this]() REQUIRES(work_queue_mutex_) { return work_queue_ == nullptr; }, return work_queue_ == nullptr;
common::FromSeconds(1.))) { };
absl::MutexLock locker(&work_queue_mutex_);
while (!work_queue_mutex_.AwaitWithTimeout(
absl::Condition(&predicate),
absl::FromChrono(common::FromSeconds(1.)))) {
report_progress(); report_progress();
} }
} }
// Now wait for any pending constraint computations to finish. // Now wait for any pending constraint computations to finish.
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
bool notification = false; bool notification = false;
constraint_builder_.WhenDone( constraint_builder_.WhenDone(
[this, [this,
&notification](const constraints::ConstraintBuilder2D::Result& result) &notification](const constraints::ConstraintBuilder2D::Result& result)
EXCLUDES(mutex_) { LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
data_.constraints.insert(data_.constraints.end(), result.begin(), data_.constraints.insert(data_.constraints.end(), result.begin(),
result.end()); result.end());
notification = true; notification = true;
}); });
while (!locker.AwaitWithTimeout([&notification]() const auto predicate = [&notification]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
REQUIRES(mutex_) { return notification; }, return notification;
common::FromSeconds(1.))) { };
while (!mutex_.AwaitWithTimeout(absl::Condition(&predicate),
absl::FromChrono(common::FromSeconds(1.)))) {
report_progress(); report_progress();
} }
CHECK_EQ(constraint_builder_.GetNumFinishedNodes(), num_trajectory_nodes); CHECK_EQ(constraint_builder_.GetNumFinishedNodes(), num_trajectory_nodes);
@ -555,7 +561,7 @@ void PoseGraph2D::WaitForAllComputations() {
void PoseGraph2D::DeleteTrajectory(const int trajectory_id) { void PoseGraph2D::DeleteTrajectory(const int trajectory_id) {
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
auto it = data_.trajectories_state.find(trajectory_id); auto it = data_.trajectories_state.find(trajectory_id);
if (it == data_.trajectories_state.end()) { if (it == data_.trajectories_state.end()) {
LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: " 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 = it->second.deletion_state =
InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION; InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION;
} }
AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) { AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(data_.trajectories_state.at(trajectory_id).state != CHECK(data_.trajectories_state.at(trajectory_id).state !=
TrajectoryState::ACTIVE); TrajectoryState::ACTIVE);
CHECK(data_.trajectories_state.at(trajectory_id).state != 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) { void PoseGraph2D::FinishTrajectory(const int trajectory_id) {
AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) { AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(!IsTrajectoryFinished(trajectory_id)); CHECK(!IsTrajectoryFinished(trajectory_id));
data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED; 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) { void PoseGraph2D::FreezeTrajectory(const int trajectory_id) {
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
data_.trajectory_connectivity_state.Add(trajectory_id); data_.trajectory_connectivity_state.Add(trajectory_id);
} }
AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) { AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(!IsTrajectoryFrozen(trajectory_id)); CHECK(!IsTrajectoryFrozen(trajectory_id));
data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN; data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN;
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
@ -629,7 +635,7 @@ void PoseGraph2D::AddSubmapFromProto(
const transform::Rigid2d global_submap_pose_2d = const transform::Rigid2d global_submap_pose_2d =
transform::Project2D(global_submap_pose); transform::Project2D(global_submap_pose);
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
const std::shared_ptr<const Submap2D> submap_ptr = const std::shared_ptr<const Submap2D> submap_ptr =
std::make_shared<const Submap2D>(submap.submap_2d(), std::make_shared<const Submap2D>(submap.submap_2d(),
&conversion_tables_); &conversion_tables_);
@ -641,12 +647,13 @@ void PoseGraph2D::AddSubmapFromProto(
data_.global_submap_poses_2d.Insert( data_.global_submap_poses_2d.Insert(
submap_id, optimization::SubmapSpec2D{global_submap_pose_2d}); submap_id, optimization::SubmapSpec2D{global_submap_pose_2d});
} }
AddWorkItem([this, submap_id, global_submap_pose_2d]() EXCLUDES(mutex_) { AddWorkItem(
common::MutexLocker locker(&mutex_); [this, submap_id, global_submap_pose_2d]() LOCKS_EXCLUDED(mutex_) {
data_.submap_data.at(submap_id).state = SubmapState::kFinished; absl::MutexLock locker(&mutex_);
optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d); data_.submap_data.at(submap_id).state = SubmapState::kFinished;
return WorkItem::Result::kDoNotRunOptimization; optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d);
}); return WorkItem::Result::kDoNotRunOptimization;
});
} }
void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose, void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
@ -657,15 +664,15 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
std::make_shared<const TrajectoryNode::Data>(FromProto(node.node_data())); std::make_shared<const TrajectoryNode::Data>(FromProto(node.node_data()));
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
AddTrajectoryIfNeeded(node_id.trajectory_id); AddTrajectoryIfNeeded(node_id.trajectory_id);
if (!CanAddWorkItemModifying(node_id.trajectory_id)) return; if (!CanAddWorkItemModifying(node_id.trajectory_id)) return;
data_.trajectory_nodes.Insert(node_id, data_.trajectory_nodes.Insert(node_id,
TrajectoryNode{constant_data, global_pose}); TrajectoryNode{constant_data, global_pose});
} }
AddWorkItem([this, node_id, global_pose]() EXCLUDES(mutex_) { AddWorkItem([this, node_id, global_pose]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
const auto& constant_data = const auto& constant_data =
data_.trajectory_nodes.at(node_id).constant_data; data_.trajectory_nodes.at(node_id).constant_data;
const auto gravity_alignment_inverse = transform::Rigid3d::Rotation( const auto gravity_alignment_inverse = transform::Rigid3d::Rotation(
@ -689,8 +696,8 @@ void PoseGraph2D::SetTrajectoryDataFromProto(
void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id, void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id,
const SubmapId& submap_id) { const SubmapId& submap_id) {
AddWorkItem([this, node_id, submap_id]() EXCLUDES(mutex_) { AddWorkItem([this, node_id, submap_id]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(submap_id.trajectory_id)) { if (CanAddWorkItemModifying(submap_id.trajectory_id)) {
data_.submap_data.at(submap_id).node_ids.insert(node_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( void PoseGraph2D::AddSerializedConstraints(
const std::vector<Constraint>& constraints) { const std::vector<Constraint>& constraints) {
AddWorkItem([this, constraints]() EXCLUDES(mutex_) { AddWorkItem([this, constraints]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
for (const auto& constraint : constraints) { for (const auto& constraint : constraints) {
CHECK(data_.trajectory_nodes.Contains(constraint.node_id)); CHECK(data_.trajectory_nodes.Contains(constraint.node_id));
CHECK(data_.submap_data.Contains(constraint.submap_id)); CHECK(data_.submap_data.Contains(constraint.submap_id));
@ -735,8 +742,8 @@ void PoseGraph2D::AddSerializedConstraints(
void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) { void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {
// C++11 does not allow us to move a unique_ptr into a lambda. // C++11 does not allow us to move a unique_ptr into a lambda.
PoseGraphTrimmer* const trimmer_ptr = trimmer.release(); PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
AddWorkItem([this, trimmer_ptr]() EXCLUDES(mutex_) { AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
trimmers_.emplace_back(trimmer_ptr); trimmers_.emplace_back(trimmer_ptr);
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
}); });
@ -744,14 +751,14 @@ void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {
void PoseGraph2D::RunFinalOptimization() { void PoseGraph2D::RunFinalOptimization() {
{ {
AddWorkItem([this]() EXCLUDES(mutex_) { AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
optimization_problem_->SetMaxNumIterations( optimization_problem_->SetMaxNumIterations(
options_.max_num_final_iterations()); options_.max_num_final_iterations());
return WorkItem::Result::kRunOptimization; return WorkItem::Result::kRunOptimization;
}); });
AddWorkItem([this]() EXCLUDES(mutex_) { AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
optimization_problem_->SetMaxNumIterations( optimization_problem_->SetMaxNumIterations(
options_.optimization_problem_options() options_.optimization_problem_options()
.ceres_solver_options() .ceres_solver_options()
@ -773,7 +780,7 @@ void PoseGraph2D::RunOptimization() {
// before Solve to avoid blocking foreground processing. // before Solve to avoid blocking foreground processing.
optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(), optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
data_.landmark_nodes); data_.landmark_nodes);
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
const auto& submap_data = optimization_problem_->submap_data(); const auto& submap_data = optimization_problem_->submap_data();
const auto& node_data = optimization_problem_->node_data(); const auto& node_data = optimization_problem_->node_data();
@ -841,14 +848,14 @@ bool PoseGraph2D::CanAddWorkItemModifying(int trajectory_id) {
} }
MapById<NodeId, TrajectoryNode> PoseGraph2D::GetTrajectoryNodes() const { MapById<NodeId, TrajectoryNode> PoseGraph2D::GetTrajectoryNodes() const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return data_.trajectory_nodes; return data_.trajectory_nodes;
} }
MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses() MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses()
const { const {
MapById<NodeId, TrajectoryNodePose> node_poses; MapById<NodeId, TrajectoryNodePose> node_poses;
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
for (const auto& node_id_data : data_.trajectory_nodes) { for (const auto& node_id_data : data_.trajectory_nodes) {
absl::optional<TrajectoryNodePose::ConstantPoseData> constant_pose_data; absl::optional<TrajectoryNodePose::ConstantPoseData> constant_pose_data;
if (node_id_data.data.constant_data != nullptr) { if (node_id_data.data.constant_data != nullptr) {
@ -866,7 +873,7 @@ MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses()
std::map<int, PoseGraphInterface::TrajectoryState> std::map<int, PoseGraphInterface::TrajectoryState>
PoseGraph2D::GetTrajectoryStates() const { PoseGraph2D::GetTrajectoryStates() const {
std::map<int, PoseGraphInterface::TrajectoryState> trajectories_state; std::map<int, PoseGraphInterface::TrajectoryState> trajectories_state;
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
for (const auto& it : data_.trajectories_state) { for (const auto& it : data_.trajectories_state) {
trajectories_state[it.first] = it.second.state; trajectories_state[it.first] = it.second.state;
} }
@ -876,7 +883,7 @@ PoseGraph2D::GetTrajectoryStates() const {
std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses() std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses()
const { const {
std::map<std::string, transform::Rigid3d> landmark_poses; std::map<std::string, transform::Rigid3d> landmark_poses;
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
for (const auto& landmark : data_.landmark_nodes) { for (const auto& landmark : data_.landmark_nodes) {
// Landmark without value has not been optimized yet. // Landmark without value has not been optimized yet.
if (!landmark.second.global_landmark_pose.has_value()) continue; if (!landmark.second.global_landmark_pose.has_value()) continue;
@ -888,26 +895,26 @@ std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses()
void PoseGraph2D::SetLandmarkPose(const std::string& landmark_id, void PoseGraph2D::SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) { const transform::Rigid3d& global_pose) {
AddWorkItem([=]() EXCLUDES(mutex_) { AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose; data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose;
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
}); });
} }
sensor::MapByTime<sensor::ImuData> PoseGraph2D::GetImuData() const { sensor::MapByTime<sensor::ImuData> PoseGraph2D::GetImuData() const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return optimization_problem_->imu_data(); return optimization_problem_->imu_data();
} }
sensor::MapByTime<sensor::OdometryData> PoseGraph2D::GetOdometryData() const { sensor::MapByTime<sensor::OdometryData> PoseGraph2D::GetOdometryData() const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return optimization_problem_->odometry_data(); return optimization_problem_->odometry_data();
} }
std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode> std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
PoseGraph2D::GetLandmarkNodes() const { PoseGraph2D::GetLandmarkNodes() const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return data_.landmark_nodes; return data_.landmark_nodes;
} }
@ -926,7 +933,7 @@ PoseGraph2D::GetFixedFramePoseData() const {
std::vector<PoseGraphInterface::Constraint> PoseGraph2D::constraints() const { std::vector<PoseGraphInterface::Constraint> PoseGraph2D::constraints() const {
std::vector<PoseGraphInterface::Constraint> result; std::vector<PoseGraphInterface::Constraint> result;
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
for (const Constraint& constraint : data_.constraints) { for (const Constraint& constraint : data_.constraints) {
result.push_back(Constraint{ result.push_back(Constraint{
constraint.submap_id, constraint.node_id, constraint.submap_id, constraint.node_id,
@ -945,7 +952,7 @@ void PoseGraph2D::SetInitialTrajectoryPose(const int from_trajectory_id,
const int to_trajectory_id, const int to_trajectory_id,
const transform::Rigid3d& pose, const transform::Rigid3d& pose,
const common::Time time) { const common::Time time) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
data_.initial_trajectory_poses[from_trajectory_id] = data_.initial_trajectory_poses[from_trajectory_id] =
InitialTrajectoryPose{to_trajectory_id, pose, time}; InitialTrajectoryPose{to_trajectory_id, pose, time};
} }
@ -973,7 +980,7 @@ transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose(
transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform( transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform(
const int trajectory_id) const { const int trajectory_id) const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return ComputeLocalToGlobalTransform(data_.global_submap_poses_2d, return ComputeLocalToGlobalTransform(data_.global_submap_poses_2d,
trajectory_id); trajectory_id);
} }
@ -984,19 +991,19 @@ std::vector<std::vector<int>> PoseGraph2D::GetConnectedTrajectories() const {
PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapData( PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapData(
const SubmapId& submap_id) const { const SubmapId& submap_id) const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return GetSubmapDataUnderLock(submap_id); return GetSubmapDataUnderLock(submap_id);
} }
MapById<SubmapId, PoseGraphInterface::SubmapData> MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph2D::GetAllSubmapData() const { PoseGraph2D::GetAllSubmapData() const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return GetSubmapDataUnderLock(); return GetSubmapDataUnderLock();
} }
MapById<SubmapId, PoseGraphInterface::SubmapPose> MapById<SubmapId, PoseGraphInterface::SubmapPose>
PoseGraph2D::GetAllSubmapPoses() const { PoseGraph2D::GetAllSubmapPoses() const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
MapById<SubmapId, SubmapPose> submap_poses; MapById<SubmapId, SubmapPose> submap_poses;
for (const auto& submap_id_data : data_.submap_data) { for (const auto& submap_id_data : data_.submap_data) {
auto submap_data = GetSubmapDataUnderLock(submap_id_data.id); auto submap_data = GetSubmapDataUnderLock(submap_id_data.id);

108
cartographer/mapping/internal/2d/pose_graph_2d.h
View File

@ -28,8 +28,8 @@
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "absl/synchronization/mutex.h"
#include "cartographer/common/fixed_ratio_sampler.h" #include "cartographer/common/fixed_ratio_sampler.h"
#include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.h" #include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/2d/submap_2d.h" #include "cartographer/mapping/2d/submap_2d.h"
@ -81,26 +81,28 @@ class PoseGraph2D : public PoseGraph {
std::shared_ptr<const TrajectoryNode::Data> constant_data, std::shared_ptr<const TrajectoryNode::Data> constant_data,
int trajectory_id, int trajectory_id,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps)
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
void AddOdometryData(int trajectory_id, void AddOdometryData(int trajectory_id,
const sensor::OdometryData& odometry_data) override const sensor::OdometryData& odometry_data) override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
void AddFixedFramePoseData( void AddFixedFramePoseData(
int trajectory_id, int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) override const sensor::FixedFramePoseData& fixed_frame_pose_data) override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
void AddLandmarkData(int trajectory_id, void AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data) override const sensor::LandmarkData& landmark_data) override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
void DeleteTrajectory(int trajectory_id) override; void DeleteTrajectory(int trajectory_id) override;
void FinishTrajectory(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; 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, void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
const proto::Submap& submap) override; const proto::Submap& submap) override;
void AddNodeFromProto(const transform::Rigid3d& global_pose, void AddNodeFromProto(const transform::Rigid3d& global_pose,
@ -114,56 +116,58 @@ class PoseGraph2D : public PoseGraph {
void RunFinalOptimization() override; void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() const override; std::vector<std::vector<int>> GetConnectedTrajectories() const override;
PoseGraphInterface::SubmapData GetSubmapData(const SubmapId& submap_id) const PoseGraphInterface::SubmapData GetSubmapData(const SubmapId& submap_id) const
EXCLUDES(mutex_) override; LOCKS_EXCLUDED(mutex_) override;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() const MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() const
EXCLUDES(mutex_) override; LOCKS_EXCLUDED(mutex_) override;
MapById<SubmapId, SubmapPose> GetAllSubmapPoses() const MapById<SubmapId, SubmapPose> GetAllSubmapPoses() const
EXCLUDES(mutex_) override; LOCKS_EXCLUDED(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) const transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) const
EXCLUDES(mutex_) override; LOCKS_EXCLUDED(mutex_) override;
MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
std::map<int, TrajectoryState> GetTrajectoryStates() const override std::map<int, TrajectoryState> GetTrajectoryStates() const override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
void SetLandmarkPose(const std::string& landmark_id, void SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) override const transform::Rigid3d& global_pose) override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
sensor::MapByTime<sensor::ImuData> GetImuData() const override sensor::MapByTime<sensor::ImuData> GetImuData() const override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
sensor::MapByTime<sensor::OdometryData> GetOdometryData() const override sensor::MapByTime<sensor::OdometryData> GetOdometryData() const override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData() sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData()
const override EXCLUDES(mutex_); const override LOCKS_EXCLUDED(mutex_);
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode> std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
GetLandmarkNodes() const override EXCLUDES(mutex_); GetLandmarkNodes() const override LOCKS_EXCLUDED(mutex_);
std::map<int, TrajectoryData> GetTrajectoryData() const override std::map<int, TrajectoryData> GetTrajectoryData() const override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
std::vector<Constraint> constraints() const override EXCLUDES(mutex_); std::vector<Constraint> constraints() const override LOCKS_EXCLUDED(mutex_);
void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id, void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,
const transform::Rigid3d& pose, const transform::Rigid3d& pose,
const common::Time time) override const common::Time time) override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
void SetGlobalSlamOptimizationCallback( void SetGlobalSlamOptimizationCallback(
PoseGraphInterface::GlobalSlamOptimizationCallback callback) override; PoseGraphInterface::GlobalSlamOptimizationCallback callback) override;
transform::Rigid3d GetInterpolatedGlobalTrajectoryPose( 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); static void RegisterMetrics(metrics::FamilyFactory* family_factory);
private: private:
MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock() MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock()
const REQUIRES(mutex_); const EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Handles a new work item. // Handles a new work item.
void AddWorkItem(const std::function<WorkItem::Result()>& work_item) void AddWorkItem(const std::function<WorkItem::Result()>& 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. // 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 // Appends the new node and submap (if needed) to the internal data
// structures. // structures.
@ -171,68 +175,71 @@ class PoseGraph2D : public PoseGraph {
std::shared_ptr<const TrajectoryNode::Data> constant_data, std::shared_ptr<const TrajectoryNode::Data> constant_data,
int trajectory_id, int trajectory_id,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps, const std::vector<std::shared_ptr<const Submap2D>>& 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 // Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
std::vector<SubmapId> InitializeGlobalSubmapPoses( std::vector<SubmapId> InitializeGlobalSubmapPoses(
int trajectory_id, const common::Time time, int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps)
REQUIRES(mutex_); EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Adds constraints for a node, and starts scan matching in the background. // Adds constraints for a node, and starts scan matching in the background.
WorkItem::Result ComputeConstraintsForNode( WorkItem::Result ComputeConstraintsForNode(
const NodeId& node_id, const NodeId& node_id,
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps, std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
bool newly_finished_submap) EXCLUDES(mutex_); bool newly_finished_submap) LOCKS_EXCLUDED(mutex_);
// Computes constraints for a node and submap pair. // Computes constraints for a node and submap pair.
void ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id) 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 // Deletes trajectories waiting for deletion. Must not be called during
// constraint search. // constraint search.
void DeleteTrajectoriesIfNeeded() REQUIRES(mutex_); void DeleteTrajectoriesIfNeeded() EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Runs the optimization, executes the trimmers and processes the work queue. // Runs the optimization, executes the trimmers and processes the work queue.
void HandleWorkQueue(const constraints::ConstraintBuilder2D::Result& result) 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 // Process pending tasks in the work queue on the calling thread, until the
// queue is either empty or an optimization is required. // 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 // Waits until we caught up (i.e. nothing is waiting to be scheduled), and
// all computations have finished. // 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 // Runs the optimization. Callers have to make sure, that there is only one
// optimization being run at a time. // 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 // Computes the local to global map frame transform based on the given
// 'global_submap_poses'. // 'global_submap_poses'.
transform::Rigid3d ComputeLocalToGlobalTransform( transform::Rigid3d ComputeLocalToGlobalTransform(
const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses, const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses,
int trajectory_id) const REQUIRES(mutex_); int trajectory_id) const EXCLUSIVE_LOCKS_REQUIRED(mutex_);
SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) const SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) const
REQUIRES(mutex_); EXCLUSIVE_LOCKS_REQUIRED(mutex_);
common::Time GetLatestNodeTime(const NodeId& node_id, common::Time GetLatestNodeTime(const NodeId& node_id,
const SubmapId& submap_id) const const SubmapId& submap_id) const
REQUIRES(mutex_); EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Updates the trajectory connectivity structure with a new constraint. // Updates the trajectory connectivity structure with a new constraint.
void UpdateTrajectoryConnectivity(const Constraint& constraint) void UpdateTrajectoryConnectivity(const Constraint& constraint)
REQUIRES(mutex_); EXCLUSIVE_LOCKS_REQUIRED(mutex_);
const proto::PoseGraphOptions options_; const proto::PoseGraphOptions options_;
GlobalSlamOptimizationCallback global_slam_optimization_callback_; GlobalSlamOptimizationCallback global_slam_optimization_callback_;
mutable common::Mutex mutex_; mutable absl::Mutex mutex_;
common::Mutex work_queue_mutex_; absl::Mutex work_queue_mutex_;
// If it exists, further work items must be added to this queue, and will be // If it exists, further work items must be added to this queue, and will be
// considered later. // considered later.
@ -269,16 +276,17 @@ class PoseGraph2D : public PoseGraph {
int num_submaps(int trajectory_id) const override; int num_submaps(int trajectory_id) const override;
std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override; std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
MapById<SubmapId, SubmapData> GetOptimizedSubmapData() const override MapById<SubmapId, SubmapData> GetOptimizedSubmapData() const override
REQUIRES(parent_->mutex_); EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
REQUIRES(parent_->mutex_); EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
const std::vector<Constraint>& GetConstraints() const override const std::vector<Constraint>& GetConstraints() const override
REQUIRES(parent_->mutex_); EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
void TrimSubmap(const SubmapId& submap_id) void TrimSubmap(const SubmapId& submap_id)
REQUIRES(parent_->mutex_) override; EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_) override;
bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_); bool IsFinished(int trajectory_id) const override
EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
void SetTrajectoryState(int trajectory_id, TrajectoryState state) override void SetTrajectoryState(int trajectory_id, TrajectoryState state) override
REQUIRES(parent_->mutex_); EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
private: private:
PoseGraph2D* const parent_; PoseGraph2D* const parent_;

152
cartographer/mapping/internal/3d/pose_graph_3d.cc
View File

@ -52,7 +52,7 @@ PoseGraph3D::PoseGraph3D(
PoseGraph3D::~PoseGraph3D() { PoseGraph3D::~PoseGraph3D() {
WaitForAllComputations(); WaitForAllComputations();
common::MutexLocker locker(&work_queue_mutex_); absl::MutexLock locker(&work_queue_mutex_);
CHECK(work_queue_ == nullptr); CHECK(work_queue_ == nullptr);
} }
@ -110,7 +110,7 @@ NodeId PoseGraph3D::AppendNode(
const int trajectory_id, const int trajectory_id,
const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps, const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps,
const transform::Rigid3d& optimized_pose) { const transform::Rigid3d& optimized_pose) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
AddTrajectoryIfNeeded(trajectory_id); AddTrajectoryIfNeeded(trajectory_id);
if (!CanAddWorkItemModifying(trajectory_id)) { if (!CanAddWorkItemModifying(trajectory_id)) {
LOG(WARNING) << "AddNode was called for finished or deleted trajectory."; 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 // We have to check this here, because it might have changed by the time we
// execute the lambda. // execute the lambda.
const bool newly_finished_submap = insertion_submaps.front()->finished(); const bool newly_finished_submap = insertion_submaps.front()->finished();
AddWorkItem([=]() EXCLUDES(mutex_) { AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
return ComputeConstraintsForNode(node_id, insertion_submaps, return ComputeConstraintsForNode(node_id, insertion_submaps,
newly_finished_submap); newly_finished_submap);
}); });
@ -153,7 +153,7 @@ NodeId PoseGraph3D::AddNode(
void PoseGraph3D::AddWorkItem( void PoseGraph3D::AddWorkItem(
const std::function<WorkItem::Result()>& work_item) { const std::function<WorkItem::Result()>& work_item) {
common::MutexLocker locker(&work_queue_mutex_); absl::MutexLock locker(&work_queue_mutex_);
if (work_queue_ == nullptr) { if (work_queue_ == nullptr) {
work_queue_ = absl::make_unique<WorkQueue>(); work_queue_ = absl::make_unique<WorkQueue>();
auto task = absl::make_unique<common::Task>(); auto task = absl::make_unique<common::Task>();
@ -187,8 +187,8 @@ void PoseGraph3D::AddTrajectoryIfNeeded(const int trajectory_id) {
void PoseGraph3D::AddImuData(const int trajectory_id, void PoseGraph3D::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) { const sensor::ImuData& imu_data) {
AddWorkItem([=]() EXCLUDES(mutex_) { AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) { if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddImuData(trajectory_id, imu_data); 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, void PoseGraph3D::AddOdometryData(const int trajectory_id,
const sensor::OdometryData& odometry_data) { const sensor::OdometryData& odometry_data) {
AddWorkItem([=]() EXCLUDES(mutex_) { AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) { if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddOdometryData(trajectory_id, odometry_data); optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
} }
@ -210,8 +210,8 @@ void PoseGraph3D::AddOdometryData(const int trajectory_id,
void PoseGraph3D::AddFixedFramePoseData( void PoseGraph3D::AddFixedFramePoseData(
const int trajectory_id, const int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) { const sensor::FixedFramePoseData& fixed_frame_pose_data) {
AddWorkItem([=]() EXCLUDES(mutex_) { AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) { if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddFixedFramePoseData(trajectory_id, optimization_problem_->AddFixedFramePoseData(trajectory_id,
fixed_frame_pose_data); fixed_frame_pose_data);
@ -222,8 +222,8 @@ void PoseGraph3D::AddFixedFramePoseData(
void PoseGraph3D::AddLandmarkData(int trajectory_id, void PoseGraph3D::AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data) { const sensor::LandmarkData& landmark_data) {
AddWorkItem([=]() EXCLUDES(mutex_) { AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) { if (CanAddWorkItemModifying(trajectory_id)) {
for (const auto& observation : landmark_data.landmark_observations) { for (const auto& observation : landmark_data.landmark_observations) {
data_.landmark_nodes[observation.id].landmark_observations.emplace_back( data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
@ -254,7 +254,7 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
const Submap3D* submap; const Submap3D* submap;
std::vector<TrajectoryNode> submap_nodes; std::vector<TrajectoryNode> submap_nodes;
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished); CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
if (!data_.submap_data.at(submap_id).submap->finished()) { if (!data_.submap_data.at(submap_id).submap->finished()) {
// Uplink server only receives grids when they are finished, so skip // Uplink server only receives grids when they are finished, so skip
@ -316,7 +316,7 @@ WorkItem::Result PoseGraph3D::ComputeConstraintsForNode(
std::vector<SubmapId> finished_submap_ids; std::vector<SubmapId> finished_submap_ids;
std::set<NodeId> newly_finished_submap_node_ids; std::set<NodeId> newly_finished_submap_node_ids;
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
const auto& constant_data = const auto& constant_data =
data_.trajectory_nodes.at(node_id).constant_data; data_.trajectory_nodes.at(node_id).constant_data;
submap_ids = InitializeGlobalSubmapPoses( submap_ids = InitializeGlobalSubmapPoses(
@ -432,7 +432,7 @@ void PoseGraph3D::DeleteTrajectoriesIfNeeded() {
void PoseGraph3D::HandleWorkQueue( void PoseGraph3D::HandleWorkQueue(
const constraints::ConstraintBuilder3D::Result& result) { const constraints::ConstraintBuilder3D::Result& result) {
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
data_.constraints.insert(data_.constraints.end(), result.begin(), data_.constraints.insert(data_.constraints.end(), result.begin(),
result.end()); result.end());
} }
@ -442,7 +442,7 @@ void PoseGraph3D::HandleWorkQueue(
std::map<int, NodeId> trajectory_id_to_last_optimized_node_id; std::map<int, NodeId> trajectory_id_to_last_optimized_node_id;
std::map<int, SubmapId> trajectory_id_to_last_optimized_submap_id; std::map<int, SubmapId> 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& submap_data = optimization_problem_->submap_data();
const auto& node_data = optimization_problem_->node_data(); const auto& node_data = optimization_problem_->node_data();
for (const int trajectory_id : node_data.trajectory_ids()) { 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) { for (const Constraint& constraint : result) {
UpdateTrajectoryConnectivity(constraint); UpdateTrajectoryConnectivity(constraint);
} }
@ -492,7 +492,7 @@ void PoseGraph3D::DrainWorkQueue() {
while (process_work_queue) { while (process_work_queue) {
std::function<WorkItem::Result()> work_item; std::function<WorkItem::Result()> work_item;
{ {
common::MutexLocker locker(&work_queue_mutex_); absl::MutexLock locker(&work_queue_mutex_);
if (work_queue_->empty()) { if (work_queue_->empty()) {
work_queue_.reset(); work_queue_.reset();
return; return;
@ -514,7 +514,7 @@ void PoseGraph3D::DrainWorkQueue() {
void PoseGraph3D::WaitForAllComputations() { void PoseGraph3D::WaitForAllComputations() {
int num_trajectory_nodes; int num_trajectory_nodes;
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
num_trajectory_nodes = data_.num_trajectory_nodes; 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 // First wait for the work queue to drain so that it's safe to schedule
// a WhenDone() callback. // a WhenDone() callback.
{ {
common::MutexLocker locker(&work_queue_mutex_); const auto predicate = [this]()
while (!locker.AwaitWithTimeout( EXCLUSIVE_LOCKS_REQUIRED(work_queue_mutex_) {
[this]() REQUIRES(work_queue_mutex_) { return work_queue_ == nullptr; }, return work_queue_ == nullptr;
common::FromSeconds(1.))) { };
absl::MutexLock locker(&work_queue_mutex_);
while (!work_queue_mutex_.AwaitWithTimeout(
absl::Condition(&predicate),
absl::FromChrono(common::FromSeconds(1.)))) {
report_progress(); report_progress();
} }
} }
// Now wait for any pending constraint computations to finish. // Now wait for any pending constraint computations to finish.
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
bool notification = false; bool notification = false;
constraint_builder_.WhenDone( constraint_builder_.WhenDone(
[this, [this,
&notification](const constraints::ConstraintBuilder3D::Result& result) &notification](const constraints::ConstraintBuilder3D::Result& result)
EXCLUDES(mutex_) { LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
data_.constraints.insert(data_.constraints.end(), result.begin(), data_.constraints.insert(data_.constraints.end(), result.begin(),
result.end()); result.end());
notification = true; notification = true;
}); });
while (!locker.AwaitWithTimeout([&notification]() const auto predicate = [&notification]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
REQUIRES(mutex_) { return notification; }, return notification;
common::FromSeconds(1.))) { };
while (!mutex_.AwaitWithTimeout(absl::Condition(&predicate),
absl::FromChrono(common::FromSeconds(1.)))) {
report_progress(); report_progress();
} }
CHECK_EQ(constraint_builder_.GetNumFinishedNodes(), num_trajectory_nodes); CHECK_EQ(constraint_builder_.GetNumFinishedNodes(), num_trajectory_nodes);
@ -570,7 +576,7 @@ void PoseGraph3D::WaitForAllComputations() {
void PoseGraph3D::DeleteTrajectory(const int trajectory_id) { void PoseGraph3D::DeleteTrajectory(const int trajectory_id) {
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
auto it = data_.trajectories_state.find(trajectory_id); auto it = data_.trajectories_state.find(trajectory_id);
if (it == data_.trajectories_state.end()) { if (it == data_.trajectories_state.end()) {
LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: " 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 = it->second.deletion_state =
InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION; InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION;
} }
AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) { AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(data_.trajectories_state.at(trajectory_id).state != CHECK(data_.trajectories_state.at(trajectory_id).state !=
TrajectoryState::ACTIVE); TrajectoryState::ACTIVE);
CHECK(data_.trajectories_state.at(trajectory_id).state != 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) { void PoseGraph3D::FinishTrajectory(const int trajectory_id) {
AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) { AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(!IsTrajectoryFinished(trajectory_id)); CHECK(!IsTrajectoryFinished(trajectory_id));
data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED; 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) { void PoseGraph3D::FreezeTrajectory(const int trajectory_id) {
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
data_.trajectory_connectivity_state.Add(trajectory_id); data_.trajectory_connectivity_state.Add(trajectory_id);
} }
AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) { AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(!IsTrajectoryFrozen(trajectory_id)); CHECK(!IsTrajectoryFrozen(trajectory_id));
data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN; data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN;
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
@ -644,7 +650,7 @@ void PoseGraph3D::AddSubmapFromProto(
std::make_shared<const Submap3D>(submap.submap_3d()); std::make_shared<const Submap3D>(submap.submap_3d());
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
AddTrajectoryIfNeeded(submap_id.trajectory_id); AddTrajectoryIfNeeded(submap_id.trajectory_id);
if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return; if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return;
data_.submap_data.Insert(submap_id, InternalSubmapData()); data_.submap_data.Insert(submap_id, InternalSubmapData());
@ -653,8 +659,8 @@ void PoseGraph3D::AddSubmapFromProto(
data_.global_submap_poses_3d.Insert( data_.global_submap_poses_3d.Insert(
submap_id, optimization::SubmapSpec3D{global_submap_pose}); submap_id, optimization::SubmapSpec3D{global_submap_pose});
} }
AddWorkItem([this, submap_id, global_submap_pose]() EXCLUDES(mutex_) { AddWorkItem([this, submap_id, global_submap_pose]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
data_.submap_data.at(submap_id).state = SubmapState::kFinished; data_.submap_data.at(submap_id).state = SubmapState::kFinished;
optimization_problem_->InsertSubmap(submap_id, global_submap_pose); optimization_problem_->InsertSubmap(submap_id, global_submap_pose);
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
@ -669,15 +675,15 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose,
std::make_shared<const TrajectoryNode::Data>(FromProto(node.node_data())); std::make_shared<const TrajectoryNode::Data>(FromProto(node.node_data()));
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
AddTrajectoryIfNeeded(node_id.trajectory_id); AddTrajectoryIfNeeded(node_id.trajectory_id);
if (!CanAddWorkItemModifying(node_id.trajectory_id)) return; if (!CanAddWorkItemModifying(node_id.trajectory_id)) return;
data_.trajectory_nodes.Insert(node_id, data_.trajectory_nodes.Insert(node_id,
TrajectoryNode{constant_data, global_pose}); TrajectoryNode{constant_data, global_pose});
} }
AddWorkItem([this, node_id, global_pose]() EXCLUDES(mutex_) { AddWorkItem([this, node_id, global_pose]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
const auto& constant_data = const auto& constant_data =
data_.trajectory_nodes.at(node_id).constant_data; data_.trajectory_nodes.at(node_id).constant_data;
optimization_problem_->InsertTrajectoryNode( optimization_problem_->InsertTrajectoryNode(
@ -701,8 +707,8 @@ void PoseGraph3D::SetTrajectoryDataFromProto(
} }
const int trajectory_id = data.trajectory_id(); const int trajectory_id = data.trajectory_id();
AddWorkItem([this, trajectory_id, trajectory_data]() EXCLUDES(mutex_) { AddWorkItem([this, trajectory_id, trajectory_data]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) { if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->SetTrajectoryData(trajectory_id, trajectory_data); optimization_problem_->SetTrajectoryData(trajectory_id, trajectory_data);
} }
@ -712,8 +718,8 @@ void PoseGraph3D::SetTrajectoryDataFromProto(
void PoseGraph3D::AddNodeToSubmap(const NodeId& node_id, void PoseGraph3D::AddNodeToSubmap(const NodeId& node_id,
const SubmapId& submap_id) { const SubmapId& submap_id) {
AddWorkItem([this, node_id, submap_id]() EXCLUDES(mutex_) { AddWorkItem([this, node_id, submap_id]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(submap_id.trajectory_id)) { if (CanAddWorkItemModifying(submap_id.trajectory_id)) {
data_.submap_data.at(submap_id).node_ids.insert(node_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( void PoseGraph3D::AddSerializedConstraints(
const std::vector<Constraint>& constraints) { const std::vector<Constraint>& constraints) {
AddWorkItem([this, constraints]() EXCLUDES(mutex_) { AddWorkItem([this, constraints]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
for (const auto& constraint : constraints) { for (const auto& constraint : constraints) {
CHECK(data_.trajectory_nodes.Contains(constraint.node_id)); CHECK(data_.trajectory_nodes.Contains(constraint.node_id));
CHECK(data_.submap_data.Contains(constraint.submap_id)); CHECK(data_.submap_data.Contains(constraint.submap_id));
@ -751,8 +757,8 @@ void PoseGraph3D::AddSerializedConstraints(
void PoseGraph3D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) { void PoseGraph3D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {
// C++11 does not allow us to move a unique_ptr into a lambda. // C++11 does not allow us to move a unique_ptr into a lambda.
PoseGraphTrimmer* const trimmer_ptr = trimmer.release(); PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
AddWorkItem([this, trimmer_ptr]() EXCLUDES(mutex_) { AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
trimmers_.emplace_back(trimmer_ptr); trimmers_.emplace_back(trimmer_ptr);
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
}); });
@ -760,14 +766,14 @@ void PoseGraph3D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {
void PoseGraph3D::RunFinalOptimization() { void PoseGraph3D::RunFinalOptimization() {
{ {
AddWorkItem([this]() EXCLUDES(mutex_) { AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
optimization_problem_->SetMaxNumIterations( optimization_problem_->SetMaxNumIterations(
options_.max_num_final_iterations()); options_.max_num_final_iterations());
return WorkItem::Result::kRunOptimization; return WorkItem::Result::kRunOptimization;
}); });
AddWorkItem([this]() EXCLUDES(mutex_) { AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
optimization_problem_->SetMaxNumIterations( optimization_problem_->SetMaxNumIterations(
options_.optimization_problem_options() options_.optimization_problem_options()
.ceres_solver_options() .ceres_solver_options()
@ -815,7 +821,7 @@ void PoseGraph3D::RunOptimization() {
// avoid blocking foreground processing. // avoid blocking foreground processing.
optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(), optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
data_.landmark_nodes); data_.landmark_nodes);
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
const auto& submap_data = optimization_problem_->submap_data(); const auto& submap_data = optimization_problem_->submap_data();
const auto& node_data = optimization_problem_->node_data(); const auto& node_data = optimization_problem_->node_data();
@ -884,14 +890,14 @@ bool PoseGraph3D::CanAddWorkItemModifying(int trajectory_id) {
} }
MapById<NodeId, TrajectoryNode> PoseGraph3D::GetTrajectoryNodes() const { MapById<NodeId, TrajectoryNode> PoseGraph3D::GetTrajectoryNodes() const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return data_.trajectory_nodes; return data_.trajectory_nodes;
} }
MapById<NodeId, TrajectoryNodePose> PoseGraph3D::GetTrajectoryNodePoses() MapById<NodeId, TrajectoryNodePose> PoseGraph3D::GetTrajectoryNodePoses()
const { const {
MapById<NodeId, TrajectoryNodePose> node_poses; MapById<NodeId, TrajectoryNodePose> node_poses;
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
for (const auto& node_id_data : data_.trajectory_nodes) { for (const auto& node_id_data : data_.trajectory_nodes) {
absl::optional<TrajectoryNodePose::ConstantPoseData> constant_pose_data; absl::optional<TrajectoryNodePose::ConstantPoseData> constant_pose_data;
if (node_id_data.data.constant_data != nullptr) { if (node_id_data.data.constant_data != nullptr) {
@ -909,7 +915,7 @@ MapById<NodeId, TrajectoryNodePose> PoseGraph3D::GetTrajectoryNodePoses()
std::map<int, PoseGraphInterface::TrajectoryState> std::map<int, PoseGraphInterface::TrajectoryState>
PoseGraph3D::GetTrajectoryStates() const { PoseGraph3D::GetTrajectoryStates() const {
std::map<int, PoseGraphInterface::TrajectoryState> trajectories_state; std::map<int, PoseGraphInterface::TrajectoryState> trajectories_state;
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
for (const auto& it : data_.trajectories_state) { for (const auto& it : data_.trajectories_state) {
trajectories_state[it.first] = it.second.state; trajectories_state[it.first] = it.second.state;
} }
@ -919,7 +925,7 @@ PoseGraph3D::GetTrajectoryStates() const {
std::map<std::string, transform::Rigid3d> PoseGraph3D::GetLandmarkPoses() std::map<std::string, transform::Rigid3d> PoseGraph3D::GetLandmarkPoses()
const { const {
std::map<std::string, transform::Rigid3d> landmark_poses; std::map<std::string, transform::Rigid3d> landmark_poses;
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
for (const auto& landmark : data_.landmark_nodes) { for (const auto& landmark : data_.landmark_nodes) {
// Landmark without value has not been optimized yet. // Landmark without value has not been optimized yet.
if (!landmark.second.global_landmark_pose.has_value()) continue; if (!landmark.second.global_landmark_pose.has_value()) continue;
@ -931,43 +937,43 @@ std::map<std::string, transform::Rigid3d> PoseGraph3D::GetLandmarkPoses()
void PoseGraph3D::SetLandmarkPose(const std::string& landmark_id, void PoseGraph3D::SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) { const transform::Rigid3d& global_pose) {
AddWorkItem([=]() EXCLUDES(mutex_) { AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose; data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose;
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
}); });
} }
sensor::MapByTime<sensor::ImuData> PoseGraph3D::GetImuData() const { sensor::MapByTime<sensor::ImuData> PoseGraph3D::GetImuData() const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return optimization_problem_->imu_data(); return optimization_problem_->imu_data();
} }
sensor::MapByTime<sensor::OdometryData> PoseGraph3D::GetOdometryData() const { sensor::MapByTime<sensor::OdometryData> PoseGraph3D::GetOdometryData() const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return optimization_problem_->odometry_data(); return optimization_problem_->odometry_data();
} }
sensor::MapByTime<sensor::FixedFramePoseData> sensor::MapByTime<sensor::FixedFramePoseData>
PoseGraph3D::GetFixedFramePoseData() const { PoseGraph3D::GetFixedFramePoseData() const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return optimization_problem_->fixed_frame_pose_data(); return optimization_problem_->fixed_frame_pose_data();
} }
std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode> std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
PoseGraph3D::GetLandmarkNodes() const { PoseGraph3D::GetLandmarkNodes() const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return data_.landmark_nodes; return data_.landmark_nodes;
} }
std::map<int, PoseGraphInterface::TrajectoryData> std::map<int, PoseGraphInterface::TrajectoryData>
PoseGraph3D::GetTrajectoryData() const { PoseGraph3D::GetTrajectoryData() const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return optimization_problem_->trajectory_data(); return optimization_problem_->trajectory_data();
} }
std::vector<PoseGraphInterface::Constraint> PoseGraph3D::constraints() const { std::vector<PoseGraphInterface::Constraint> PoseGraph3D::constraints() const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return data_.constraints; return data_.constraints;
} }
@ -975,7 +981,7 @@ void PoseGraph3D::SetInitialTrajectoryPose(const int from_trajectory_id,
const int to_trajectory_id, const int to_trajectory_id,
const transform::Rigid3d& pose, const transform::Rigid3d& pose,
const common::Time time) { const common::Time time) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
data_.initial_trajectory_poses[from_trajectory_id] = data_.initial_trajectory_poses[from_trajectory_id] =
InitialTrajectoryPose{to_trajectory_id, pose, time}; InitialTrajectoryPose{to_trajectory_id, pose, time};
} }
@ -1003,7 +1009,7 @@ transform::Rigid3d PoseGraph3D::GetInterpolatedGlobalTrajectoryPose(
transform::Rigid3d PoseGraph3D::GetLocalToGlobalTransform( transform::Rigid3d PoseGraph3D::GetLocalToGlobalTransform(
const int trajectory_id) const { const int trajectory_id) const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return ComputeLocalToGlobalTransform(data_.global_submap_poses_3d, return ComputeLocalToGlobalTransform(data_.global_submap_poses_3d,
trajectory_id); trajectory_id);
} }
@ -1014,19 +1020,19 @@ std::vector<std::vector<int>> PoseGraph3D::GetConnectedTrajectories() const {
PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapData( PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapData(
const SubmapId& submap_id) const { const SubmapId& submap_id) const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return GetSubmapDataUnderLock(submap_id); return GetSubmapDataUnderLock(submap_id);
} }
MapById<SubmapId, PoseGraphInterface::SubmapData> MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph3D::GetAllSubmapData() const { PoseGraph3D::GetAllSubmapData() const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return GetSubmapDataUnderLock(); return GetSubmapDataUnderLock();
} }
MapById<SubmapId, PoseGraphInterface::SubmapPose> MapById<SubmapId, PoseGraphInterface::SubmapPose>
PoseGraph3D::GetAllSubmapPoses() const { PoseGraph3D::GetAllSubmapPoses() const {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
MapById<SubmapId, SubmapPose> submap_poses; MapById<SubmapId, SubmapPose> submap_poses;
for (const auto& submap_id_data : data_.submap_data) { for (const auto& submap_id_data : data_.submap_data) {
auto submap_data = GetSubmapDataUnderLock(submap_id_data.id); auto submap_data = GetSubmapDataUnderLock(submap_id_data.id);

109
cartographer/mapping/internal/3d/pose_graph_3d.h
View File

@ -28,8 +28,8 @@
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "absl/synchronization/mutex.h"
#include "cartographer/common/fixed_ratio_sampler.h" #include "cartographer/common/fixed_ratio_sampler.h"
#include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.h" #include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/3d/submap_3d.h" #include "cartographer/mapping/3d/submap_3d.h"
@ -79,26 +79,28 @@ class PoseGraph3D : public PoseGraph {
std::shared_ptr<const TrajectoryNode::Data> constant_data, std::shared_ptr<const TrajectoryNode::Data> constant_data,
int trajectory_id, int trajectory_id,
const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps) const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps)
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
void AddOdometryData(int trajectory_id, void AddOdometryData(int trajectory_id,
const sensor::OdometryData& odometry_data) override const sensor::OdometryData& odometry_data) override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
void AddFixedFramePoseData( void AddFixedFramePoseData(
int trajectory_id, int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) override const sensor::FixedFramePoseData& fixed_frame_pose_data) override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
void AddLandmarkData(int trajectory_id, void AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data) override const sensor::LandmarkData& landmark_data) override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
void DeleteTrajectory(int trajectory_id) override; void DeleteTrajectory(int trajectory_id) override;
void FinishTrajectory(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; 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, void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
const proto::Submap& submap) override; const proto::Submap& submap) override;
void AddNodeFromProto(const transform::Rigid3d& global_pose, void AddNodeFromProto(const transform::Rigid3d& global_pose,
@ -112,128 +114,134 @@ class PoseGraph3D : public PoseGraph {
void RunFinalOptimization() override; void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() const override; std::vector<std::vector<int>> GetConnectedTrajectories() const override;
PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id) const PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id) const
EXCLUDES(mutex_) override; LOCKS_EXCLUDED(mutex_) override;
MapById<SubmapId, SubmapData> GetAllSubmapData() const MapById<SubmapId, SubmapData> GetAllSubmapData() const
EXCLUDES(mutex_) override; LOCKS_EXCLUDED(mutex_) override;
MapById<SubmapId, SubmapPose> GetAllSubmapPoses() const MapById<SubmapId, SubmapPose> GetAllSubmapPoses() const
EXCLUDES(mutex_) override; LOCKS_EXCLUDED(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) const transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) const
EXCLUDES(mutex_) override; LOCKS_EXCLUDED(mutex_) override;
MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
std::map<int, TrajectoryState> GetTrajectoryStates() const override std::map<int, TrajectoryState> GetTrajectoryStates() const override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
void SetLandmarkPose(const std::string& landmark_id, void SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) override const transform::Rigid3d& global_pose) override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
sensor::MapByTime<sensor::ImuData> GetImuData() const override sensor::MapByTime<sensor::ImuData> GetImuData() const override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
sensor::MapByTime<sensor::OdometryData> GetOdometryData() const override sensor::MapByTime<sensor::OdometryData> GetOdometryData() const override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData() sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData()
const override EXCLUDES(mutex_); const override LOCKS_EXCLUDED(mutex_);
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode> std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
GetLandmarkNodes() const override EXCLUDES(mutex_); GetLandmarkNodes() const override LOCKS_EXCLUDED(mutex_);
std::map<int, TrajectoryData> GetTrajectoryData() const override; std::map<int, TrajectoryData> GetTrajectoryData() const override;
std::vector<Constraint> constraints() const override EXCLUDES(mutex_); std::vector<Constraint> constraints() const override LOCKS_EXCLUDED(mutex_);
void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id, void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,
const transform::Rigid3d& pose, const transform::Rigid3d& pose,
const common::Time time) override const common::Time time) override
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
void SetGlobalSlamOptimizationCallback( void SetGlobalSlamOptimizationCallback(
PoseGraphInterface::GlobalSlamOptimizationCallback callback) override; PoseGraphInterface::GlobalSlamOptimizationCallback callback) override;
transform::Rigid3d GetInterpolatedGlobalTrajectoryPose( 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); static void RegisterMetrics(metrics::FamilyFactory* family_factory);
protected: protected:
// Waits until we caught up (i.e. nothing is waiting to be scheduled), and // Waits until we caught up (i.e. nothing is waiting to be scheduled), and
// all computations have finished. // all computations have finished.
void WaitForAllComputations() EXCLUDES(mutex_) EXCLUDES(work_queue_mutex_); void WaitForAllComputations() LOCKS_EXCLUDED(mutex_)
LOCKS_EXCLUDED(work_queue_mutex_);
private: private:
MapById<SubmapId, SubmapData> GetSubmapDataUnderLock() const REQUIRES(mutex_); MapById<SubmapId, SubmapData> GetSubmapDataUnderLock() const
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Handles a new work item. // Handles a new work item.
void AddWorkItem(const std::function<WorkItem::Result()>& work_item) void AddWorkItem(const std::function<WorkItem::Result()>& 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. // 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. // Appends the new node and submap (if needed) to the internal data stuctures.
NodeId AppendNode( NodeId AppendNode(
std::shared_ptr<const TrajectoryNode::Data> constant_data, std::shared_ptr<const TrajectoryNode::Data> constant_data,
int trajectory_id, int trajectory_id,
const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps, const std::vector<std::shared_ptr<const Submap3D>>& 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 // Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
std::vector<SubmapId> InitializeGlobalSubmapPoses( std::vector<SubmapId> InitializeGlobalSubmapPoses(
int trajectory_id, const common::Time time, int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps) const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps)
REQUIRES(mutex_); EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Adds constraints for a node, and starts scan matching in the background. // Adds constraints for a node, and starts scan matching in the background.
WorkItem::Result ComputeConstraintsForNode( WorkItem::Result ComputeConstraintsForNode(
const NodeId& node_id, const NodeId& node_id,
std::vector<std::shared_ptr<const Submap3D>> insertion_submaps, std::vector<std::shared_ptr<const Submap3D>> insertion_submaps,
bool newly_finished_submap) EXCLUDES(mutex_); bool newly_finished_submap) LOCKS_EXCLUDED(mutex_);
// Computes constraints for a node and submap pair. // Computes constraints for a node and submap pair.
void ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id) 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 // Deletes trajectories waiting for deletion. Must not be called during
// constraint search. // constraint search.
void DeleteTrajectoriesIfNeeded() REQUIRES(mutex_); void DeleteTrajectoriesIfNeeded() EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Runs the optimization, executes the trimmers and processes the work queue. // Runs the optimization, executes the trimmers and processes the work queue.
void HandleWorkQueue(const constraints::ConstraintBuilder3D::Result& result) 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 // Process pending tasks in the work queue on the calling thread, until the
// queue is either empty or an optimization is required. // 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 // Runs the optimization. Callers have to make sure, that there is only one
// optimization being run at a time. // 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 // Computes the local to global map frame transform based on the given
// 'global_submap_poses'. // 'global_submap_poses'.
transform::Rigid3d ComputeLocalToGlobalTransform( transform::Rigid3d ComputeLocalToGlobalTransform(
const MapById<SubmapId, optimization::SubmapSpec3D>& global_submap_poses, const MapById<SubmapId, optimization::SubmapSpec3D>& 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 PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) const
REQUIRES(mutex_); EXCLUSIVE_LOCKS_REQUIRED(mutex_);
common::Time GetLatestNodeTime(const NodeId& node_id, common::Time GetLatestNodeTime(const NodeId& node_id,
const SubmapId& submap_id) const const SubmapId& submap_id) const
REQUIRES(mutex_); EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Logs histograms for the translational and rotational residual of node // Logs histograms for the translational and rotational residual of node
// poses. // poses.
void LogResidualHistograms() const REQUIRES(mutex_); void LogResidualHistograms() const EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Updates the trajectory connectivity structure with a new constraint. // Updates the trajectory connectivity structure with a new constraint.
void UpdateTrajectoryConnectivity(const Constraint& constraint) void UpdateTrajectoryConnectivity(const Constraint& constraint)
REQUIRES(mutex_); EXCLUSIVE_LOCKS_REQUIRED(mutex_);
const proto::PoseGraphOptions options_; const proto::PoseGraphOptions options_;
GlobalSlamOptimizationCallback global_slam_optimization_callback_; GlobalSlamOptimizationCallback global_slam_optimization_callback_;
mutable common::Mutex mutex_; mutable absl::Mutex mutex_;
common::Mutex work_queue_mutex_; absl::Mutex work_queue_mutex_;
// If it exists, further work items must be added to this queue, and will be // If it exists, further work items must be added to this queue, and will be
// considered later. // considered later.
@ -268,17 +276,18 @@ class PoseGraph3D : public PoseGraph {
int num_submaps(int trajectory_id) const override; int num_submaps(int trajectory_id) const override;
std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override; std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
MapById<SubmapId, SubmapData> GetOptimizedSubmapData() const override MapById<SubmapId, SubmapData> GetOptimizedSubmapData() const override
REQUIRES(parent_->mutex_); EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
REQUIRES(parent_->mutex_); EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
const std::vector<Constraint>& GetConstraints() const override const std::vector<Constraint>& GetConstraints() const override
REQUIRES(parent_->mutex_); EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
void TrimSubmap(const SubmapId& submap_id) void TrimSubmap(const SubmapId& submap_id)
REQUIRES(parent_->mutex_) override; EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_) override;
bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_); bool IsFinished(int trajectory_id) const override
EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
void SetTrajectoryState(int trajectory_id, TrajectoryState state) override void SetTrajectoryState(int trajectory_id, TrajectoryState state) override
REQUIRES(parent_->mutex_); EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
private: private:
PoseGraph3D* const parent_; PoseGraph3D* const parent_;

12
cartographer/mapping/internal/connected_components.cc
View File

@ -29,13 +29,13 @@ ConnectedComponents::ConnectedComponents()
: lock_(), forest_(), connection_map_() {} : lock_(), forest_(), connection_map_() {}
void ConnectedComponents::Add(const int trajectory_id) { void ConnectedComponents::Add(const int trajectory_id) {
common::MutexLocker locker(&lock_); absl::MutexLock locker(&lock_);
forest_.emplace(trajectory_id, trajectory_id); forest_.emplace(trajectory_id, trajectory_id);
} }
void ConnectedComponents::Connect(const int trajectory_id_a, void ConnectedComponents::Connect(const int trajectory_id_a,
const int trajectory_id_b) { const int trajectory_id_b) {
common::MutexLocker locker(&lock_); absl::MutexLock locker(&lock_);
Union(trajectory_id_a, trajectory_id_b); Union(trajectory_id_a, trajectory_id_b);
auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b); auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b);
++connection_map_[sorted_pair]; ++connection_map_[sorted_pair];
@ -66,7 +66,7 @@ bool ConnectedComponents::TransitivelyConnected(const int trajectory_id_a,
return true; return true;
} }
common::MutexLocker locker(&lock_); absl::MutexLock locker(&lock_);
if (forest_.count(trajectory_id_a) == 0 || if (forest_.count(trajectory_id_a) == 0 ||
forest_.count(trajectory_id_b) == 0) { forest_.count(trajectory_id_b) == 0) {
@ -78,7 +78,7 @@ bool ConnectedComponents::TransitivelyConnected(const int trajectory_id_a,
std::vector<std::vector<int>> ConnectedComponents::Components() { std::vector<std::vector<int>> ConnectedComponents::Components() {
// Map from cluster exemplar -> growing cluster. // Map from cluster exemplar -> growing cluster.
std::unordered_map<int, std::vector<int>> map; std::unordered_map<int, std::vector<int>> map;
common::MutexLocker locker(&lock_); absl::MutexLock locker(&lock_);
for (const auto& trajectory_id_entry : forest_) { for (const auto& trajectory_id_entry : forest_) {
map[FindSet(trajectory_id_entry.first)].push_back( map[FindSet(trajectory_id_entry.first)].push_back(
trajectory_id_entry.first); trajectory_id_entry.first);
@ -93,7 +93,7 @@ std::vector<std::vector<int>> ConnectedComponents::Components() {
} }
std::vector<int> ConnectedComponents::GetComponent(const int trajectory_id) { std::vector<int> ConnectedComponents::GetComponent(const int trajectory_id) {
common::MutexLocker locker(&lock_); absl::MutexLock locker(&lock_);
const int set_id = FindSet(trajectory_id); const int set_id = FindSet(trajectory_id);
std::vector<int> trajectory_ids; std::vector<int> trajectory_ids;
for (const auto& entry : forest_) { for (const auto& entry : forest_) {
@ -106,7 +106,7 @@ std::vector<int> ConnectedComponents::GetComponent(const int trajectory_id) {
int ConnectedComponents::ConnectionCount(const int trajectory_id_a, int ConnectedComponents::ConnectionCount(const int trajectory_id_a,
const int trajectory_id_b) { const int trajectory_id_b) {
common::MutexLocker locker(&lock_); absl::MutexLock locker(&lock_);
const auto it = const auto it =
connection_map_.find(std::minmax(trajectory_id_a, trajectory_id_b)); connection_map_.find(std::minmax(trajectory_id_a, trajectory_id_b));
return it != connection_map_.end() ? it->second : 0; return it != connection_map_.end() ? it->second : 0;

22
cartographer/mapping/internal/connected_components.h
View File

@ -20,7 +20,7 @@
#include <map> #include <map>
#include <unordered_map> #include <unordered_map>
#include "cartographer/common/mutex.h" #include "absl/synchronization/mutex.h"
#include "cartographer/mapping/proto/connected_components.pb.h" #include "cartographer/mapping/proto/connected_components.pb.h"
#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/submaps.h"
@ -41,38 +41,40 @@ class ConnectedComponents {
ConnectedComponents& operator=(const ConnectedComponents&) = delete; ConnectedComponents& operator=(const ConnectedComponents&) = delete;
// Add a trajectory which is initially connected to only itself. // 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 // Connect two trajectories. If either trajectory is untracked, it will be
// tracked. This function is invariant to the order of its arguments. Repeated // tracked. This function is invariant to the order of its arguments. Repeated
// calls to Connect increment the connectivity count. // 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 // Determines if two trajectories have been (transitively) connected. If
// either trajectory is not being tracked, returns false, except when it is // 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 same trajectory, where it returns true. This function is invariant to
// the order of its arguments. // the order of its arguments.
bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b) 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 // Return the number of _direct_ connections between 'trajectory_id_a' and
// 'trajectory_id_b'. If either trajectory is not being tracked, returns 0. // 'trajectory_id_b'. If either trajectory is not being tracked, returns 0.
// This function is invariant to the order of its arguments. // 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. // The trajectory IDs, grouped by connectivity.
std::vector<std::vector<int>> Components() EXCLUDES(lock_); std::vector<std::vector<int>> Components() LOCKS_EXCLUDED(lock_);
// The list of trajectory IDs that belong to the same connected component as // The list of trajectory IDs that belong to the same connected component as
// 'trajectory_id'. // 'trajectory_id'.
std::vector<int> GetComponent(int trajectory_id) EXCLUDES(lock_); std::vector<int> GetComponent(int trajectory_id) LOCKS_EXCLUDED(lock_);
private: private:
// Find the representative and compresses the path to it. // Find the representative and compresses the path to it.
int FindSet(int trajectory_id) REQUIRES(lock_); int FindSet(int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(lock_);
void Union(int trajectory_id_a, int trajectory_id_b) REQUIRES(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 // Tracks transitive connectivity using a disjoint set forest, i.e. each
// entry points towards the representative for the given trajectory. // entry points towards the representative for the given trajectory.
std::map<int, int> forest_ GUARDED_BY(lock_); std::map<int, int> forest_ GUARDED_BY(lock_);

24
cartographer/mapping/internal/constraints/constraint_builder_2d.cc
View File

@ -64,7 +64,7 @@ ConstraintBuilder2D::ConstraintBuilder2D(
ceres_scan_matcher_(options.ceres_scan_matcher_options()) {} ceres_scan_matcher_(options.ceres_scan_matcher_options()) {}
ConstraintBuilder2D::~ConstraintBuilder2D() { ConstraintBuilder2D::~ConstraintBuilder2D() {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK_EQ(finish_node_task_->GetState(), common::Task::NEW); CHECK_EQ(finish_node_task_->GetState(), common::Task::NEW);
CHECK_EQ(when_done_task_->GetState(), common::Task::NEW); CHECK_EQ(when_done_task_->GetState(), common::Task::NEW);
CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called"; CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called";
@ -82,7 +82,7 @@ void ConstraintBuilder2D::MaybeAddConstraint(
} }
if (!sampler_.Pulse()) return; if (!sampler_.Pulse()) return;
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (when_done_) { if (when_done_) {
LOG(WARNING) LOG(WARNING)
<< "MaybeAddConstraint was called while WhenDone was scheduled."; << "MaybeAddConstraint was called while WhenDone was scheduled.";
@ -93,7 +93,7 @@ void ConstraintBuilder2D::MaybeAddConstraint(
const auto* scan_matcher = const auto* scan_matcher =
DispatchScanMatcherConstruction(submap_id, submap->grid()); DispatchScanMatcherConstruction(submap_id, submap->grid());
auto constraint_task = absl::make_unique<common::Task>(); auto constraint_task = absl::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) { constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */ ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */
constant_data, initial_relative_pose, *scan_matcher, constant_data, initial_relative_pose, *scan_matcher,
constraint); constraint);
@ -107,7 +107,7 @@ void ConstraintBuilder2D::MaybeAddConstraint(
void ConstraintBuilder2D::MaybeAddGlobalConstraint( void ConstraintBuilder2D::MaybeAddGlobalConstraint(
const SubmapId& submap_id, const Submap2D* const submap, const SubmapId& submap_id, const Submap2D* const submap,
const NodeId& node_id, const TrajectoryNode::Data* const constant_data) { const NodeId& node_id, const TrajectoryNode::Data* const constant_data) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (when_done_) { if (when_done_) {
LOG(WARNING) LOG(WARNING)
<< "MaybeAddGlobalConstraint was called while WhenDone was scheduled."; << "MaybeAddGlobalConstraint was called while WhenDone was scheduled.";
@ -118,7 +118,7 @@ void ConstraintBuilder2D::MaybeAddGlobalConstraint(
const auto* scan_matcher = const auto* scan_matcher =
DispatchScanMatcherConstruction(submap_id, submap->grid()); DispatchScanMatcherConstruction(submap_id, submap->grid());
auto constraint_task = absl::make_unique<common::Task>(); auto constraint_task = absl::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) { constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */ ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */
constant_data, transform::Rigid2d::Identity(), constant_data, transform::Rigid2d::Identity(),
*scan_matcher, constraint); *scan_matcher, constraint);
@ -130,10 +130,10 @@ void ConstraintBuilder2D::MaybeAddGlobalConstraint(
} }
void ConstraintBuilder2D::NotifyEndOfNode() { void ConstraintBuilder2D::NotifyEndOfNode() {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(finish_node_task_ != nullptr); CHECK(finish_node_task_ != nullptr);
finish_node_task_->SetWorkItem([this] { finish_node_task_->SetWorkItem([this] {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
++num_finished_nodes_; ++num_finished_nodes_;
}); });
auto finish_node_task_handle = auto finish_node_task_handle =
@ -145,7 +145,7 @@ void ConstraintBuilder2D::NotifyEndOfNode() {
void ConstraintBuilder2D::WhenDone( void ConstraintBuilder2D::WhenDone(
const std::function<void(const ConstraintBuilder2D::Result&)>& callback) { const std::function<void(const ConstraintBuilder2D::Result&)>& callback) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(when_done_ == nullptr); CHECK(when_done_ == nullptr);
// TODO(gaschler): Consider using just std::function, it can also be empty. // TODO(gaschler): Consider using just std::function, it can also be empty.
when_done_ = absl::make_unique<std::function<void(const Result&)>>(callback); when_done_ = absl::make_unique<std::function<void(const Result&)>>(callback);
@ -227,7 +227,7 @@ void ConstraintBuilder2D::ComputeConstraint(
} }
} }
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
score_histogram_.Add(score); score_histogram_.Add(score);
} }
@ -272,7 +272,7 @@ void ConstraintBuilder2D::RunWhenDoneCallback() {
Result result; Result result;
std::unique_ptr<std::function<void(const Result&)>> callback; std::unique_ptr<std::function<void(const Result&)>> callback;
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(when_done_ != nullptr); CHECK(when_done_ != nullptr);
for (const std::unique_ptr<Constraint>& constraint : constraints_) { for (const std::unique_ptr<Constraint>& constraint : constraints_) {
if (constraint == nullptr) continue; if (constraint == nullptr) continue;
@ -292,12 +292,12 @@ void ConstraintBuilder2D::RunWhenDoneCallback() {
} }
int ConstraintBuilder2D::GetNumFinishedNodes() { int ConstraintBuilder2D::GetNumFinishedNodes() {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return num_finished_nodes_; return num_finished_nodes_;
} }
void ConstraintBuilder2D::DeleteScanMatcher(const SubmapId& submap_id) { void ConstraintBuilder2D::DeleteScanMatcher(const SubmapId& submap_id) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (when_done_) { if (when_done_) {
LOG(WARNING) LOG(WARNING)
<< "DeleteScanMatcher was called while WhenDone was scheduled."; << "DeleteScanMatcher was called while WhenDone was scheduled.";

11
cartographer/mapping/internal/constraints/constraint_builder_2d.h
View File

@ -25,10 +25,10 @@
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "absl/synchronization/mutex.h"
#include "cartographer/common/fixed_ratio_sampler.h" #include "cartographer/common/fixed_ratio_sampler.h"
#include "cartographer/common/histogram.h" #include "cartographer/common/histogram.h"
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/common/mutex.h"
#include "cartographer/common/task.h" #include "cartographer/common/task.h"
#include "cartographer/common/thread_pool.h" #include "cartographer/common/thread_pool.h"
#include "cartographer/mapping/2d/submap_2d.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 // The returned 'grid' and 'fast_correlative_scan_matcher' must only be
// accessed after 'creation_task_handle' has completed. // accessed after 'creation_task_handle' has completed.
const SubmapScanMatcher* DispatchScanMatcherConstruction( 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 // Runs in a background thread and does computations for an additional
// constraint, assuming 'submap' and 'compressed_point_cloud' do not change // constraint, assuming 'submap' and 'compressed_point_cloud' do not change
@ -127,13 +128,13 @@ class ConstraintBuilder2D {
const transform::Rigid2d& initial_relative_pose, const transform::Rigid2d& initial_relative_pose,
const SubmapScanMatcher& submap_scan_matcher, const SubmapScanMatcher& submap_scan_matcher,
std::unique_ptr<Constraint>* constraint) std::unique_ptr<Constraint>* constraint)
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
void RunWhenDoneCallback() EXCLUDES(mutex_); void RunWhenDoneCallback() LOCKS_EXCLUDED(mutex_);
const constraints::proto::ConstraintBuilderOptions options_; const constraints::proto::ConstraintBuilderOptions options_;
common::ThreadPoolInterface* thread_pool_; common::ThreadPoolInterface* thread_pool_;
common::Mutex mutex_; absl::Mutex mutex_;
// 'callback' set by WhenDone(). // 'callback' set by WhenDone().
std::unique_ptr<std::function<void(const Result&)>> when_done_ std::unique_ptr<std::function<void(const Result&)>> when_done_

24
cartographer/mapping/internal/constraints/constraint_builder_3d.cc
View File

@ -66,7 +66,7 @@ ConstraintBuilder3D::ConstraintBuilder3D(
ceres_scan_matcher_(options.ceres_scan_matcher_options_3d()) {} ceres_scan_matcher_(options.ceres_scan_matcher_options_3d()) {}
ConstraintBuilder3D::~ConstraintBuilder3D() { ConstraintBuilder3D::~ConstraintBuilder3D() {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK_EQ(finish_node_task_->GetState(), common::Task::NEW); CHECK_EQ(finish_node_task_->GetState(), common::Task::NEW);
CHECK_EQ(when_done_task_->GetState(), common::Task::NEW); CHECK_EQ(when_done_task_->GetState(), common::Task::NEW);
CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called"; CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called";
@ -86,7 +86,7 @@ void ConstraintBuilder3D::MaybeAddConstraint(
} }
if (!sampler_.Pulse()) return; if (!sampler_.Pulse()) return;
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (when_done_) { if (when_done_) {
LOG(WARNING) LOG(WARNING)
<< "MaybeAddConstraint was called while WhenDone was scheduled."; << "MaybeAddConstraint was called while WhenDone was scheduled.";
@ -97,7 +97,7 @@ void ConstraintBuilder3D::MaybeAddConstraint(
const auto* scan_matcher = const auto* scan_matcher =
DispatchScanMatcherConstruction(submap_id, submap_nodes, submap); DispatchScanMatcherConstruction(submap_id, submap_nodes, submap);
auto constraint_task = absl::make_unique<common::Task>(); auto constraint_task = absl::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) { constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
ComputeConstraint(submap_id, node_id, false, /* match_full_submap */ ComputeConstraint(submap_id, node_id, false, /* match_full_submap */
constant_data, global_node_pose, global_submap_pose, constant_data, global_node_pose, global_submap_pose,
*scan_matcher, constraint); *scan_matcher, constraint);
@ -114,7 +114,7 @@ void ConstraintBuilder3D::MaybeAddGlobalConstraint(
const std::vector<TrajectoryNode>& submap_nodes, const std::vector<TrajectoryNode>& submap_nodes,
const Eigen::Quaterniond& global_node_rotation, const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation) { const Eigen::Quaterniond& global_submap_rotation) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (when_done_) { if (when_done_) {
LOG(WARNING) LOG(WARNING)
<< "MaybeAddGlobalConstraint was called while WhenDone was scheduled."; << "MaybeAddGlobalConstraint was called while WhenDone was scheduled.";
@ -125,7 +125,7 @@ void ConstraintBuilder3D::MaybeAddGlobalConstraint(
const auto* scan_matcher = const auto* scan_matcher =
DispatchScanMatcherConstruction(submap_id, submap_nodes, submap); DispatchScanMatcherConstruction(submap_id, submap_nodes, submap);
auto constraint_task = absl::make_unique<common::Task>(); auto constraint_task = absl::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) { constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
ComputeConstraint(submap_id, node_id, true, /* match_full_submap */ ComputeConstraint(submap_id, node_id, true, /* match_full_submap */
constant_data, constant_data,
transform::Rigid3d::Rotation(global_node_rotation), transform::Rigid3d::Rotation(global_node_rotation),
@ -139,10 +139,10 @@ void ConstraintBuilder3D::MaybeAddGlobalConstraint(
} }
void ConstraintBuilder3D::NotifyEndOfNode() { void ConstraintBuilder3D::NotifyEndOfNode() {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(finish_node_task_ != nullptr); CHECK(finish_node_task_ != nullptr);
finish_node_task_->SetWorkItem([this] { finish_node_task_->SetWorkItem([this] {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
++num_finished_nodes_; ++num_finished_nodes_;
}); });
auto finish_node_task_handle = auto finish_node_task_handle =
@ -154,7 +154,7 @@ void ConstraintBuilder3D::NotifyEndOfNode() {
void ConstraintBuilder3D::WhenDone( void ConstraintBuilder3D::WhenDone(
const std::function<void(const ConstraintBuilder3D::Result&)>& callback) { const std::function<void(const ConstraintBuilder3D::Result&)>& callback) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(when_done_ == nullptr); CHECK(when_done_ == nullptr);
// TODO(gaschler): Consider using just std::function, it can also be empty. // TODO(gaschler): Consider using just std::function, it can also be empty.
when_done_ = absl::make_unique<std::function<void(const Result&)>>(callback); when_done_ = absl::make_unique<std::function<void(const Result&)>>(callback);
@ -248,7 +248,7 @@ void ConstraintBuilder3D::ComputeConstraint(
} }
} }
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
score_histogram_.Add(match_result->score); score_histogram_.Add(match_result->score);
rotational_score_histogram_.Add(match_result->rotational_score); rotational_score_histogram_.Add(match_result->rotational_score);
low_resolution_score_histogram_.Add(match_result->low_resolution_score); low_resolution_score_histogram_.Add(match_result->low_resolution_score);
@ -301,7 +301,7 @@ void ConstraintBuilder3D::RunWhenDoneCallback() {
Result result; Result result;
std::unique_ptr<std::function<void(const Result&)>> callback; std::unique_ptr<std::function<void(const Result&)>> callback;
{ {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
CHECK(when_done_ != nullptr); CHECK(when_done_ != nullptr);
for (const std::unique_ptr<Constraint>& constraint : constraints_) { for (const std::unique_ptr<Constraint>& constraint : constraints_) {
if (constraint == nullptr) continue; if (constraint == nullptr) continue;
@ -326,12 +326,12 @@ void ConstraintBuilder3D::RunWhenDoneCallback() {
} }
int ConstraintBuilder3D::GetNumFinishedNodes() { int ConstraintBuilder3D::GetNumFinishedNodes() {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return num_finished_nodes_; return num_finished_nodes_;
} }
void ConstraintBuilder3D::DeleteScanMatcher(const SubmapId& submap_id) { void ConstraintBuilder3D::DeleteScanMatcher(const SubmapId& submap_id) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
if (when_done_) { if (when_done_) {
LOG(WARNING) LOG(WARNING)
<< "DeleteScanMatcher was called while WhenDone was scheduled."; << "DeleteScanMatcher was called while WhenDone was scheduled.";

10
cartographer/mapping/internal/constraints/constraint_builder_3d.h
View File

@ -25,11 +25,11 @@
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "absl/synchronization/mutex.h"
#include "cartographer/common/fixed_ratio_sampler.h" #include "cartographer/common/fixed_ratio_sampler.h"
#include "cartographer/common/histogram.h" #include "cartographer/common/histogram.h"
#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/common/mutex.h"
#include "cartographer/common/task.h" #include "cartographer/common/task.h"
#include "cartographer/common/thread_pool.h" #include "cartographer/common/thread_pool.h"
#include "cartographer/mapping/3d/submap_3d.h" #include "cartographer/mapping/3d/submap_3d.h"
@ -128,7 +128,7 @@ class ConstraintBuilder3D {
const SubmapScanMatcher* DispatchScanMatcherConstruction( const SubmapScanMatcher* DispatchScanMatcherConstruction(
const SubmapId& submap_id, const SubmapId& submap_id,
const std::vector<TrajectoryNode>& submap_nodes, const Submap3D* submap) const std::vector<TrajectoryNode>& submap_nodes, const Submap3D* submap)
REQUIRES(mutex_); EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Runs in a background thread and does computations for an additional // Runs in a background thread and does computations for an additional
// constraint. // constraint.
@ -140,13 +140,13 @@ class ConstraintBuilder3D {
const transform::Rigid3d& global_submap_pose, const transform::Rigid3d& global_submap_pose,
const SubmapScanMatcher& submap_scan_matcher, const SubmapScanMatcher& submap_scan_matcher,
std::unique_ptr<Constraint>* constraint) std::unique_ptr<Constraint>* constraint)
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
void RunWhenDoneCallback() EXCLUDES(mutex_); void RunWhenDoneCallback() LOCKS_EXCLUDED(mutex_);
const proto::ConstraintBuilderOptions options_; const proto::ConstraintBuilderOptions options_;
common::ThreadPoolInterface* thread_pool_; common::ThreadPoolInterface* thread_pool_;
common::Mutex mutex_; absl::Mutex mutex_;
// 'callback' set by WhenDone(). // 'callback' set by WhenDone().
std::unique_ptr<std::function<void(const Result&)>> when_done_ std::unique_ptr<std::function<void(const Result&)>> when_done_

6
cartographer/pose_graph/pose_graph_controller.cc
View File

@ -20,17 +20,17 @@ namespace cartographer {
namespace pose_graph { namespace pose_graph {
void PoseGraphController::AddNode(const proto::Node& node) { void PoseGraphController::AddNode(const proto::Node& node) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
AddNodeToPoseGraphData(node, &data_); AddNodeToPoseGraphData(node, &data_);
} }
void PoseGraphController::AddConstraint(const proto::Constraint& constraint) { void PoseGraphController::AddConstraint(const proto::Constraint& constraint) {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
AddConstraintToPoseGraphData(constraint, &data_); AddConstraintToPoseGraphData(constraint, &data_);
} }
Optimizer::SolverStatus PoseGraphController::Optimize() { Optimizer::SolverStatus PoseGraphController::Optimize() {
common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return optimizer_->Solve(&data_); return optimizer_->Solve(&data_);
} }

11
cartographer/pose_graph/pose_graph_controller.h
View File

@ -17,7 +17,7 @@
#ifndef CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_ #ifndef CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_
#define 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/optimizer/optimizer.h"
#include "cartographer/pose_graph/pose_graph_data.h" #include "cartographer/pose_graph/pose_graph_data.h"
@ -31,15 +31,16 @@ class PoseGraphController {
PoseGraphController(const PoseGraphController&) = delete; PoseGraphController(const PoseGraphController&) = delete;
PoseGraphController& operator=(const PoseGraphController&) = delete; PoseGraphController& operator=(const PoseGraphController&) = delete;
void AddNode(const proto::Node& node) EXCLUDES(mutex_); void AddNode(const proto::Node& node) LOCKS_EXCLUDED(mutex_);
void AddConstraint(const proto::Constraint& constraint) EXCLUDES(mutex_); void AddConstraint(const proto::Constraint& constraint)
LOCKS_EXCLUDED(mutex_);
Optimizer::SolverStatus Optimize() EXCLUDES(mutex_); Optimizer::SolverStatus Optimize() LOCKS_EXCLUDED(mutex_);
private: private:
std::unique_ptr<Optimizer> optimizer_; std::unique_ptr<Optimizer> optimizer_;
mutable common::Mutex mutex_; mutable absl::Mutex mutex_;
PoseGraphData data_ GUARDED_BY(mutex_); PoseGraphData data_ GUARDED_BY(mutex_);
}; };