From c041635fb5d0c93c59689118ba27222485eec70d Mon Sep 17 00:00:00 2001
From: Julius Kammerl <julius.kammerl@gmail.com>
Date: Tue, 31 Jul 2018 10:06:54 +0200
Subject: [PATCH] Add sensor counter metrics to TrajectoryCollator class
 (#1342)

Adds metrics to monitor the incoming sensor messages processed by the TrajectoryCollator class.
---
 cartographer/metrics/register.cc              |  2 ++
 .../sensor/internal/trajectory_collator.cc    | 30 +++++++++++++++++++
 .../sensor/internal/trajectory_collator.h     | 13 ++++++++
 3 files changed, 45 insertions(+)

diff --git a/cartographer/metrics/register.cc b/cartographer/metrics/register.cc
index d11f55d..d1c84e1 100644
--- a/cartographer/metrics/register.cc
+++ b/cartographer/metrics/register.cc
@@ -23,6 +23,7 @@
 #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"
+#include "cartographer/sensor/internal/trajectory_collator.h"
 
 namespace cartographer {
 namespace metrics {
@@ -35,6 +36,7 @@ void RegisterAllMetrics(FamilyFactory* registry) {
   mapping::LocalTrajectoryBuilder3D::RegisterMetrics(registry);
   mapping::PoseGraph2D::RegisterMetrics(registry);
   mapping::PoseGraph3D::RegisterMetrics(registry);
+  sensor::TrajectoryCollator::RegisterMetrics(registry);
 }
 
 }  // namespace metrics
diff --git a/cartographer/sensor/internal/trajectory_collator.cc b/cartographer/sensor/internal/trajectory_collator.cc
index 78204e7..4b7c89c 100644
--- a/cartographer/sensor/internal/trajectory_collator.cc
+++ b/cartographer/sensor/internal/trajectory_collator.cc
@@ -19,6 +19,10 @@
 namespace cartographer {
 namespace sensor {
 
+metrics::Family<metrics::Counter>*
+    TrajectoryCollator::collator_metrics_family_ =
+        metrics::Family<metrics::Counter>::Null();
+
 void TrajectoryCollator::AddTrajectory(
     const int trajectory_id,
     const std::unordered_set<std::string>& expected_sensor_ids,
@@ -43,6 +47,8 @@ void TrajectoryCollator::FinishTrajectory(const int trajectory_id) {
 void TrajectoryCollator::AddSensorData(const int trajectory_id,
                                        std::unique_ptr<Data> data) {
   QueueKey queue_key{trajectory_id, data->GetSensorId()};
+  auto* metric = GetOrCreateSensorMetric(data->GetSensorId(), trajectory_id);
+  metric->Increment();
   trajectory_to_queue_.at(trajectory_id)
       .Add(std::move(queue_key), std::move(data));
 }
@@ -57,5 +63,29 @@ absl::optional<int> TrajectoryCollator::GetBlockingTrajectoryId() const {
   return absl::optional<int>();
 }
 
+void TrajectoryCollator::RegisterMetrics(
+    metrics::FamilyFactory* family_factory) {
+  collator_metrics_family_ = family_factory->NewCounterFamily(
+      "collator_input_total", "Sensor data received");
+}
+
+metrics::Counter* TrajectoryCollator::GetOrCreateSensorMetric(
+    const std::string& sensor_id, int trajectory_id) {
+  const std::string map_key = sensor_id + "/" + std::to_string(trajectory_id);
+
+  auto metrics_map_itr = metrics_map_.find(map_key);
+  if (metrics_map_itr != metrics_map_.end()) {
+    return metrics_map_itr->second;
+  }
+
+  LOG(INFO) << "Create metrics handler for key: " << map_key;
+  auto new_counter = collator_metrics_family_->Add(
+      {{"sensor_id", sensor_id},
+       {"trajectory_id", std::to_string(trajectory_id)}});
+
+  metrics_map_[map_key] = new_counter;
+  return new_counter;
+}
+
 }  // namespace sensor
 }  // namespace cartographer
diff --git a/cartographer/sensor/internal/trajectory_collator.h b/cartographer/sensor/internal/trajectory_collator.h
index 6707acd..85f58fb 100644
--- a/cartographer/sensor/internal/trajectory_collator.h
+++ b/cartographer/sensor/internal/trajectory_collator.h
@@ -21,6 +21,8 @@
 #include <unordered_map>
 #include <vector>
 
+#include "cartographer/metrics/counter.h"
+#include "cartographer/metrics/family_factory.h"
 #include "cartographer/sensor/collator_interface.h"
 #include "cartographer/sensor/internal/ordered_multi_queue.h"
 
@@ -52,7 +54,18 @@ class TrajectoryCollator : public CollatorInterface {
 
   absl::optional<int> GetBlockingTrajectoryId() const override;
 
+  static void RegisterMetrics(metrics::FamilyFactory* family_factory);
+
  private:
+  metrics::Counter* GetOrCreateSensorMetric(const std::string& sensor_id,
+                                            int trajectory_id);
+
+  static cartographer::metrics::Family<metrics::Counter>*
+      collator_metrics_family_;
+
+  // Holds individual counters for each trajectory/sensor pair.
+  std::unordered_map<std::string, metrics::Counter*> metrics_map_;
+
   std::unordered_map<int, OrderedMultiQueue> trajectory_to_queue_;
 
   // Map of trajectory ID to all associated QueueKeys.