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

Don't process PoseGraph3D work items in foreground (#1285) 

This helps the foreground thread in keeping up real time.
Also see issue #1250.
master
danielsievers 2018-07-20 14:41:57 +02:00 committed by Wally B. Feed
parent 5077224f8e
commit 2de0e5f04b
2 changed files with 26 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -47,7 +47,8 @@ PoseGraph3D::PoseGraph3D(
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) {}
PoseGraph3D::~PoseGraph3D() { PoseGraph3D::~PoseGraph3D() {
WaitForAllComputations(); WaitForAllComputations();
@ -152,26 +153,19 @@ NodeId PoseGraph3D::AddNode(
void PoseGraph3D::AddWorkItem( void PoseGraph3D::AddWorkItem(
const std::function<WorkItem::Result()>& work_item) { const std::function<WorkItem::Result()>& work_item) {
{ common::MutexLocker locker(&work_queue_mutex_);
common::MutexLocker locker(&work_queue_mutex_); if (work_queue_ == nullptr) {
if (work_queue_ != nullptr) { work_queue_ = common::make_unique<WorkQueue>();
const auto now = std::chrono::steady_clock::now(); auto task = common::make_unique<common::Task>();
work_queue_->push_back({now, work_item}); task->SetWorkItem([this]() { DrainWorkQueue(); });
kWorkQueueDelayMetric->Set( thread_pool_->Schedule(std::move(task));
common::ToSeconds(now - work_queue_->front().time));
return;
}
}
if (work_item() == WorkItem::Result::kRunOptimization) {
{
common::MutexLocker locker(&work_queue_mutex_);
work_queue_ = common::make_unique<WorkQueue>();
}
constraint_builder_.WhenDone(
[this](const constraints::ConstraintBuilder3D::Result& result) {
HandleWorkQueue(result);
});
} }
const auto now = std::chrono::steady_clock::now();
work_queue_->push_back({now, work_item});
kWorkQueueDelayMetric->Set(
std::chrono::duration_cast<std::chrono::duration<double>>(
now - work_queue_->front().time)
.count());
} }
void PoseGraph3D::AddTrajectoryIfNeeded(const int trajectory_id) { void PoseGraph3D::AddTrajectoryIfNeeded(const int trajectory_id) {
@ -484,8 +478,12 @@ void PoseGraph3D::HandleWorkQueue(
num_nodes_since_last_loop_closure_ = 0; num_nodes_since_last_loop_closure_ = 0;
} }
size_t work_queue_size; DrainWorkQueue();
}
void PoseGraph3D::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/3d/pose_graph_3d.h
View File

@ -199,6 +199,10 @@ class PoseGraph3D : public PoseGraph {
void HandleWorkQueue(const constraints::ConstraintBuilder3D::Result& result) void HandleWorkQueue(const constraints::ConstraintBuilder3D::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_);
// Runs the optimization. Callers have to make sure, that there is only one // Runs the optimization. Callers have to make sure, that there is only one
// optimization being run at a time. // optimization being run at a time.
void RunOptimization() EXCLUDES(mutex_); void RunOptimization() EXCLUDES(mutex_);
@ -246,6 +250,9 @@ class PoseGraph3D : public PoseGraph {
std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem_; std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem_;
constraints::ConstraintBuilder3D constraint_builder_; constraints::ConstraintBuilder3D 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_);