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

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::OptimizationProblem2D> optimization_problem,
@@ -145,7 +147,12 @@ void PoseGraph2D::AddWorkItem(const std::function<void()>& 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<std::chrono::duration<double>>(
+            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<std::deque<std::function<void()>>>();
+    work_queue_ = common::make_unique<WorkQueue>();
     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

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<SubmapId, PoseGraphInterface::SubmapData> 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<std::deque<std::function<void()>>> work_queue_
-      GUARDED_BY(mutex_);
+  std::unique_ptr<WorkQueue> work_queue_ GUARDED_BY(mutex_);
 
   // We globally localize a fraction of the nodes from each trajectory.
   std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>

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::OptimizationProblem3D> optimization_problem,
@@ -143,7 +145,12 @@ void PoseGraph3D::AddWorkItem(const std::function<void()>& 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<std::chrono::duration<double>>(
+            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<std::deque<std::function<void()>>>();
+    work_queue_ = common::make_unique<WorkQueue>();
     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

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<std::deque<std::function<void()>>> work_queue_
-      GUARDED_BY(mutex_);
+  std::unique_ptr<WorkQueue> work_queue_ GUARDED_BY(mutex_);
 
   // We globally localize a fraction of the nodes from each trajectory.
   std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>

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 <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

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 <chrono>
 #include <vector>
 
 #include "cartographer/common/optional.h"

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