Add function to get the work queue delay from PoseGraph. (#1218)

master
Arno Fleck 2018-07-06 09:40:44 +02:00 committed by Christoph Schütte
parent bad8c96bc6
commit 627ebb39b9
7 changed files with 86 additions and 10 deletions

View File

@ -39,6 +39,8 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
static auto* kWorkQueueDelayMetric = metrics::Gauge::Null();
PoseGraph2D::PoseGraph2D( PoseGraph2D::PoseGraph2D(
const proto::PoseGraphOptions& options, const proto::PoseGraphOptions& options,
std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem, std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,
@ -145,7 +147,12 @@ void PoseGraph2D::AddWorkItem(const std::function<void()>& work_item) {
if (work_queue_ == nullptr) { if (work_queue_ == nullptr) {
work_item(); work_item();
} else { } else {
work_queue_->push_back(work_item); 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());
} }
} }
@ -323,7 +330,7 @@ void PoseGraph2D::DispatchOptimization() {
run_loop_closure_ = true; run_loop_closure_ = true;
// If there is a 'work_queue_' already, some other thread will take care. // If there is a 'work_queue_' already, some other thread will take care.
if (work_queue_ == nullptr) { if (work_queue_ == nullptr) {
work_queue_ = common::make_unique<std::deque<std::function<void()>>>(); work_queue_ = common::make_unique<WorkQueue>();
constraint_builder_.WhenDone( constraint_builder_.WhenDone(
std::bind(&PoseGraph2D::HandleWorkQueue, this, std::placeholders::_1)); std::bind(&PoseGraph2D::HandleWorkQueue, this, std::placeholders::_1));
} }
@ -425,7 +432,7 @@ void PoseGraph2D::HandleWorkQueue(
work_queue_.reset(); work_queue_.reset();
return; return;
} }
work_queue_->front()(); work_queue_->front().task();
work_queue_->pop_front(); work_queue_->pop_front();
} }
LOG(INFO) << "Remaining work items in queue: " << work_queue_->size(); LOG(INFO) << "Remaining work items in queue: " << work_queue_->size();
@ -1077,5 +1084,12 @@ void PoseGraph2D::SetGlobalSlamOptimizationCallback(
global_slam_optimization_callback_ = callback; global_slam_optimization_callback_ = callback;
} }
void PoseGraph2D::RegisterMetrics(metrics::FamilyFactory* family_factory) {
auto* latency = family_factory->NewGaugeFamily(
"mapping_2d_pose_graph_work_queue_delay",
"Age of the oldest entry in the work queue in seconds");
kWorkQueueDelayMetric = latency->Add({});
}
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer

View File

@ -36,9 +36,11 @@
#include "cartographer/mapping/internal/constraints/constraint_builder_2d.h" #include "cartographer/mapping/internal/constraints/constraint_builder_2d.h"
#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h" #include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
#include "cartographer/mapping/internal/trajectory_connectivity_state.h" #include "cartographer/mapping/internal/trajectory_connectivity_state.h"
#include "cartographer/mapping/internal/work_queue.h"
#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph_data.h" #include "cartographer/mapping/pose_graph_data.h"
#include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/mapping/pose_graph_trimmer.h"
#include "cartographer/metrics/family_factory.h"
#include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/landmark_data.h"
#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/odometry_data.h"
@ -149,6 +151,8 @@ class PoseGraph2D : public PoseGraph {
transform::Rigid3d GetInterpolatedGlobalTrajectoryPose( transform::Rigid3d GetInterpolatedGlobalTrajectoryPose(
int trajectory_id, const common::Time time) const REQUIRES(mutex_); int trajectory_id, const common::Time time) const REQUIRES(mutex_);
static void RegisterMetrics(metrics::FamilyFactory* family_factory);
private: private:
MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock() MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock()
const REQUIRES(mutex_); const REQUIRES(mutex_);
@ -221,8 +225,7 @@ class PoseGraph2D : public PoseGraph {
// If it exists, further work items must be added to this queue, and will be // If it exists, further work items must be added to this queue, and will be
// considered later. // considered later.
std::unique_ptr<std::deque<std::function<void()>>> work_queue_ std::unique_ptr<WorkQueue> work_queue_ GUARDED_BY(mutex_);
GUARDED_BY(mutex_);
// We globally localize a fraction of the nodes from each trajectory. // We globally localize a fraction of the nodes from each trajectory.
std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>> std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>

View File

@ -39,6 +39,8 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
static auto* kWorkQueueDelayMetric = metrics::Gauge::Null();
PoseGraph3D::PoseGraph3D( PoseGraph3D::PoseGraph3D(
const proto::PoseGraphOptions& options, const proto::PoseGraphOptions& options,
std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem, std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
@ -143,7 +145,12 @@ void PoseGraph3D::AddWorkItem(const std::function<void()>& work_item) {
if (work_queue_ == nullptr) { if (work_queue_ == nullptr) {
work_item(); work_item();
} else { } else {
work_queue_->push_back(work_item); 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());
} }
} }
@ -339,7 +346,7 @@ void PoseGraph3D::DispatchOptimization() {
run_loop_closure_ = true; run_loop_closure_ = true;
// If there is a 'work_queue_' already, some other thread will take care. // If there is a 'work_queue_' already, some other thread will take care.
if (work_queue_ == nullptr) { if (work_queue_ == nullptr) {
work_queue_ = common::make_unique<std::deque<std::function<void()>>>(); work_queue_ = common::make_unique<WorkQueue>();
constraint_builder_.WhenDone( constraint_builder_.WhenDone(
std::bind(&PoseGraph3D::HandleWorkQueue, this, std::placeholders::_1)); std::bind(&PoseGraph3D::HandleWorkQueue, this, std::placeholders::_1));
} }
@ -442,7 +449,7 @@ void PoseGraph3D::HandleWorkQueue(
work_queue_.reset(); work_queue_.reset();
return; return;
} }
work_queue_->front()(); work_queue_->front().task();
work_queue_->pop_front(); work_queue_->pop_front();
} }
LOG(INFO) << "Remaining work items in queue: " << work_queue_->size(); LOG(INFO) << "Remaining work items in queue: " << work_queue_->size();
@ -1103,5 +1110,12 @@ void PoseGraph3D::SetGlobalSlamOptimizationCallback(
global_slam_optimization_callback_ = callback; global_slam_optimization_callback_ = callback;
} }
void PoseGraph3D::RegisterMetrics(metrics::FamilyFactory* family_factory) {
auto* latency = family_factory->NewGaugeFamily(
"mapping_3d_pose_graph_work_queue_delay",
"Age of the oldest entry in the work queue in seconds");
kWorkQueueDelayMetric = latency->Add({});
}
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer

View File

@ -36,9 +36,11 @@
#include "cartographer/mapping/internal/constraints/constraint_builder_3d.h" #include "cartographer/mapping/internal/constraints/constraint_builder_3d.h"
#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h" #include "cartographer/mapping/internal/optimization/optimization_problem_3d.h"
#include "cartographer/mapping/internal/trajectory_connectivity_state.h" #include "cartographer/mapping/internal/trajectory_connectivity_state.h"
#include "cartographer/mapping/internal/work_queue.h"
#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph_data.h" #include "cartographer/mapping/pose_graph_data.h"
#include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/mapping/pose_graph_trimmer.h"
#include "cartographer/metrics/family_factory.h"
#include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/landmark_data.h"
#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/odometry_data.h"
@ -148,6 +150,8 @@ class PoseGraph3D : public PoseGraph {
transform::Rigid3d GetInterpolatedGlobalTrajectoryPose( transform::Rigid3d GetInterpolatedGlobalTrajectoryPose(
int trajectory_id, const common::Time time) const REQUIRES(mutex_); int trajectory_id, const common::Time time) const REQUIRES(mutex_);
static void RegisterMetrics(metrics::FamilyFactory* family_factory);
protected: protected:
// Waits until we caught up (i.e. nothing is waiting to be scheduled), and // Waits until we caught up (i.e. nothing is waiting to be scheduled), and
// all computations have finished. // all computations have finished.
@ -227,8 +231,7 @@ class PoseGraph3D : public PoseGraph {
// If it exists, further work items must be added to this queue, and will be // If it exists, further work items must be added to this queue, and will be
// considered later. // considered later.
std::unique_ptr<std::deque<std::function<void()>>> work_queue_ std::unique_ptr<WorkQueue> work_queue_ GUARDED_BY(mutex_);
GUARDED_BY(mutex_);
// We globally localize a fraction of the nodes from each trajectory. // We globally localize a fraction of the nodes from each trajectory.
std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>> std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>

View File

@ -0,0 +1,37 @@
/*
* Copyright 2018 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_MAPPING_INTERNAL_WORK_QUEUE_H
#define CARTOGRAPHER_MAPPING_INTERNAL_WORK_QUEUE_H
#include <chrono>
#include <deque>
#include <functional>
namespace cartographer {
namespace mapping {
struct WorkItem {
std::chrono::steady_clock::time_point time;
std::function<void()> task;
};
using WorkQueue = std::deque<WorkItem>;
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_WORK_QUEUE_H

View File

@ -17,6 +17,7 @@
#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_INTERFACE_H_ #ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_INTERFACE_H_
#define CARTOGRAPHER_MAPPING_POSE_GRAPH_INTERFACE_H_ #define CARTOGRAPHER_MAPPING_POSE_GRAPH_INTERFACE_H_
#include <chrono>
#include <vector> #include <vector>
#include "cartographer/common/optional.h" #include "cartographer/common/optional.h"

View File

@ -17,7 +17,9 @@
#include "cartographer/metrics/register.h" #include "cartographer/metrics/register.h"
#include "cartographer/mapping/internal/2d/local_trajectory_builder_2d.h" #include "cartographer/mapping/internal/2d/local_trajectory_builder_2d.h"
#include "cartographer/mapping/internal/2d/pose_graph_2d.h"
#include "cartographer/mapping/internal/3d/local_trajectory_builder_3d.h" #include "cartographer/mapping/internal/3d/local_trajectory_builder_3d.h"
#include "cartographer/mapping/internal/3d/pose_graph_3d.h"
#include "cartographer/mapping/internal/constraints/constraint_builder_2d.h" #include "cartographer/mapping/internal/constraints/constraint_builder_2d.h"
#include "cartographer/mapping/internal/constraints/constraint_builder_3d.h" #include "cartographer/mapping/internal/constraints/constraint_builder_3d.h"
#include "cartographer/mapping/internal/global_trajectory_builder.h" #include "cartographer/mapping/internal/global_trajectory_builder.h"
@ -31,6 +33,8 @@ void RegisterAllMetrics(FamilyFactory* registry) {
mapping::GlobalTrajectoryBuilderRegisterMetrics(registry); mapping::GlobalTrajectoryBuilderRegisterMetrics(registry);
mapping::LocalTrajectoryBuilder2D::RegisterMetrics(registry); mapping::LocalTrajectoryBuilder2D::RegisterMetrics(registry);
mapping::LocalTrajectoryBuilder3D::RegisterMetrics(registry); mapping::LocalTrajectoryBuilder3D::RegisterMetrics(registry);
mapping::PoseGraph2D::RegisterMetrics(registry);
mapping::PoseGraph3D::RegisterMetrics(registry);
} }
} // namespace metrics } // namespace metrics