From 627ebb39b9d58390118b2f9507695fb6bac4103f Mon Sep 17 00:00:00 2001 From: Arno Fleck <38908098+CodeArno@users.noreply.github.com> Date: Fri, 6 Jul 2018 09:40:44 +0200 Subject: [PATCH] Add function to get the work queue delay from PoseGraph. (#1218) --- .../mapping/internal/2d/pose_graph_2d.cc | 20 ++++++++-- .../mapping/internal/2d/pose_graph_2d.h | 7 +++- .../mapping/internal/3d/pose_graph_3d.cc | 20 ++++++++-- .../mapping/internal/3d/pose_graph_3d.h | 7 +++- cartographer/mapping/internal/work_queue.h | 37 +++++++++++++++++++ cartographer/mapping/pose_graph_interface.h | 1 + cartographer/metrics/register.cc | 4 ++ 7 files changed, 86 insertions(+), 10 deletions(-) create mode 100644 cartographer/mapping/internal/work_queue.h diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 4a94279..141c646 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -39,6 +39,8 @@ namespace cartographer { namespace mapping { +static auto* kWorkQueueDelayMetric = metrics::Gauge::Null(); + PoseGraph2D::PoseGraph2D( const proto::PoseGraphOptions& options, std::unique_ptr optimization_problem, @@ -145,7 +147,12 @@ void PoseGraph2D::AddWorkItem(const std::function& work_item) { if (work_queue_ == nullptr) { work_item(); } 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>( + now - work_queue_->front().time) + .count()); } } @@ -323,7 +330,7 @@ void PoseGraph2D::DispatchOptimization() { run_loop_closure_ = true; // If there is a 'work_queue_' already, some other thread will take care. if (work_queue_ == nullptr) { - work_queue_ = common::make_unique>>(); + work_queue_ = common::make_unique(); constraint_builder_.WhenDone( std::bind(&PoseGraph2D::HandleWorkQueue, this, std::placeholders::_1)); } @@ -425,7 +432,7 @@ void PoseGraph2D::HandleWorkQueue( work_queue_.reset(); return; } - work_queue_->front()(); + work_queue_->front().task(); work_queue_->pop_front(); } LOG(INFO) << "Remaining work items in queue: " << work_queue_->size(); @@ -1077,5 +1084,12 @@ void PoseGraph2D::SetGlobalSlamOptimizationCallback( 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 cartographer diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index 6a1032e..4079bda 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -36,9 +36,11 @@ #include "cartographer/mapping/internal/constraints/constraint_builder_2d.h" #include "cartographer/mapping/internal/optimization/optimization_problem_2d.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_data.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/landmark_data.h" #include "cartographer/sensor/odometry_data.h" @@ -149,6 +151,8 @@ class PoseGraph2D : public PoseGraph { transform::Rigid3d GetInterpolatedGlobalTrajectoryPose( int trajectory_id, const common::Time time) const REQUIRES(mutex_); + static void RegisterMetrics(metrics::FamilyFactory* family_factory); + private: MapById GetSubmapDataUnderLock() 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 // considered later. - std::unique_ptr>> work_queue_ - GUARDED_BY(mutex_); + std::unique_ptr work_queue_ GUARDED_BY(mutex_); // We globally localize a fraction of the nodes from each trajectory. std::unordered_map> diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 27b482d..b5aa5a6 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -39,6 +39,8 @@ namespace cartographer { namespace mapping { +static auto* kWorkQueueDelayMetric = metrics::Gauge::Null(); + PoseGraph3D::PoseGraph3D( const proto::PoseGraphOptions& options, std::unique_ptr optimization_problem, @@ -143,7 +145,12 @@ void PoseGraph3D::AddWorkItem(const std::function& work_item) { if (work_queue_ == nullptr) { work_item(); } 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>( + now - work_queue_->front().time) + .count()); } } @@ -339,7 +346,7 @@ void PoseGraph3D::DispatchOptimization() { run_loop_closure_ = true; // If there is a 'work_queue_' already, some other thread will take care. if (work_queue_ == nullptr) { - work_queue_ = common::make_unique>>(); + work_queue_ = common::make_unique(); constraint_builder_.WhenDone( std::bind(&PoseGraph3D::HandleWorkQueue, this, std::placeholders::_1)); } @@ -442,7 +449,7 @@ void PoseGraph3D::HandleWorkQueue( work_queue_.reset(); return; } - work_queue_->front()(); + work_queue_->front().task(); work_queue_->pop_front(); } LOG(INFO) << "Remaining work items in queue: " << work_queue_->size(); @@ -1103,5 +1110,12 @@ void PoseGraph3D::SetGlobalSlamOptimizationCallback( 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 cartographer diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index 5e35319..7c2d935 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -36,9 +36,11 @@ #include "cartographer/mapping/internal/constraints/constraint_builder_3d.h" #include "cartographer/mapping/internal/optimization/optimization_problem_3d.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_data.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/landmark_data.h" #include "cartographer/sensor/odometry_data.h" @@ -148,6 +150,8 @@ class PoseGraph3D : public PoseGraph { transform::Rigid3d GetInterpolatedGlobalTrajectoryPose( int trajectory_id, const common::Time time) const REQUIRES(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. @@ -227,8 +231,7 @@ class PoseGraph3D : public PoseGraph { // If it exists, further work items must be added to this queue, and will be // considered later. - std::unique_ptr>> work_queue_ - GUARDED_BY(mutex_); + std::unique_ptr work_queue_ GUARDED_BY(mutex_); // We globally localize a fraction of the nodes from each trajectory. std::unordered_map> diff --git a/cartographer/mapping/internal/work_queue.h b/cartographer/mapping/internal/work_queue.h new file mode 100644 index 0000000..f94baf2 --- /dev/null +++ b/cartographer/mapping/internal/work_queue.h @@ -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 +#include +#include + +namespace cartographer { +namespace mapping { + +struct WorkItem { + std::chrono::steady_clock::time_point time; + std::function task; +}; + +using WorkQueue = std::deque; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_WORK_QUEUE_H diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h index 5e7bdcc..5f20dca 100644 --- a/cartographer/mapping/pose_graph_interface.h +++ b/cartographer/mapping/pose_graph_interface.h @@ -17,6 +17,7 @@ #ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_INTERFACE_H_ #define CARTOGRAPHER_MAPPING_POSE_GRAPH_INTERFACE_H_ +#include #include #include "cartographer/common/optional.h" diff --git a/cartographer/metrics/register.cc b/cartographer/metrics/register.cc index 5efcaea..d11f55d 100644 --- a/cartographer/metrics/register.cc +++ b/cartographer/metrics/register.cc @@ -17,7 +17,9 @@ #include "cartographer/metrics/register.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/pose_graph_3d.h" #include "cartographer/mapping/internal/constraints/constraint_builder_2d.h" #include "cartographer/mapping/internal/constraints/constraint_builder_3d.h" #include "cartographer/mapping/internal/global_trajectory_builder.h" @@ -31,6 +33,8 @@ void RegisterAllMetrics(FamilyFactory* registry) { mapping::GlobalTrajectoryBuilderRegisterMetrics(registry); mapping::LocalTrajectoryBuilder2D::RegisterMetrics(registry); mapping::LocalTrajectoryBuilder3D::RegisterMetrics(registry); + mapping::PoseGraph2D::RegisterMetrics(registry); + mapping::PoseGraph3D::RegisterMetrics(registry); } } // namespace metrics