Don't process PoseGraph3D work items in foreground (#1285)
This helps the foreground thread in keeping up real time. Also see issue #1250.master
parent
5077224f8e
commit
2de0e5f04b
|
@ -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>();
|
||||||
|
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::ConstraintBuilder3D::Result& result) {
|
|
||||||
HandleWorkQueue(result);
|
|
||||||
});
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
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;
|
||||||
{
|
{
|
||||||
|
|
|
@ -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_);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue