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

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_;
common::BlockingQueue<std::unique_ptr<MapBuilderContextInterface::Data>>
incoming_data_queue_;
common::Mutex subscriptions_lock_;
absl::Mutex subscriptions_lock_;
int current_subscription_index_ = 0;
std::map<int /* trajectory ID */, LocalSlamResultHandlerSubscriptions>
local_slam_subscriptions_ GUARDED_BY(subscriptions_lock_);

65
cartographer/common/blocking_queue.h
View File

@ -21,7 +21,7 @@
#include <deque>
#include <memory>
#include "cartographer/common/mutex.h"
#include "absl/synchronization/mutex.h"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "glog/logging.h"
@ -47,17 +47,22 @@ class BlockingQueue {
// Pushes a value onto the queue. Blocks if the queue is full.
void Push(T t) {
MutexLocker lock(&mutex_);
lock.Await([this]() REQUIRES(mutex_) { return QueueNotFullCondition(); });
const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return QueueNotFullCondition();
};
absl::MutexLock lock(&mutex_);
mutex_.Await(absl::Condition(&predicate));
deque_.push_back(std::move(t));
}
// Like push, but returns false if 'timeout' is reached.
bool PushWithTimeout(T t, const common::Duration timeout) {
MutexLocker lock(&mutex_);
if (!lock.AwaitWithTimeout(
[this]() REQUIRES(mutex_) { return QueueNotFullCondition(); },
timeout)) {
const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return QueueNotFullCondition();
};
absl::MutexLock lock(&mutex_);
if (!mutex_.AwaitWithTimeout(absl::Condition(&predicate),
absl::FromChrono(timeout))) {
return false;
}
deque_.push_back(std::move(t));
@ -66,8 +71,11 @@ class BlockingQueue {
// Pops the next value from the queue. Blocks until a value is available.
T Pop() {
MutexLocker lock(&mutex_);
lock.Await([this]() REQUIRES(mutex_) { return !QueueEmptyCondition(); });
const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return !QueueEmptyCondition();
};
absl::MutexLock lock(&mutex_);
mutex_.Await(absl::Condition(&predicate));
T t = std::move(deque_.front());
deque_.pop_front();
@ -76,10 +84,12 @@ class BlockingQueue {
// Like Pop, but can timeout. Returns nullptr in this case.
T PopWithTimeout(const common::Duration timeout) {
MutexLocker lock(&mutex_);
if (!lock.AwaitWithTimeout(
[this]() REQUIRES(mutex_) { return !QueueEmptyCondition(); },
timeout)) {
const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return !QueueEmptyCondition();
};
absl::MutexLock lock(&mutex_);
if (!mutex_.AwaitWithTimeout(absl::Condition(&predicate),
absl::FromChrono(timeout))) {
return nullptr;
}
T t = std::move(deque_.front());
@ -90,10 +100,12 @@ class BlockingQueue {
// Like Peek, but can timeout. Returns nullptr in this case.
template <typename R>
R* PeekWithTimeout(const common::Duration timeout) {
MutexLocker lock(&mutex_);
if (!lock.AwaitWithTimeout(
[this]() REQUIRES(mutex_) { return !QueueEmptyCondition(); },
timeout)) {
const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return !QueueEmptyCondition();
};
absl::MutexLock lock(&mutex_);
if (!mutex_.AwaitWithTimeout(absl::Condition(&predicate),
absl::FromChrono(timeout))) {
return nullptr;
}
return deque_.front().get();
@ -104,7 +116,7 @@ class BlockingQueue {
// a pointer to the given type R.
template <typename R>
const R* Peek() {
MutexLocker lock(&mutex_);
absl::MutexLock lock(&mutex_);
if (deque_.empty()) {
return nullptr;
}
@ -113,26 +125,31 @@ class BlockingQueue {
// Returns the number of items currently in the queue.
size_t Size() {
MutexLocker lock(&mutex_);
absl::MutexLock lock(&mutex_);
return deque_.size();
}
// Blocks until the queue is empty.
void WaitUntilEmpty() {
MutexLocker lock(&mutex_);
lock.Await([this]() REQUIRES(mutex_) { return QueueEmptyCondition(); });
const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return QueueEmptyCondition();
};
absl::MutexLock lock(&mutex_);
mutex_.Await(absl::Condition(&predicate));
}
private:
// Returns true iff the queue is empty.
bool QueueEmptyCondition() REQUIRES(mutex_) { return deque_.empty(); }
bool QueueEmptyCondition() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return deque_.empty();
}
// Returns true iff the queue is not full.
bool QueueNotFullCondition() REQUIRES(mutex_) {
bool QueueNotFullCondition() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return queue_size_ == kInfiniteQueueSize || deque_.size() < queue_size_;
}
Mutex mutex_;
absl::Mutex mutex_;
const size_t queue_size_ GUARDED_BY(mutex_);
std::deque<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() {
{
MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
CHECK(running_);
running_ = false;
CHECK_EQ(task_queue_.size(), 0);
@ -45,7 +45,7 @@ ThreadPoolForTesting::~ThreadPoolForTesting() {
}
void ThreadPoolForTesting::NotifyDependenciesCompleted(Task* task) {
MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
CHECK(running_);
auto it = tasks_not_ready_.find(task);
CHECK(it != tasks_not_ready_.end());
@ -56,7 +56,7 @@ void ThreadPoolForTesting::NotifyDependenciesCompleted(Task* task) {
std::weak_ptr<Task> ThreadPoolForTesting::Schedule(std::unique_ptr<Task> task) {
std::shared_ptr<Task> shared_task;
{
MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
idle_ = false;
CHECK(running_);
auto insert_result =
@ -69,11 +69,13 @@ std::weak_ptr<Task> ThreadPoolForTesting::Schedule(std::unique_ptr<Task> task) {
}
void ThreadPoolForTesting::WaitUntilIdle() {
const auto predicate = [this]()
EXCLUSIVE_LOCKS_REQUIRED(mutex_) { return idle_; };
for (;;) {
{
common::MutexLocker locker(&mutex_);
if (locker.AwaitWithTimeout([this]() REQUIRES(mutex_) { return idle_; },
common::FromSeconds(0.1))) {
absl::MutexLock locker(&mutex_);
if (mutex_.AwaitWithTimeout(absl::Condition(&predicate),
absl::FromChrono(common::FromSeconds(0.1)))) {
return;
}
}
@ -81,14 +83,15 @@ void ThreadPoolForTesting::WaitUntilIdle() {
}
void ThreadPoolForTesting::DoWork() {
const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return !task_queue_.empty() || !running_;
};
for (;;) {
std::shared_ptr<Task> task;
{
MutexLocker locker(&mutex_);
locker.AwaitWithTimeout(
[this]()
REQUIRES(mutex_) { return !task_queue_.empty() || !running_; },
common::FromSeconds(0.1));
absl::MutexLock locker(&mutex_);
mutex_.AwaitWithTimeout(absl::Condition(&predicate),
absl::FromChrono(common::FromSeconds(0.1)));
if (!task_queue_.empty()) {
task = task_queue_.front();
task_queue_.pop_front();

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

@ -22,7 +22,7 @@
#include <map>
#include <thread>
#include "cartographer/common/mutex.h"
#include "absl/synchronization/mutex.h"
#include "cartographer/common/thread_pool.h"
namespace cartographer {
@ -35,7 +35,7 @@ class ThreadPoolForTesting : public ThreadPoolInterface {
~ThreadPoolForTesting();
std::weak_ptr<Task> Schedule(std::unique_ptr<Task> task)
EXCLUDES(mutex_) override;
LOCKS_EXCLUDED(mutex_) override;
void WaitUntilIdle();
@ -44,9 +44,9 @@ class ThreadPoolForTesting : public ThreadPoolInterface {
void DoWork();
void NotifyDependenciesCompleted(Task* task) EXCLUDES(mutex_) override;
void NotifyDependenciesCompleted(Task* task) LOCKS_EXCLUDED(mutex_) override;
Mutex mutex_;
absl::Mutex mutex_;
bool running_ GUARDED_BY(mutex_) = true;
bool idle_ GUARDED_BY(mutex_) = true;
std::deque<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() {
MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return state_;
}
void Task::SetWorkItem(const WorkItem& work_item) {
MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
CHECK_EQ(state_, NEW);
work_item_ = work_item;
}
@ -40,7 +40,7 @@ void Task::SetWorkItem(const WorkItem& work_item) {
void Task::AddDependency(std::weak_ptr<Task> dependency) {
std::shared_ptr<Task> shared_dependency;
{
MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
CHECK_EQ(state_, NEW);
if ((shared_dependency = dependency.lock())) {
++uncompleted_dependencies_;
@ -52,7 +52,7 @@ void Task::AddDependency(std::weak_ptr<Task> dependency) {
}
void Task::SetThreadPool(ThreadPoolInterface* thread_pool) {
MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
CHECK_EQ(state_, NEW);
state_ = DISPATCHED;
thread_pool_to_notify_ = thread_pool;
@ -64,7 +64,7 @@ void Task::SetThreadPool(ThreadPoolInterface* thread_pool) {
}
void Task::AddDependentTask(Task* dependent_task) {
MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
if (state_ == COMPLETED) {
dependent_task->OnDependenyCompleted();
return;
@ -74,7 +74,7 @@ void Task::AddDependentTask(Task* dependent_task) {
}
void Task::OnDependenyCompleted() {
MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
CHECK(state_ == NEW || state_ == DISPATCHED);
--uncompleted_dependencies_;
if (uncompleted_dependencies_ == 0 && state_ == DISPATCHED) {
@ -86,7 +86,7 @@ void Task::OnDependenyCompleted() {
void Task::Execute() {
{
MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
CHECK_EQ(state_, DEPENDENCIES_COMPLETED);
state_ = RUNNING;
}
@ -96,7 +96,7 @@ void Task::Execute() {
work_item_();
}
MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
state_ = COMPLETED;
for (Task* dependent_task : dependent_tasks_) {
dependent_task->OnDependenyCompleted();

14
cartographer/common/task.h
View File

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

17
cartographer/common/thread_pool.cc
View File

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

8
cartographer/common/thread_pool.h
View File

@ -24,7 +24,7 @@
#include <unordered_map>
#include <vector>
#include "cartographer/common/mutex.h"
#include "absl/synchronization/mutex.h"
#include "cartographer/common/task.h"
namespace cartographer {
@ -65,14 +65,14 @@ class ThreadPool : public ThreadPoolInterface {
// When the returned weak pointer is expired, 'task' has certainly completed,
// so dependants no longer need to add it as a dependency.
std::weak_ptr<Task> Schedule(std::unique_ptr<Task> task)
EXCLUDES(mutex_) override;
LOCKS_EXCLUDED(mutex_) override;
private:
void DoWork();
void NotifyDependenciesCompleted(Task* task) EXCLUDES(mutex_) override;
void NotifyDependenciesCompleted(Task* task) LOCKS_EXCLUDED(mutex_) override;
Mutex mutex_;
absl::Mutex mutex_;
bool running_ GUARDED_BY(mutex_) = true;
std::vector<std::thread> pool_ 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 {
public:
void Receive(int number) {
MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
received_numbers_.push_back(number);
}
void WaitForNumberSequence(const std::vector<int>& expected_numbers) {
common::MutexLocker locker(&mutex_);
locker.Await([this, &expected_numbers]() REQUIRES(mutex_) {
return (received_numbers_.size() >= expected_numbers.size());
});
const auto predicate =
[this, &expected_numbers]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return (received_numbers_.size() >= expected_numbers.size());
};
absl::MutexLock locker(&mutex_);
mutex_.Await(absl::Condition(&predicate));
EXPECT_EQ(expected_numbers, received_numbers_);
}
Mutex mutex_;
absl::Mutex mutex_;
std::vector<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() {
WaitForAllComputations();
common::MutexLocker locker(&work_queue_mutex_);
absl::MutexLock locker(&work_queue_mutex_);
CHECK(work_queue_ == nullptr);
}
@ -113,7 +113,7 @@ NodeId PoseGraph2D::AppendNode(
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps,
const transform::Rigid3d& optimized_pose) {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
AddTrajectoryIfNeeded(trajectory_id);
if (!CanAddWorkItemModifying(trajectory_id)) {
LOG(WARNING) << "AddNode was called for finished or deleted trajectory.";
@ -147,7 +147,7 @@ NodeId PoseGraph2D::AddNode(
// We have to check this here, because it might have changed by the time we
// execute the lambda.
const bool newly_finished_submap = insertion_submaps.front()->finished();
AddWorkItem([=]() EXCLUDES(mutex_) {
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
return ComputeConstraintsForNode(node_id, insertion_submaps,
newly_finished_submap);
});
@ -156,7 +156,7 @@ NodeId PoseGraph2D::AddNode(
void PoseGraph2D::AddWorkItem(
const std::function<WorkItem::Result()>& work_item) {
common::MutexLocker locker(&work_queue_mutex_);
absl::MutexLock locker(&work_queue_mutex_);
if (work_queue_ == nullptr) {
work_queue_ = absl::make_unique<WorkQueue>();
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,
const sensor::ImuData& imu_data) {
AddWorkItem([=]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddImuData(trajectory_id, imu_data);
}
@ -201,8 +201,8 @@ void PoseGraph2D::AddImuData(const int trajectory_id,
void PoseGraph2D::AddOdometryData(const int trajectory_id,
const sensor::OdometryData& odometry_data) {
AddWorkItem([=]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
}
@ -218,8 +218,8 @@ void PoseGraph2D::AddFixedFramePoseData(
void PoseGraph2D::AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data) {
AddWorkItem([=]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) {
for (const auto& observation : landmark_data.landmark_observations) {
data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
@ -240,7 +240,7 @@ void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
const TrajectoryNode::Data* constant_data;
const Submap2D* submap;
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
if (!data_.submap_data.at(submap_id).submap->finished()) {
// Uplink server only receives grids when they are finished, so skip
@ -292,7 +292,7 @@ WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
std::vector<SubmapId> finished_submap_ids;
std::set<NodeId> newly_finished_submap_node_ids;
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
const auto& constant_data =
data_.trajectory_nodes.at(node_id).constant_data;
submap_ids = InitializeGlobalSubmapPoses(
@ -417,7 +417,7 @@ void PoseGraph2D::DeleteTrajectoriesIfNeeded() {
void PoseGraph2D::HandleWorkQueue(
const constraints::ConstraintBuilder2D::Result& result) {
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
data_.constraints.insert(data_.constraints.end(), result.begin(),
result.end());
}
@ -427,7 +427,7 @@ void PoseGraph2D::HandleWorkQueue(
std::map<int, NodeId> trajectory_id_to_last_optimized_node_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& node_data = optimization_problem_->node_data();
for (const int trajectory_id : node_data.trajectory_ids()) {
@ -449,7 +449,7 @@ void PoseGraph2D::HandleWorkQueue(
}
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
for (const Constraint& constraint : result) {
UpdateTrajectoryConnectivity(constraint);
}
@ -477,7 +477,7 @@ void PoseGraph2D::DrainWorkQueue() {
while (process_work_queue) {
std::function<WorkItem::Result()> work_item;
{
common::MutexLocker locker(&work_queue_mutex_);
absl::MutexLock locker(&work_queue_mutex_);
if (work_queue_->empty()) {
work_queue_.reset();
return;
@ -499,7 +499,7 @@ void PoseGraph2D::DrainWorkQueue() {
void PoseGraph2D::WaitForAllComputations() {
int num_trajectory_nodes;
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
num_trajectory_nodes = data_.num_trajectory_nodes;
}
@ -524,29 +524,35 @@ void PoseGraph2D::WaitForAllComputations() {
// First wait for the work queue to drain so that it's safe to schedule
// a WhenDone() callback.
{
common::MutexLocker locker(&work_queue_mutex_);
while (!locker.AwaitWithTimeout(
[this]() REQUIRES(work_queue_mutex_) { return work_queue_ == nullptr; },
common::FromSeconds(1.))) {
const auto predicate = [this]()
EXCLUSIVE_LOCKS_REQUIRED(work_queue_mutex_) {
return work_queue_ == nullptr;
};
absl::MutexLock locker(&work_queue_mutex_);
while (!work_queue_mutex_.AwaitWithTimeout(
absl::Condition(&predicate),
absl::FromChrono(common::FromSeconds(1.)))) {
report_progress();
}
}
// Now wait for any pending constraint computations to finish.
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
bool notification = false;
constraint_builder_.WhenDone(
[this,
&notification](const constraints::ConstraintBuilder2D::Result& result)
EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
data_.constraints.insert(data_.constraints.end(), result.begin(),
result.end());
notification = true;
});
while (!locker.AwaitWithTimeout([&notification]()
REQUIRES(mutex_) { return notification; },
common::FromSeconds(1.))) {
const auto predicate = [&notification]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return notification;
};
while (!mutex_.AwaitWithTimeout(absl::Condition(&predicate),
absl::FromChrono(common::FromSeconds(1.)))) {
report_progress();
}
CHECK_EQ(constraint_builder_.GetNumFinishedNodes(), num_trajectory_nodes);
@ -555,7 +561,7 @@ void PoseGraph2D::WaitForAllComputations() {
void PoseGraph2D::DeleteTrajectory(const int trajectory_id) {
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
auto it = data_.trajectories_state.find(trajectory_id);
if (it == data_.trajectories_state.end()) {
LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: "
@ -565,8 +571,8 @@ void PoseGraph2D::DeleteTrajectory(const int trajectory_id) {
it->second.deletion_state =
InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION;
}
AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
CHECK(data_.trajectories_state.at(trajectory_id).state !=
TrajectoryState::ACTIVE);
CHECK(data_.trajectories_state.at(trajectory_id).state !=
@ -580,8 +586,8 @@ void PoseGraph2D::DeleteTrajectory(const int trajectory_id) {
}
void PoseGraph2D::FinishTrajectory(const int trajectory_id) {
AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
CHECK(!IsTrajectoryFinished(trajectory_id));
data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED;
@ -600,11 +606,11 @@ bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) const {
void PoseGraph2D::FreezeTrajectory(const int trajectory_id) {
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
data_.trajectory_connectivity_state.Add(trajectory_id);
}
AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
CHECK(!IsTrajectoryFrozen(trajectory_id));
data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN;
return WorkItem::Result::kDoNotRunOptimization;
@ -629,7 +635,7 @@ void PoseGraph2D::AddSubmapFromProto(
const transform::Rigid2d global_submap_pose_2d =
transform::Project2D(global_submap_pose);
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
const std::shared_ptr<const Submap2D> submap_ptr =
std::make_shared<const Submap2D>(submap.submap_2d(),
&conversion_tables_);
@ -641,12 +647,13 @@ void PoseGraph2D::AddSubmapFromProto(
data_.global_submap_poses_2d.Insert(
submap_id, optimization::SubmapSpec2D{global_submap_pose_2d});
}
AddWorkItem([this, submap_id, global_submap_pose_2d]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
data_.submap_data.at(submap_id).state = SubmapState::kFinished;
optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d);
return WorkItem::Result::kDoNotRunOptimization;
});
AddWorkItem(
[this, submap_id, global_submap_pose_2d]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
data_.submap_data.at(submap_id).state = SubmapState::kFinished;
optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d);
return WorkItem::Result::kDoNotRunOptimization;
});
}
void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
@ -657,15 +664,15 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
std::make_shared<const TrajectoryNode::Data>(FromProto(node.node_data()));
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
AddTrajectoryIfNeeded(node_id.trajectory_id);
if (!CanAddWorkItemModifying(node_id.trajectory_id)) return;
data_.trajectory_nodes.Insert(node_id,
TrajectoryNode{constant_data, global_pose});
}
AddWorkItem([this, node_id, global_pose]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, node_id, global_pose]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
const auto& constant_data =
data_.trajectory_nodes.at(node_id).constant_data;
const auto gravity_alignment_inverse = transform::Rigid3d::Rotation(
@ -689,8 +696,8 @@ void PoseGraph2D::SetTrajectoryDataFromProto(
void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id,
const SubmapId& submap_id) {
AddWorkItem([this, node_id, submap_id]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, node_id, submap_id]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(submap_id.trajectory_id)) {
data_.submap_data.at(submap_id).node_ids.insert(node_id);
}
@ -700,8 +707,8 @@ void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id,
void PoseGraph2D::AddSerializedConstraints(
const std::vector<Constraint>& constraints) {
AddWorkItem([this, constraints]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, constraints]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
for (const auto& constraint : constraints) {
CHECK(data_.trajectory_nodes.Contains(constraint.node_id));
CHECK(data_.submap_data.Contains(constraint.submap_id));
@ -735,8 +742,8 @@ void PoseGraph2D::AddSerializedConstraints(
void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {
// C++11 does not allow us to move a unique_ptr into a lambda.
PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
AddWorkItem([this, trimmer_ptr]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
trimmers_.emplace_back(trimmer_ptr);
return WorkItem::Result::kDoNotRunOptimization;
});
@ -744,14 +751,14 @@ void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {
void PoseGraph2D::RunFinalOptimization() {
{
AddWorkItem([this]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
optimization_problem_->SetMaxNumIterations(
options_.max_num_final_iterations());
return WorkItem::Result::kRunOptimization;
});
AddWorkItem([this]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
optimization_problem_->SetMaxNumIterations(
options_.optimization_problem_options()
.ceres_solver_options()
@ -773,7 +780,7 @@ void PoseGraph2D::RunOptimization() {
// before Solve to avoid blocking foreground processing.
optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
data_.landmark_nodes);
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
const auto& submap_data = optimization_problem_->submap_data();
const auto& node_data = optimization_problem_->node_data();
@ -841,14 +848,14 @@ bool PoseGraph2D::CanAddWorkItemModifying(int trajectory_id) {
}
MapById<NodeId, TrajectoryNode> PoseGraph2D::GetTrajectoryNodes() const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return data_.trajectory_nodes;
}
MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses()
const {
MapById<NodeId, TrajectoryNodePose> node_poses;
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
for (const auto& node_id_data : data_.trajectory_nodes) {
absl::optional<TrajectoryNodePose::ConstantPoseData> constant_pose_data;
if (node_id_data.data.constant_data != nullptr) {
@ -866,7 +873,7 @@ MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses()
std::map<int, PoseGraphInterface::TrajectoryState>
PoseGraph2D::GetTrajectoryStates() const {
std::map<int, PoseGraphInterface::TrajectoryState> trajectories_state;
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
for (const auto& it : data_.trajectories_state) {
trajectories_state[it.first] = it.second.state;
}
@ -876,7 +883,7 @@ PoseGraph2D::GetTrajectoryStates() const {
std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses()
const {
std::map<std::string, transform::Rigid3d> landmark_poses;
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
for (const auto& landmark : data_.landmark_nodes) {
// Landmark without value has not been optimized yet.
if (!landmark.second.global_landmark_pose.has_value()) continue;
@ -888,26 +895,26 @@ std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses()
void PoseGraph2D::SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) {
AddWorkItem([=]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose;
return WorkItem::Result::kDoNotRunOptimization;
});
}
sensor::MapByTime<sensor::ImuData> PoseGraph2D::GetImuData() const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return optimization_problem_->imu_data();
}
sensor::MapByTime<sensor::OdometryData> PoseGraph2D::GetOdometryData() const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return optimization_problem_->odometry_data();
}
std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
PoseGraph2D::GetLandmarkNodes() const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return data_.landmark_nodes;
}
@ -926,7 +933,7 @@ PoseGraph2D::GetFixedFramePoseData() const {
std::vector<PoseGraphInterface::Constraint> PoseGraph2D::constraints() const {
std::vector<PoseGraphInterface::Constraint> result;
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
for (const Constraint& constraint : data_.constraints) {
result.push_back(Constraint{
constraint.submap_id, constraint.node_id,
@ -945,7 +952,7 @@ void PoseGraph2D::SetInitialTrajectoryPose(const int from_trajectory_id,
const int to_trajectory_id,
const transform::Rigid3d& pose,
const common::Time time) {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
data_.initial_trajectory_poses[from_trajectory_id] =
InitialTrajectoryPose{to_trajectory_id, pose, time};
}
@ -973,7 +980,7 @@ transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose(
transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform(
const int trajectory_id) const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return ComputeLocalToGlobalTransform(data_.global_submap_poses_2d,
trajectory_id);
}
@ -984,19 +991,19 @@ std::vector<std::vector<int>> PoseGraph2D::GetConnectedTrajectories() const {
PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapData(
const SubmapId& submap_id) const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return GetSubmapDataUnderLock(submap_id);
}
MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph2D::GetAllSubmapData() const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return GetSubmapDataUnderLock();
}
MapById<SubmapId, PoseGraphInterface::SubmapPose>
PoseGraph2D::GetAllSubmapPoses() const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
MapById<SubmapId, SubmapPose> submap_poses;
for (const auto& submap_id_data : data_.submap_data) {
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/Geometry"
#include "absl/synchronization/mutex.h"
#include "cartographer/common/fixed_ratio_sampler.h"
#include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/2d/submap_2d.h"
@ -81,26 +81,28 @@ class PoseGraph2D : public PoseGraph {
std::shared_ptr<const TrajectoryNode::Data> constant_data,
int trajectory_id,
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
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
void AddOdometryData(int trajectory_id,
const sensor::OdometryData& odometry_data) override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
void AddFixedFramePoseData(
int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
void AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data) override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
void DeleteTrajectory(int trajectory_id) override;
void FinishTrajectory(int trajectory_id) override;
bool IsTrajectoryFinished(int trajectory_id) const override REQUIRES(mutex_);
bool IsTrajectoryFinished(int trajectory_id) const override
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
void FreezeTrajectory(int trajectory_id) override;
bool IsTrajectoryFrozen(int trajectory_id) const override REQUIRES(mutex_);
bool IsTrajectoryFrozen(int trajectory_id) const override
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
const proto::Submap& submap) override;
void AddNodeFromProto(const transform::Rigid3d& global_pose,
@ -114,56 +116,58 @@ class PoseGraph2D : public PoseGraph {
void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() const override;
PoseGraphInterface::SubmapData GetSubmapData(const SubmapId& submap_id) const
EXCLUDES(mutex_) override;
LOCKS_EXCLUDED(mutex_) override;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() const
EXCLUDES(mutex_) override;
LOCKS_EXCLUDED(mutex_) override;
MapById<SubmapId, SubmapPose> GetAllSubmapPoses() const
EXCLUDES(mutex_) override;
LOCKS_EXCLUDED(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) const
EXCLUDES(mutex_) override;
LOCKS_EXCLUDED(mutex_) override;
MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
std::map<int, TrajectoryState> GetTrajectoryStates() const override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
void SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
sensor::MapByTime<sensor::ImuData> GetImuData() const override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
sensor::MapByTime<sensor::OdometryData> GetOdometryData() const override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData()
const override EXCLUDES(mutex_);
const override LOCKS_EXCLUDED(mutex_);
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
EXCLUDES(mutex_);
std::vector<Constraint> constraints() const override EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
std::vector<Constraint> constraints() const override LOCKS_EXCLUDED(mutex_);
void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,
const transform::Rigid3d& pose,
const common::Time time) override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
void SetGlobalSlamOptimizationCallback(
PoseGraphInterface::GlobalSlamOptimizationCallback callback) override;
transform::Rigid3d GetInterpolatedGlobalTrajectoryPose(
int trajectory_id, const common::Time time) const REQUIRES(mutex_);
int trajectory_id, const common::Time time) const
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
static void RegisterMetrics(metrics::FamilyFactory* family_factory);
private:
MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock()
const REQUIRES(mutex_);
const EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Handles a new 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.
void AddTrajectoryIfNeeded(int trajectory_id) REQUIRES(mutex_);
void AddTrajectoryIfNeeded(int trajectory_id)
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Appends the new node and submap (if needed) to the internal data
// structures.
@ -171,68 +175,71 @@ class PoseGraph2D : public PoseGraph {
std::shared_ptr<const TrajectoryNode::Data> constant_data,
int trajectory_id,
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
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
std::vector<SubmapId> InitializeGlobalSubmapPoses(
int trajectory_id, const common::Time time,
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.
WorkItem::Result ComputeConstraintsForNode(
const NodeId& node_id,
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.
void ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id)
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
// Deletes trajectories waiting for deletion. Must not be called during
// constraint search.
void DeleteTrajectoriesIfNeeded() REQUIRES(mutex_);
void DeleteTrajectoriesIfNeeded() EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Runs the optimization, executes the trimmers and processes the work queue.
void HandleWorkQueue(const constraints::ConstraintBuilder2D::Result& result)
EXCLUDES(mutex_) EXCLUDES(work_queue_mutex_);
LOCKS_EXCLUDED(mutex_) LOCKS_EXCLUDED(work_queue_mutex_);
// Process pending tasks in the work queue on the calling thread, until the
// queue is either empty or an optimization is required.
void DrainWorkQueue() EXCLUDES(mutex_) EXCLUDES(work_queue_mutex_);
void DrainWorkQueue() LOCKS_EXCLUDED(mutex_)
LOCKS_EXCLUDED(work_queue_mutex_);
// Waits until we caught up (i.e. nothing is waiting to be scheduled), and
// all computations have finished.
void WaitForAllComputations() EXCLUDES(mutex_) EXCLUDES(work_queue_mutex_);
void WaitForAllComputations() LOCKS_EXCLUDED(mutex_)
LOCKS_EXCLUDED(work_queue_mutex_);
// Runs the optimization. Callers have to make sure, that there is only one
// optimization being run at a time.
void RunOptimization() EXCLUDES(mutex_);
void RunOptimization() LOCKS_EXCLUDED(mutex_);
bool CanAddWorkItemModifying(int trajectory_id) REQUIRES(mutex_);
bool CanAddWorkItemModifying(int trajectory_id)
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Computes the local to global map frame transform based on the given
// 'global_submap_poses'.
transform::Rigid3d ComputeLocalToGlobalTransform(
const MapById<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
REQUIRES(mutex_);
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
common::Time GetLatestNodeTime(const NodeId& node_id,
const SubmapId& submap_id) const
REQUIRES(mutex_);
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Updates the trajectory connectivity structure with a new constraint.
void UpdateTrajectoryConnectivity(const Constraint& constraint)
REQUIRES(mutex_);
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
const proto::PoseGraphOptions options_;
GlobalSlamOptimizationCallback global_slam_optimization_callback_;
mutable common::Mutex mutex_;
common::Mutex work_queue_mutex_;
mutable absl::Mutex mutex_;
absl::Mutex work_queue_mutex_;
// If it exists, further work items must be added to this queue, and will be
// considered later.
@ -269,16 +276,17 @@ class PoseGraph2D : public PoseGraph {
int num_submaps(int trajectory_id) const override;
std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
MapById<SubmapId, SubmapData> GetOptimizedSubmapData() const override
REQUIRES(parent_->mutex_);
EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
REQUIRES(parent_->mutex_);
EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
const std::vector<Constraint>& GetConstraints() const override
REQUIRES(parent_->mutex_);
EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
void TrimSubmap(const SubmapId& submap_id)
REQUIRES(parent_->mutex_) override;
bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_);
EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_) override;
bool IsFinished(int trajectory_id) const override
EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
void SetTrajectoryState(int trajectory_id, TrajectoryState state) override
REQUIRES(parent_->mutex_);
EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
private:
PoseGraph2D* const parent_;

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

@ -52,7 +52,7 @@ PoseGraph3D::PoseGraph3D(
PoseGraph3D::~PoseGraph3D() {
WaitForAllComputations();
common::MutexLocker locker(&work_queue_mutex_);
absl::MutexLock locker(&work_queue_mutex_);
CHECK(work_queue_ == nullptr);
}
@ -110,7 +110,7 @@ NodeId PoseGraph3D::AppendNode(
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps,
const transform::Rigid3d& optimized_pose) {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
AddTrajectoryIfNeeded(trajectory_id);
if (!CanAddWorkItemModifying(trajectory_id)) {
LOG(WARNING) << "AddNode was called for finished or deleted trajectory.";
@ -144,7 +144,7 @@ NodeId PoseGraph3D::AddNode(
// We have to check this here, because it might have changed by the time we
// execute the lambda.
const bool newly_finished_submap = insertion_submaps.front()->finished();
AddWorkItem([=]() EXCLUDES(mutex_) {
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
return ComputeConstraintsForNode(node_id, insertion_submaps,
newly_finished_submap);
});
@ -153,7 +153,7 @@ NodeId PoseGraph3D::AddNode(
void PoseGraph3D::AddWorkItem(
const std::function<WorkItem::Result()>& work_item) {
common::MutexLocker locker(&work_queue_mutex_);
absl::MutexLock locker(&work_queue_mutex_);
if (work_queue_ == nullptr) {
work_queue_ = absl::make_unique<WorkQueue>();
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,
const sensor::ImuData& imu_data) {
AddWorkItem([=]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddImuData(trajectory_id, imu_data);
}
@ -198,8 +198,8 @@ void PoseGraph3D::AddImuData(const int trajectory_id,
void PoseGraph3D::AddOdometryData(const int trajectory_id,
const sensor::OdometryData& odometry_data) {
AddWorkItem([=]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
}
@ -210,8 +210,8 @@ void PoseGraph3D::AddOdometryData(const int trajectory_id,
void PoseGraph3D::AddFixedFramePoseData(
const int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) {
AddWorkItem([=]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddFixedFramePoseData(trajectory_id,
fixed_frame_pose_data);
@ -222,8 +222,8 @@ void PoseGraph3D::AddFixedFramePoseData(
void PoseGraph3D::AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data) {
AddWorkItem([=]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) {
for (const auto& observation : landmark_data.landmark_observations) {
data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
@ -254,7 +254,7 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
const Submap3D* submap;
std::vector<TrajectoryNode> submap_nodes;
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
if (!data_.submap_data.at(submap_id).submap->finished()) {
// Uplink server only receives grids when they are finished, so skip
@ -316,7 +316,7 @@ WorkItem::Result PoseGraph3D::ComputeConstraintsForNode(
std::vector<SubmapId> finished_submap_ids;
std::set<NodeId> newly_finished_submap_node_ids;
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
const auto& constant_data =
data_.trajectory_nodes.at(node_id).constant_data;
submap_ids = InitializeGlobalSubmapPoses(
@ -432,7 +432,7 @@ void PoseGraph3D::DeleteTrajectoriesIfNeeded() {
void PoseGraph3D::HandleWorkQueue(
const constraints::ConstraintBuilder3D::Result& result) {
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
data_.constraints.insert(data_.constraints.end(), result.begin(),
result.end());
}
@ -442,7 +442,7 @@ void PoseGraph3D::HandleWorkQueue(
std::map<int, NodeId> trajectory_id_to_last_optimized_node_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& node_data = optimization_problem_->node_data();
for (const int trajectory_id : node_data.trajectory_ids()) {
@ -464,7 +464,7 @@ void PoseGraph3D::HandleWorkQueue(
}
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
for (const Constraint& constraint : result) {
UpdateTrajectoryConnectivity(constraint);
}
@ -492,7 +492,7 @@ void PoseGraph3D::DrainWorkQueue() {
while (process_work_queue) {
std::function<WorkItem::Result()> work_item;
{
common::MutexLocker locker(&work_queue_mutex_);
absl::MutexLock locker(&work_queue_mutex_);
if (work_queue_->empty()) {
work_queue_.reset();
return;
@ -514,7 +514,7 @@ void PoseGraph3D::DrainWorkQueue() {
void PoseGraph3D::WaitForAllComputations() {
int num_trajectory_nodes;
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
num_trajectory_nodes = data_.num_trajectory_nodes;
}
@ -539,29 +539,35 @@ void PoseGraph3D::WaitForAllComputations() {
// First wait for the work queue to drain so that it's safe to schedule
// a WhenDone() callback.
{
common::MutexLocker locker(&work_queue_mutex_);
while (!locker.AwaitWithTimeout(
[this]() REQUIRES(work_queue_mutex_) { return work_queue_ == nullptr; },
common::FromSeconds(1.))) {
const auto predicate = [this]()
EXCLUSIVE_LOCKS_REQUIRED(work_queue_mutex_) {
return work_queue_ == nullptr;
};
absl::MutexLock locker(&work_queue_mutex_);
while (!work_queue_mutex_.AwaitWithTimeout(
absl::Condition(&predicate),
absl::FromChrono(common::FromSeconds(1.)))) {
report_progress();
}
}
// Now wait for any pending constraint computations to finish.
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
bool notification = false;
constraint_builder_.WhenDone(
[this,
&notification](const constraints::ConstraintBuilder3D::Result& result)
EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
data_.constraints.insert(data_.constraints.end(), result.begin(),
result.end());
notification = true;
});
while (!locker.AwaitWithTimeout([&notification]()
REQUIRES(mutex_) { return notification; },
common::FromSeconds(1.))) {
const auto predicate = [&notification]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return notification;
};
while (!mutex_.AwaitWithTimeout(absl::Condition(&predicate),
absl::FromChrono(common::FromSeconds(1.)))) {
report_progress();
}
CHECK_EQ(constraint_builder_.GetNumFinishedNodes(), num_trajectory_nodes);
@ -570,7 +576,7 @@ void PoseGraph3D::WaitForAllComputations() {
void PoseGraph3D::DeleteTrajectory(const int trajectory_id) {
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
auto it = data_.trajectories_state.find(trajectory_id);
if (it == data_.trajectories_state.end()) {
LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: "
@ -580,8 +586,8 @@ void PoseGraph3D::DeleteTrajectory(const int trajectory_id) {
it->second.deletion_state =
InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION;
}
AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
CHECK(data_.trajectories_state.at(trajectory_id).state !=
TrajectoryState::ACTIVE);
CHECK(data_.trajectories_state.at(trajectory_id).state !=
@ -595,8 +601,8 @@ void PoseGraph3D::DeleteTrajectory(const int trajectory_id) {
}
void PoseGraph3D::FinishTrajectory(const int trajectory_id) {
AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
CHECK(!IsTrajectoryFinished(trajectory_id));
data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED;
@ -615,11 +621,11 @@ bool PoseGraph3D::IsTrajectoryFinished(const int trajectory_id) const {
void PoseGraph3D::FreezeTrajectory(const int trajectory_id) {
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
data_.trajectory_connectivity_state.Add(trajectory_id);
}
AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
CHECK(!IsTrajectoryFrozen(trajectory_id));
data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN;
return WorkItem::Result::kDoNotRunOptimization;
@ -644,7 +650,7 @@ void PoseGraph3D::AddSubmapFromProto(
std::make_shared<const Submap3D>(submap.submap_3d());
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
AddTrajectoryIfNeeded(submap_id.trajectory_id);
if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return;
data_.submap_data.Insert(submap_id, InternalSubmapData());
@ -653,8 +659,8 @@ void PoseGraph3D::AddSubmapFromProto(
data_.global_submap_poses_3d.Insert(
submap_id, optimization::SubmapSpec3D{global_submap_pose});
}
AddWorkItem([this, submap_id, global_submap_pose]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, submap_id, global_submap_pose]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
data_.submap_data.at(submap_id).state = SubmapState::kFinished;
optimization_problem_->InsertSubmap(submap_id, global_submap_pose);
return WorkItem::Result::kDoNotRunOptimization;
@ -669,15 +675,15 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose,
std::make_shared<const TrajectoryNode::Data>(FromProto(node.node_data()));
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
AddTrajectoryIfNeeded(node_id.trajectory_id);
if (!CanAddWorkItemModifying(node_id.trajectory_id)) return;
data_.trajectory_nodes.Insert(node_id,
TrajectoryNode{constant_data, global_pose});
}
AddWorkItem([this, node_id, global_pose]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, node_id, global_pose]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
const auto& constant_data =
data_.trajectory_nodes.at(node_id).constant_data;
optimization_problem_->InsertTrajectoryNode(
@ -701,8 +707,8 @@ void PoseGraph3D::SetTrajectoryDataFromProto(
}
const int trajectory_id = data.trajectory_id();
AddWorkItem([this, trajectory_id, trajectory_data]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, trajectory_id, trajectory_data]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->SetTrajectoryData(trajectory_id, trajectory_data);
}
@ -712,8 +718,8 @@ void PoseGraph3D::SetTrajectoryDataFromProto(
void PoseGraph3D::AddNodeToSubmap(const NodeId& node_id,
const SubmapId& submap_id) {
AddWorkItem([this, node_id, submap_id]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, node_id, submap_id]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(submap_id.trajectory_id)) {
data_.submap_data.at(submap_id).node_ids.insert(node_id);
}
@ -723,8 +729,8 @@ void PoseGraph3D::AddNodeToSubmap(const NodeId& node_id,
void PoseGraph3D::AddSerializedConstraints(
const std::vector<Constraint>& constraints) {
AddWorkItem([this, constraints]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, constraints]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
for (const auto& constraint : constraints) {
CHECK(data_.trajectory_nodes.Contains(constraint.node_id));
CHECK(data_.submap_data.Contains(constraint.submap_id));
@ -751,8 +757,8 @@ void PoseGraph3D::AddSerializedConstraints(
void PoseGraph3D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {
// C++11 does not allow us to move a unique_ptr into a lambda.
PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
AddWorkItem([this, trimmer_ptr]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
trimmers_.emplace_back(trimmer_ptr);
return WorkItem::Result::kDoNotRunOptimization;
});
@ -760,14 +766,14 @@ void PoseGraph3D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {
void PoseGraph3D::RunFinalOptimization() {
{
AddWorkItem([this]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
optimization_problem_->SetMaxNumIterations(
options_.max_num_final_iterations());
return WorkItem::Result::kRunOptimization;
});
AddWorkItem([this]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
optimization_problem_->SetMaxNumIterations(
options_.optimization_problem_options()
.ceres_solver_options()
@ -815,7 +821,7 @@ void PoseGraph3D::RunOptimization() {
// avoid blocking foreground processing.
optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
data_.landmark_nodes);
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
const auto& submap_data = optimization_problem_->submap_data();
const auto& node_data = optimization_problem_->node_data();
@ -884,14 +890,14 @@ bool PoseGraph3D::CanAddWorkItemModifying(int trajectory_id) {
}
MapById<NodeId, TrajectoryNode> PoseGraph3D::GetTrajectoryNodes() const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return data_.trajectory_nodes;
}
MapById<NodeId, TrajectoryNodePose> PoseGraph3D::GetTrajectoryNodePoses()
const {
MapById<NodeId, TrajectoryNodePose> node_poses;
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
for (const auto& node_id_data : data_.trajectory_nodes) {
absl::optional<TrajectoryNodePose::ConstantPoseData> constant_pose_data;
if (node_id_data.data.constant_data != nullptr) {
@ -909,7 +915,7 @@ MapById<NodeId, TrajectoryNodePose> PoseGraph3D::GetTrajectoryNodePoses()
std::map<int, PoseGraphInterface::TrajectoryState>
PoseGraph3D::GetTrajectoryStates() const {
std::map<int, PoseGraphInterface::TrajectoryState> trajectories_state;
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
for (const auto& it : data_.trajectories_state) {
trajectories_state[it.first] = it.second.state;
}
@ -919,7 +925,7 @@ PoseGraph3D::GetTrajectoryStates() const {
std::map<std::string, transform::Rigid3d> PoseGraph3D::GetLandmarkPoses()
const {
std::map<std::string, transform::Rigid3d> landmark_poses;
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
for (const auto& landmark : data_.landmark_nodes) {
// Landmark without value has not been optimized yet.
if (!landmark.second.global_landmark_pose.has_value()) continue;
@ -931,43 +937,43 @@ std::map<std::string, transform::Rigid3d> PoseGraph3D::GetLandmarkPoses()
void PoseGraph3D::SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) {
AddWorkItem([=]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose;
return WorkItem::Result::kDoNotRunOptimization;
});
}
sensor::MapByTime<sensor::ImuData> PoseGraph3D::GetImuData() const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return optimization_problem_->imu_data();
}
sensor::MapByTime<sensor::OdometryData> PoseGraph3D::GetOdometryData() const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return optimization_problem_->odometry_data();
}
sensor::MapByTime<sensor::FixedFramePoseData>
PoseGraph3D::GetFixedFramePoseData() const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return optimization_problem_->fixed_frame_pose_data();
}
std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
PoseGraph3D::GetLandmarkNodes() const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return data_.landmark_nodes;
}
std::map<int, PoseGraphInterface::TrajectoryData>
PoseGraph3D::GetTrajectoryData() const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return optimization_problem_->trajectory_data();
}
std::vector<PoseGraphInterface::Constraint> PoseGraph3D::constraints() const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return data_.constraints;
}
@ -975,7 +981,7 @@ void PoseGraph3D::SetInitialTrajectoryPose(const int from_trajectory_id,
const int to_trajectory_id,
const transform::Rigid3d& pose,
const common::Time time) {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
data_.initial_trajectory_poses[from_trajectory_id] =
InitialTrajectoryPose{to_trajectory_id, pose, time};
}
@ -1003,7 +1009,7 @@ transform::Rigid3d PoseGraph3D::GetInterpolatedGlobalTrajectoryPose(
transform::Rigid3d PoseGraph3D::GetLocalToGlobalTransform(
const int trajectory_id) const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return ComputeLocalToGlobalTransform(data_.global_submap_poses_3d,
trajectory_id);
}
@ -1014,19 +1020,19 @@ std::vector<std::vector<int>> PoseGraph3D::GetConnectedTrajectories() const {
PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapData(
const SubmapId& submap_id) const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return GetSubmapDataUnderLock(submap_id);
}
MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph3D::GetAllSubmapData() const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return GetSubmapDataUnderLock();
}
MapById<SubmapId, PoseGraphInterface::SubmapPose>
PoseGraph3D::GetAllSubmapPoses() const {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
MapById<SubmapId, SubmapPose> submap_poses;
for (const auto& submap_id_data : data_.submap_data) {
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/Geometry"
#include "absl/synchronization/mutex.h"
#include "cartographer/common/fixed_ratio_sampler.h"
#include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/3d/submap_3d.h"
@ -79,26 +79,28 @@ class PoseGraph3D : public PoseGraph {
std::shared_ptr<const TrajectoryNode::Data> constant_data,
int trajectory_id,
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
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
void AddOdometryData(int trajectory_id,
const sensor::OdometryData& odometry_data) override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
void AddFixedFramePoseData(
int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
void AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data) override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
void DeleteTrajectory(int trajectory_id) override;
void FinishTrajectory(int trajectory_id) override;
bool IsTrajectoryFinished(int trajectory_id) const override REQUIRES(mutex_);
bool IsTrajectoryFinished(int trajectory_id) const override
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
void FreezeTrajectory(int trajectory_id) override;
bool IsTrajectoryFrozen(int trajectory_id) const override REQUIRES(mutex_);
bool IsTrajectoryFrozen(int trajectory_id) const override
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
const proto::Submap& submap) override;
void AddNodeFromProto(const transform::Rigid3d& global_pose,
@ -112,128 +114,134 @@ class PoseGraph3D : public PoseGraph {
void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() const override;
PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id) const
EXCLUDES(mutex_) override;
LOCKS_EXCLUDED(mutex_) override;
MapById<SubmapId, SubmapData> GetAllSubmapData() const
EXCLUDES(mutex_) override;
LOCKS_EXCLUDED(mutex_) override;
MapById<SubmapId, SubmapPose> GetAllSubmapPoses() const
EXCLUDES(mutex_) override;
LOCKS_EXCLUDED(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) const
EXCLUDES(mutex_) override;
LOCKS_EXCLUDED(mutex_) override;
MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
std::map<int, TrajectoryState> GetTrajectoryStates() const override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
void SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
sensor::MapByTime<sensor::ImuData> GetImuData() const override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
sensor::MapByTime<sensor::OdometryData> GetOdometryData() const override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData()
const override EXCLUDES(mutex_);
const override LOCKS_EXCLUDED(mutex_);
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::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,
const transform::Rigid3d& pose,
const common::Time time) override
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
void SetGlobalSlamOptimizationCallback(
PoseGraphInterface::GlobalSlamOptimizationCallback callback) override;
transform::Rigid3d GetInterpolatedGlobalTrajectoryPose(
int trajectory_id, const common::Time time) const REQUIRES(mutex_);
int trajectory_id, const common::Time time) const
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
static void RegisterMetrics(metrics::FamilyFactory* family_factory);
protected:
// Waits until we caught up (i.e. nothing is waiting to be scheduled), and
// all computations have finished.
void WaitForAllComputations() EXCLUDES(mutex_) EXCLUDES(work_queue_mutex_);
void WaitForAllComputations() LOCKS_EXCLUDED(mutex_)
LOCKS_EXCLUDED(work_queue_mutex_);
private:
MapById<SubmapId, SubmapData> GetSubmapDataUnderLock() const REQUIRES(mutex_);
MapById<SubmapId, SubmapData> GetSubmapDataUnderLock() const
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Handles a new 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.
void AddTrajectoryIfNeeded(int trajectory_id) REQUIRES(mutex_);
void AddTrajectoryIfNeeded(int trajectory_id)
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Appends the new node and submap (if needed) to the internal data stuctures.
NodeId AppendNode(
std::shared_ptr<const TrajectoryNode::Data> constant_data,
int trajectory_id,
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
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
std::vector<SubmapId> InitializeGlobalSubmapPoses(
int trajectory_id, const common::Time time,
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.
WorkItem::Result ComputeConstraintsForNode(
const NodeId& node_id,
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.
void ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id)
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
// Deletes trajectories waiting for deletion. Must not be called during
// constraint search.
void DeleteTrajectoriesIfNeeded() REQUIRES(mutex_);
void DeleteTrajectoriesIfNeeded() EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Runs the optimization, executes the trimmers and processes the work queue.
void HandleWorkQueue(const constraints::ConstraintBuilder3D::Result& result)
EXCLUDES(mutex_) EXCLUDES(work_queue_mutex_);
LOCKS_EXCLUDED(mutex_) LOCKS_EXCLUDED(work_queue_mutex_);
// Process pending tasks in the work queue on the calling thread, until the
// queue is either empty or an optimization is required.
void DrainWorkQueue() EXCLUDES(mutex_) EXCLUDES(work_queue_mutex_);
void DrainWorkQueue() LOCKS_EXCLUDED(mutex_)
LOCKS_EXCLUDED(work_queue_mutex_);
// Runs the optimization. Callers have to make sure, that there is only one
// optimization being run at a time.
void RunOptimization() EXCLUDES(mutex_);
void RunOptimization() LOCKS_EXCLUDED(mutex_);
bool CanAddWorkItemModifying(int trajectory_id) REQUIRES(mutex_);
bool CanAddWorkItemModifying(int trajectory_id)
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Computes the local to global map frame transform based on the given
// 'global_submap_poses'.
transform::Rigid3d ComputeLocalToGlobalTransform(
const MapById<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
REQUIRES(mutex_);
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
common::Time GetLatestNodeTime(const NodeId& node_id,
const SubmapId& submap_id) const
REQUIRES(mutex_);
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Logs histograms for the translational and rotational residual of node
// poses.
void LogResidualHistograms() const REQUIRES(mutex_);
void LogResidualHistograms() const EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Updates the trajectory connectivity structure with a new constraint.
void UpdateTrajectoryConnectivity(const Constraint& constraint)
REQUIRES(mutex_);
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
const proto::PoseGraphOptions options_;
GlobalSlamOptimizationCallback global_slam_optimization_callback_;
mutable common::Mutex mutex_;
common::Mutex work_queue_mutex_;
mutable absl::Mutex mutex_;
absl::Mutex work_queue_mutex_;
// If it exists, further work items must be added to this queue, and will be
// considered later.
@ -268,17 +276,18 @@ class PoseGraph3D : public PoseGraph {
int num_submaps(int trajectory_id) const override;
std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
MapById<SubmapId, SubmapData> GetOptimizedSubmapData() const override
REQUIRES(parent_->mutex_);
EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
REQUIRES(parent_->mutex_);
EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
const std::vector<Constraint>& GetConstraints() const override
REQUIRES(parent_->mutex_);
EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
void TrimSubmap(const SubmapId& submap_id)
REQUIRES(parent_->mutex_) override;
bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_);
EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_) override;
bool IsFinished(int trajectory_id) const override
EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
void SetTrajectoryState(int trajectory_id, TrajectoryState state) override
REQUIRES(parent_->mutex_);
EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
private:
PoseGraph3D* const parent_;

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

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

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

@ -20,7 +20,7 @@
#include <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/submaps.h"
@ -41,38 +41,40 @@ class ConnectedComponents {
ConnectedComponents& operator=(const ConnectedComponents&) = delete;
// Add a trajectory which is initially connected to only itself.
void Add(int trajectory_id) EXCLUDES(lock_);
void Add(int trajectory_id) LOCKS_EXCLUDED(lock_);
// Connect two trajectories. If either trajectory is untracked, it will be
// tracked. This function is invariant to the order of its arguments. Repeated
// calls to Connect increment the connectivity count.
void Connect(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_);
void Connect(int trajectory_id_a, int trajectory_id_b) LOCKS_EXCLUDED(lock_);
// Determines if two trajectories have been (transitively) connected. If
// either trajectory is not being tracked, returns false, except when it is
// the same trajectory, where it returns true. This function is invariant to
// the order of its arguments.
bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b)
EXCLUDES(lock_);
LOCKS_EXCLUDED(lock_);
// Return the number of _direct_ connections between 'trajectory_id_a' and
// 'trajectory_id_b'. If either trajectory is not being tracked, returns 0.
// This function is invariant to the order of its arguments.
int ConnectionCount(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_);
int ConnectionCount(int trajectory_id_a, int trajectory_id_b)
LOCKS_EXCLUDED(lock_);
// The trajectory IDs, grouped by connectivity.
std::vector<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
// 'trajectory_id'.
std::vector<int> GetComponent(int trajectory_id) EXCLUDES(lock_);
std::vector<int> GetComponent(int trajectory_id) LOCKS_EXCLUDED(lock_);
private:
// Find the representative and compresses the path to it.
int FindSet(int trajectory_id) REQUIRES(lock_);
void Union(int trajectory_id_a, int trajectory_id_b) REQUIRES(lock_);
int FindSet(int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(lock_);
void Union(int trajectory_id_a, int trajectory_id_b)
EXCLUSIVE_LOCKS_REQUIRED(lock_);
common::Mutex lock_;
absl::Mutex lock_;
// Tracks transitive connectivity using a disjoint set forest, i.e. each
// entry points towards the representative for the given trajectory.
std::map<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()) {}
ConstraintBuilder2D::~ConstraintBuilder2D() {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
CHECK_EQ(finish_node_task_->GetState(), common::Task::NEW);
CHECK_EQ(when_done_task_->GetState(), common::Task::NEW);
CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called";
@ -82,7 +82,7 @@ void ConstraintBuilder2D::MaybeAddConstraint(
}
if (!sampler_.Pulse()) return;
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
if (when_done_) {
LOG(WARNING)
<< "MaybeAddConstraint was called while WhenDone was scheduled.";
@ -93,7 +93,7 @@ void ConstraintBuilder2D::MaybeAddConstraint(
const auto* scan_matcher =
DispatchScanMatcherConstruction(submap_id, submap->grid());
auto constraint_task = absl::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) {
constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */
constant_data, initial_relative_pose, *scan_matcher,
constraint);
@ -107,7 +107,7 @@ void ConstraintBuilder2D::MaybeAddConstraint(
void ConstraintBuilder2D::MaybeAddGlobalConstraint(
const SubmapId& submap_id, const Submap2D* const submap,
const NodeId& node_id, const TrajectoryNode::Data* const constant_data) {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
if (when_done_) {
LOG(WARNING)
<< "MaybeAddGlobalConstraint was called while WhenDone was scheduled.";
@ -118,7 +118,7 @@ void ConstraintBuilder2D::MaybeAddGlobalConstraint(
const auto* scan_matcher =
DispatchScanMatcherConstruction(submap_id, submap->grid());
auto constraint_task = absl::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) {
constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */
constant_data, transform::Rigid2d::Identity(),
*scan_matcher, constraint);
@ -130,10 +130,10 @@ void ConstraintBuilder2D::MaybeAddGlobalConstraint(
}
void ConstraintBuilder2D::NotifyEndOfNode() {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
CHECK(finish_node_task_ != nullptr);
finish_node_task_->SetWorkItem([this] {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
++num_finished_nodes_;
});
auto finish_node_task_handle =
@ -145,7 +145,7 @@ void ConstraintBuilder2D::NotifyEndOfNode() {
void ConstraintBuilder2D::WhenDone(
const std::function<void(const ConstraintBuilder2D::Result&)>& callback) {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
CHECK(when_done_ == nullptr);
// TODO(gaschler): Consider using just std::function, it can also be empty.
when_done_ = absl::make_unique<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);
}
@ -272,7 +272,7 @@ void ConstraintBuilder2D::RunWhenDoneCallback() {
Result result;
std::unique_ptr<std::function<void(const Result&)>> callback;
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
CHECK(when_done_ != nullptr);
for (const std::unique_ptr<Constraint>& constraint : constraints_) {
if (constraint == nullptr) continue;
@ -292,12 +292,12 @@ void ConstraintBuilder2D::RunWhenDoneCallback() {
}
int ConstraintBuilder2D::GetNumFinishedNodes() {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return num_finished_nodes_;
}
void ConstraintBuilder2D::DeleteScanMatcher(const SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
if (when_done_) {
LOG(WARNING)
<< "DeleteScanMatcher was called while WhenDone was scheduled.";

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

@ -25,10 +25,10 @@
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "absl/synchronization/mutex.h"
#include "cartographer/common/fixed_ratio_sampler.h"
#include "cartographer/common/histogram.h"
#include "cartographer/common/math.h"
#include "cartographer/common/mutex.h"
#include "cartographer/common/task.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/mapping/2d/submap_2d.h"
@ -116,7 +116,8 @@ class ConstraintBuilder2D {
// The returned 'grid' and 'fast_correlative_scan_matcher' must only be
// accessed after 'creation_task_handle' has completed.
const SubmapScanMatcher* DispatchScanMatcherConstruction(
const SubmapId& submap_id, const Grid2D* grid) REQUIRES(mutex_);
const SubmapId& submap_id, const Grid2D* grid)
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Runs in a background thread and does computations for an additional
// constraint, assuming 'submap' and 'compressed_point_cloud' do not change
@ -127,13 +128,13 @@ class ConstraintBuilder2D {
const transform::Rigid2d& initial_relative_pose,
const SubmapScanMatcher& submap_scan_matcher,
std::unique_ptr<Constraint>* constraint)
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
void RunWhenDoneCallback() EXCLUDES(mutex_);
void RunWhenDoneCallback() LOCKS_EXCLUDED(mutex_);
const constraints::proto::ConstraintBuilderOptions options_;
common::ThreadPoolInterface* thread_pool_;
common::Mutex mutex_;
absl::Mutex mutex_;
// 'callback' set by WhenDone().
std::unique_ptr<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()) {}
ConstraintBuilder3D::~ConstraintBuilder3D() {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
CHECK_EQ(finish_node_task_->GetState(), common::Task::NEW);
CHECK_EQ(when_done_task_->GetState(), common::Task::NEW);
CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called";
@ -86,7 +86,7 @@ void ConstraintBuilder3D::MaybeAddConstraint(
}
if (!sampler_.Pulse()) return;
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
if (when_done_) {
LOG(WARNING)
<< "MaybeAddConstraint was called while WhenDone was scheduled.";
@ -97,7 +97,7 @@ void ConstraintBuilder3D::MaybeAddConstraint(
const auto* scan_matcher =
DispatchScanMatcherConstruction(submap_id, submap_nodes, submap);
auto constraint_task = absl::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) {
constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
ComputeConstraint(submap_id, node_id, false, /* match_full_submap */
constant_data, global_node_pose, global_submap_pose,
*scan_matcher, constraint);
@ -114,7 +114,7 @@ void ConstraintBuilder3D::MaybeAddGlobalConstraint(
const std::vector<TrajectoryNode>& submap_nodes,
const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation) {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
if (when_done_) {
LOG(WARNING)
<< "MaybeAddGlobalConstraint was called while WhenDone was scheduled.";
@ -125,7 +125,7 @@ void ConstraintBuilder3D::MaybeAddGlobalConstraint(
const auto* scan_matcher =
DispatchScanMatcherConstruction(submap_id, submap_nodes, submap);
auto constraint_task = absl::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) {
constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
ComputeConstraint(submap_id, node_id, true, /* match_full_submap */
constant_data,
transform::Rigid3d::Rotation(global_node_rotation),
@ -139,10 +139,10 @@ void ConstraintBuilder3D::MaybeAddGlobalConstraint(
}
void ConstraintBuilder3D::NotifyEndOfNode() {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
CHECK(finish_node_task_ != nullptr);
finish_node_task_->SetWorkItem([this] {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
++num_finished_nodes_;
});
auto finish_node_task_handle =
@ -154,7 +154,7 @@ void ConstraintBuilder3D::NotifyEndOfNode() {
void ConstraintBuilder3D::WhenDone(
const std::function<void(const ConstraintBuilder3D::Result&)>& callback) {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
CHECK(when_done_ == nullptr);
// TODO(gaschler): Consider using just std::function, it can also be empty.
when_done_ = absl::make_unique<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);
rotational_score_histogram_.Add(match_result->rotational_score);
low_resolution_score_histogram_.Add(match_result->low_resolution_score);
@ -301,7 +301,7 @@ void ConstraintBuilder3D::RunWhenDoneCallback() {
Result result;
std::unique_ptr<std::function<void(const Result&)>> callback;
{
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
CHECK(when_done_ != nullptr);
for (const std::unique_ptr<Constraint>& constraint : constraints_) {
if (constraint == nullptr) continue;
@ -326,12 +326,12 @@ void ConstraintBuilder3D::RunWhenDoneCallback() {
}
int ConstraintBuilder3D::GetNumFinishedNodes() {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return num_finished_nodes_;
}
void ConstraintBuilder3D::DeleteScanMatcher(const SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
if (when_done_) {
LOG(WARNING)
<< "DeleteScanMatcher was called while WhenDone was scheduled.";

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

@ -25,11 +25,11 @@
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "absl/synchronization/mutex.h"
#include "cartographer/common/fixed_ratio_sampler.h"
#include "cartographer/common/histogram.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/math.h"
#include "cartographer/common/mutex.h"
#include "cartographer/common/task.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/mapping/3d/submap_3d.h"
@ -128,7 +128,7 @@ class ConstraintBuilder3D {
const SubmapScanMatcher* DispatchScanMatcherConstruction(
const SubmapId& submap_id,
const std::vector<TrajectoryNode>& submap_nodes, const Submap3D* submap)
REQUIRES(mutex_);
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Runs in a background thread and does computations for an additional
// constraint.
@ -140,13 +140,13 @@ class ConstraintBuilder3D {
const transform::Rigid3d& global_submap_pose,
const SubmapScanMatcher& submap_scan_matcher,
std::unique_ptr<Constraint>* constraint)
EXCLUDES(mutex_);
LOCKS_EXCLUDED(mutex_);
void RunWhenDoneCallback() EXCLUDES(mutex_);
void RunWhenDoneCallback() LOCKS_EXCLUDED(mutex_);
const proto::ConstraintBuilderOptions options_;
common::ThreadPoolInterface* thread_pool_;
common::Mutex mutex_;
absl::Mutex mutex_;
// 'callback' set by WhenDone().
std::unique_ptr<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 {
void PoseGraphController::AddNode(const proto::Node& node) {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
AddNodeToPoseGraphData(node, &data_);
}
void PoseGraphController::AddConstraint(const proto::Constraint& constraint) {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
AddConstraintToPoseGraphData(constraint, &data_);
}
Optimizer::SolverStatus PoseGraphController::Optimize() {
common::MutexLocker locker(&mutex_);
absl::MutexLock locker(&mutex_);
return optimizer_->Solve(&data_);
}

11
cartographer/pose_graph/pose_graph_controller.h
View File

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