From 666095cb41562ba779523f20815418f071d69003 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= <cschuet@google.com>
Date: Wed, 25 Apr 2018 22:09:56 +0200
Subject: [PATCH] Introduce metric to measure the length of the SLAM queue
 (#1117)

---
 cartographer/cloud/internal/map_builder_server.cc | 9 +++++++++
 cartographer/cloud/internal/map_builder_server.h  | 3 +++
 2 files changed, 12 insertions(+)

diff --git a/cartographer/cloud/internal/map_builder_server.cc b/cartographer/cloud/internal/map_builder_server.cc
index ca4719f..4eeec76 100644
--- a/cartographer/cloud/internal/map_builder_server.cc
+++ b/cartographer/cloud/internal/map_builder_server.cc
@@ -44,6 +44,7 @@ namespace cartographer {
 namespace cloud {
 namespace {
 
+static auto* kIncomingDataQueueMetric = metrics::Gauge::Null();
 const common::Duration kPopTimeout = common::FromMilliseconds(100);
 
 }  // namespace
@@ -135,6 +136,7 @@ void MapBuilderServer::Shutdown() {
 void MapBuilderServer::ProcessSensorDataQueue() {
   LOG(INFO) << "Starting SLAM thread.";
   while (!shutting_down_) {
+    kIncomingDataQueueMetric->Set(incoming_data_queue_.Size());
     std::unique_ptr<MapBuilderContextInterface::Data> sensor_data =
         incoming_data_queue_.PopWithTimeout(kPopTimeout);
     if (sensor_data) {
@@ -233,5 +235,12 @@ void MapBuilderServer::WaitUntilIdle() {
   map_builder_->pose_graph()->RunFinalOptimization();
 }
 
+void MapBuilderServer::RegisterMetrics(metrics::FamilyFactory* factory) {
+  auto* queue_length = factory->NewGaugeFamily(
+      "cloud_internal_map_builder_server_incoming_data_queue_length",
+      "Incoming SLAM Data Queue length");
+  kIncomingDataQueueMetric = queue_length->Add({});
+}
+
 }  // namespace cloud
 }  // namespace cartographer
diff --git a/cartographer/cloud/internal/map_builder_server.h b/cartographer/cloud/internal/map_builder_server.h
index 8e0cef1..2c403d3 100644
--- a/cartographer/cloud/internal/map_builder_server.h
+++ b/cartographer/cloud/internal/map_builder_server.h
@@ -31,6 +31,7 @@
 #include "cartographer/mapping/local_slam_result_data.h"
 #include "cartographer/mapping/map_builder.h"
 #include "cartographer/mapping/trajectory_builder_interface.h"
+#include "cartographer/metrics/family_factory.h"
 #include "cartographer/sensor/internal/dispatchable.h"
 
 namespace cartographer {
@@ -91,6 +92,8 @@ class MapBuilderServer : public MapBuilderServerInterface {
   // thread.
   void Shutdown() final;
 
+  static void RegisterMetrics(metrics::FamilyFactory* family_factory);
+
  private:
   using LocalSlamResultHandlerSubscriptions =
       std::map<int /* subscription_index */,