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

Don't process PoseGraph2D work items in foreground. (#1341) 

This is #1285 for 2D.
Fixes #1250.
master
Wolfgang Hess 2018-07-26 17:05:23 +02:00 committed by Wally B. Feed
parent c2c341397f
commit 8b329efc8e
2 changed files with 26 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -48,7 +48,8 @@ PoseGraph2D::PoseGraph2D(
common::ThreadPool* thread_pool) common::ThreadPool* thread_pool)
: options_(options), : options_(options),
optimization_problem_(std::move(optimization_problem)), optimization_problem_(std::move(optimization_problem)),
constraint_builder_(options_.constraint_builder_options(), thread_pool) {} constraint_builder_(options_.constraint_builder_options(), thread_pool),
thread_pool_(thread_pool) {}
PoseGraph2D::~PoseGraph2D() { PoseGraph2D::~PoseGraph2D() {
WaitForAllComputations(); WaitForAllComputations();
@ -155,26 +156,19 @@ NodeId PoseGraph2D::AddNode(
void PoseGraph2D::AddWorkItem( void PoseGraph2D::AddWorkItem(
const std::function<WorkItem::Result()>& work_item) { const std::function<WorkItem::Result()>& work_item) {
{
common::MutexLocker locker(&work_queue_mutex_); common::MutexLocker locker(&work_queue_mutex_);
if (work_queue_ != nullptr) { if (work_queue_ == nullptr) {
work_queue_ = common::make_unique<WorkQueue>();
auto task = common::make_unique<common::Task>();
task->SetWorkItem([this]() { DrainWorkQueue(); });
thread_pool_->Schedule(std::move(task));
}
const auto now = std::chrono::steady_clock::now(); const auto now = std::chrono::steady_clock::now();
work_queue_->push_back({now, work_item}); work_queue_->push_back({now, work_item});
kWorkQueueDelayMetric->Set( kWorkQueueDelayMetric->Set(
common::ToSeconds(now - work_queue_->front().time)); std::chrono::duration_cast<std::chrono::duration<double>>(
return; now - work_queue_->front().time)
} .count());
}
if (work_item() == WorkItem::Result::kRunOptimization) {
{
common::MutexLocker locker(&work_queue_mutex_);
work_queue_ = common::make_unique<WorkQueue>();
}
constraint_builder_.WhenDone(
[this](const constraints::ConstraintBuilder2D::Result& result) {
HandleWorkQueue(result);
});
}
} }
void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) { void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) {
@ -469,8 +463,12 @@ void PoseGraph2D::HandleWorkQueue(
num_nodes_since_last_loop_closure_ = 0; num_nodes_since_last_loop_closure_ = 0;
} }
size_t work_queue_size; DrainWorkQueue();
}
void PoseGraph2D::DrainWorkQueue() {
bool process_work_queue = true; bool process_work_queue = true;
size_t work_queue_size;
while (process_work_queue) { while (process_work_queue) {
std::function<WorkItem::Result()> work_item; std::function<WorkItem::Result()> work_item;
{ {

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

@ -198,6 +198,10 @@ class PoseGraph2D : public PoseGraph {
void HandleWorkQueue(const constraints::ConstraintBuilder2D::Result& result) void HandleWorkQueue(const constraints::ConstraintBuilder2D::Result& result)
EXCLUDES(mutex_) EXCLUDES(work_queue_mutex_); EXCLUDES(mutex_) EXCLUDES(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_);
// Waits until we caught up (i.e. nothing is waiting to be scheduled), and // Waits until we caught up (i.e. nothing is waiting to be scheduled), and
// all computations have finished. // all computations have finished.
void WaitForAllComputations() EXCLUDES(mutex_) EXCLUDES(work_queue_mutex_); void WaitForAllComputations() EXCLUDES(mutex_) EXCLUDES(work_queue_mutex_);
@ -245,6 +249,9 @@ class PoseGraph2D : public PoseGraph {
std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_; std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_;
constraints::ConstraintBuilder2D constraint_builder_; constraints::ConstraintBuilder2D constraint_builder_;
// Thread pool used for handling the work queue.
common::ThreadPool* const thread_pool_;
// List of all trimmers to consult when optimizations finish. // List of all trimmers to consult when optimizations finish.
std::vector<std::unique_ptr<PoseGraphTrimmer>> trimmers_ GUARDED_BY(mutex_); std::vector<std::unique_ptr<PoseGraphTrimmer>> trimmers_ GUARDED_BY(mutex_);