Add metric counters to track sensor data in the AddSensorDataBatchHandler class (#1338)

Adds additional metrics to track incoming sensor and local slam result data in Prometheus.

cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.cc

@@ -21,6 +21,7 @@
 #include "cartographer/cloud/internal/map_builder_context_interface.h"
 #include "cartographer/cloud/proto/map_builder_service.pb.h"
 #include "cartographer/mapping/local_slam_result_data.h"
+#include "cartographer/metrics/counter.h"
 #include "cartographer/sensor/internal/dispatchable.h"
 #include "cartographer/sensor/timed_point_cloud_data.h"
 #include "google/protobuf/empty.pb.h"
@@ -29,6 +30,10 @@ namespace cartographer {
 namespace cloud {
 namespace handlers {
 
+metrics::Family<metrics::Counter>*
+    AddSensorDataBatchHandler::counter_metrics_family_ =
+        metrics::Family<metrics::Counter>::Null();
+
 void AddSensorDataBatchHandler::OnRequest(
     const proto::AddSensorDataBatchRequest& request) {
   for (const proto::SensorData& sensor_data : request.sensor_data()) {
@@ -42,6 +47,9 @@ void AddSensorDataBatchHandler::OnRequest(
       Finish(::grpc::Status(::grpc::NOT_FOUND, "Unknown trajectory"));
       return;
     }
+    ClientMetrics* const metrics =
+        GetOrCreateClientMetrics(sensor_data.sensor_metadata().client_id(),
+                                 sensor_data.sensor_metadata().trajectory_id());
     switch (sensor_data.sensor_data_case()) {
       case proto::SensorData::kOdometryData:
         GetUnsynchronizedContext<MapBuilderContextInterface>()
@@ -50,6 +58,7 @@ void AddSensorDataBatchHandler::OnRequest(
                 sensor::MakeDispatchable(
                     sensor_data.sensor_metadata().sensor_id(),
                     sensor::FromProto(sensor_data.odometry_data())));
+        metrics->odometry_sensor_counter->Increment();
         break;
       case proto::SensorData::kImuData:
         GetUnsynchronizedContext<MapBuilderContextInterface>()
@@ -57,6 +66,7 @@ void AddSensorDataBatchHandler::OnRequest(
                                 sensor::MakeDispatchable(
                                     sensor_data.sensor_metadata().sensor_id(),
                                     sensor::FromProto(sensor_data.imu_data())));
+        metrics->imu_sensor_counter->Increment();
         break;
       case proto::SensorData::kTimedPointCloudData:
         GetUnsynchronizedContext<MapBuilderContextInterface>()
@@ -65,6 +75,7 @@ void AddSensorDataBatchHandler::OnRequest(
                 sensor::MakeDispatchable(
                     sensor_data.sensor_metadata().sensor_id(),
                     sensor::FromProto(sensor_data.timed_point_cloud_data())));
+        metrics->timed_point_cloud_counter->Increment();
         break;
       case proto::SensorData::kFixedFramePoseData:
         GetUnsynchronizedContext<MapBuilderContextInterface>()
@@ -73,6 +84,7 @@ void AddSensorDataBatchHandler::OnRequest(
                 sensor::MakeDispatchable(
                     sensor_data.sensor_metadata().sensor_id(),
                     sensor::FromProto(sensor_data.fixed_frame_pose_data())));
+        metrics->fixed_frame_pose_counter->Increment();
         break;
       case proto::SensorData::kLandmarkData:
         GetUnsynchronizedContext<MapBuilderContextInterface>()
@@ -81,12 +93,14 @@ void AddSensorDataBatchHandler::OnRequest(
                 sensor::MakeDispatchable(
                     sensor_data.sensor_metadata().sensor_id(),
                     sensor::FromProto(sensor_data.landmark_data())));
+        metrics->landmark_counter->Increment();
         break;
       case proto::SensorData::kLocalSlamResultData:
         GetContext<MapBuilderContextInterface>()->EnqueueLocalSlamResultData(
             sensor_data.sensor_metadata().trajectory_id(),
             sensor_data.sensor_metadata().sensor_id(),
             sensor_data.local_slam_result_data());
+        metrics->local_slam_result_counter->Increment();
         break;
       default:
         LOG(FATAL) << "Unknown sensor data type: "
@@ -96,6 +110,54 @@ void AddSensorDataBatchHandler::OnRequest(
   Send(absl::make_unique<google::protobuf::Empty>());
 }
 
+void AddSensorDataBatchHandler::RegisterMetrics(
+    metrics::FamilyFactory* family_factory) {
+  counter_metrics_family_ = family_factory->NewCounterFamily(
+      "cartographer_sensor_data_total", "Sensor data received");
+}
+
+AddSensorDataBatchHandler::ClientMetrics*
+AddSensorDataBatchHandler::GetOrCreateClientMetrics(
+    const std::string& client_id, int trajectory_id) {
+  const std::string map_key = client_id + std::to_string(trajectory_id);
+  auto client_metric_map_itr = client_metric_map_.find(map_key);
+  if (client_metric_map_itr != client_metric_map_.end()) {
+    return client_metric_map_itr->second.get();
+  }
+
+  LOG(INFO) << "Create metrics handler for client: " << client_id;
+  auto new_metrics = absl::make_unique<ClientMetrics>();
+  new_metrics->odometry_sensor_counter = counter_metrics_family_->Add(
+      {{"client_id", client_id},
+       {"trajectory_id", std::to_string(trajectory_id)},
+       {"sensor", "odometry"}});
+  new_metrics->imu_sensor_counter = counter_metrics_family_->Add(
+      {{"client_id", client_id},
+       {"trajectory_id", std::to_string(trajectory_id)},
+       {"sensor", "imu"}});
+  new_metrics->fixed_frame_pose_counter = counter_metrics_family_->Add(
+      {{"client_id", client_id},
+       {"trajectory_id", std::to_string(trajectory_id)},
+       {"sensor", "fixed_frame_pose"}});
+  new_metrics->landmark_counter = counter_metrics_family_->Add(
+      {{"client_id", client_id},
+       {"trajectory_id", std::to_string(trajectory_id)},
+       {"sensor", "landmark"}});
+  new_metrics->local_slam_result_counter = counter_metrics_family_->Add(
+      {{"client_id", client_id},
+       {"trajectory_id", std::to_string(trajectory_id)},
+       {"sensor", "local_slam_result"}});
+  new_metrics->timed_point_cloud_counter = counter_metrics_family_->Add(
+      {{"client_id", client_id},
+       {"trajectory_id", std::to_string(trajectory_id)},
+       {"sensor", "timed_point_cloud"}});
+
+  // Obtain pointer before ownership is transferred.
+  auto* new_metrics_ptr = new_metrics.get();
+  client_metric_map_[map_key] = std::move(new_metrics);
+  return new_metrics_ptr;
+}
+
 }  // namespace handlers
 }  // namespace cloud
 }  // namespace cartographer

cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.h

@@ -17,8 +17,13 @@
 #ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_SENSOR_DATA_BATCH_HANDLER_H
 #define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_SENSOR_DATA_BATCH_HANDLER_H
 
+#include <memory>
+#include <string>
+
 #include "async_grpc/rpc_handler.h"
 #include "cartographer/cloud/proto/map_builder_service.pb.h"
+#include "cartographer/metrics/counter.h"
+#include "cartographer/metrics/family_factory.h"
 #include "google/protobuf/empty.pb.h"
 
 namespace cartographer {
@@ -34,6 +39,28 @@ class AddSensorDataBatchHandler
     : public async_grpc::RpcHandler<AddSensorDataBatchSignature> {
  public:
   void OnRequest(const proto::AddSensorDataBatchRequest& request) override;
+
+  static void RegisterMetrics(metrics::FamilyFactory* family_factory);
+
+ private:
+  struct ClientMetrics {
+    metrics::Counter* odometry_sensor_counter;
+    metrics::Counter* imu_sensor_counter;
+    metrics::Counter* timed_point_cloud_counter;
+    metrics::Counter* fixed_frame_pose_counter;
+    metrics::Counter* landmark_counter;
+    metrics::Counter* local_slam_result_counter;
+  };
+
+  ClientMetrics* GetOrCreateClientMetrics(const std::string& client_id,
+                                          int trajectory_id);
+
+  static cartographer::metrics::Family<metrics::Counter>*
+      counter_metrics_family_;
+
+  // Holds individual metrics for each client.
+  std::unordered_map<std::string, std::unique_ptr<ClientMetrics>>
+      client_metric_map_;
 };
 
 }  // namespace handlers

cartographer/cloud/metrics/prometheus/family_factory.cc

@@ -14,6 +14,8 @@
  * limitations under the License.
  */
 
+#include <unordered_map>
+
 #include "cartographer/cloud/metrics/prometheus/family_factory.h"
 
 #include "absl/memory/memory.h"
@@ -30,6 +32,23 @@ namespace {
 
 using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries;
 
+// Creates or looks up already existing objects from a wrapper map.
+template <typename WrapperMap,
+          typename ObjectPtr = typename WrapperMap::key_type,
+          typename Wrapper = typename WrapperMap::mapped_type::element_type>
+Wrapper* GetOrCreateWrapper(ObjectPtr object_ptr, WrapperMap* wrapper_map,
+                            std::mutex* wrapper_mutex) {
+  std::lock_guard<std::mutex> lock(*wrapper_mutex);
+  auto wrappers_itr = wrapper_map->find(object_ptr);
+  if (wrappers_itr == wrapper_map->end()) {
+    auto wrapper = absl::make_unique<Wrapper>(object_ptr);
+    auto* ptr = wrapper.get();
+    (*wrapper_map)[object_ptr] = std::unique_ptr<Wrapper>(std::move(wrapper));
+    return ptr;
+  }
+  return wrappers_itr->second.get();
+}
+
 class Counter : public ::cartographer::metrics::Counter {
  public:
   explicit Counter(::prometheus::Counter* prometheus)
@@ -51,15 +70,14 @@ class CounterFamily
 
   Counter* Add(const std::map<std::string, std::string>& labels) override {
     ::prometheus::Counter* counter = &prometheus_->Add(labels);
-    auto wrapper = absl::make_unique<Counter>(counter);
+    return GetOrCreateWrapper<>(counter, &wrappers_, &wrappers_mutex_);
-    auto* ptr = wrapper.get();
-    wrappers_.emplace_back(std::move(wrapper));
-    return ptr;
   }
 
  private:
   ::prometheus::Family<::prometheus::Counter>* prometheus_;
-  std::vector<std::unique_ptr<Counter>> wrappers_;
+  std::mutex wrappers_mutex_;
+  std::unordered_map<::prometheus::Counter*, std::unique_ptr<Counter>>
+      wrappers_;
 };
 
 class Gauge : public ::cartographer::metrics::Gauge {
@@ -84,15 +102,13 @@ class GaugeFamily
 
   Gauge* Add(const std::map<std::string, std::string>& labels) override {
     ::prometheus::Gauge* gauge = &prometheus_->Add(labels);
-    auto wrapper = absl::make_unique<Gauge>(gauge);
+    return GetOrCreateWrapper<>(gauge, &wrappers_, &wrappers_mutex_);
-    auto* ptr = wrapper.get();
-    wrappers_.emplace_back(std::move(wrapper));
-    return ptr;
   }
 
  private:
   ::prometheus::Family<::prometheus::Gauge>* prometheus_;
-  std::vector<std::unique_ptr<Gauge>> wrappers_;
+  std::mutex wrappers_mutex_;
+  std::unordered_map<::prometheus::Gauge*, std::unique_ptr<Gauge>> wrappers_;
 };
 
 class Histogram : public ::cartographer::metrics::Histogram {
@@ -115,15 +131,14 @@ class HistogramFamily : public ::cartographer::metrics::Family<
 
   Histogram* Add(const std::map<std::string, std::string>& labels) override {
     ::prometheus::Histogram* histogram = &prometheus_->Add(labels, boundaries_);
-    auto wrapper = absl::make_unique<Histogram>(histogram);
+    return GetOrCreateWrapper<>(histogram, &wrappers_, &wrappers_mutex_);
-    auto* ptr = wrapper.get();
-    wrappers_.emplace_back(std::move(wrapper));
-    return ptr;
   }
 
  private:
   ::prometheus::Family<::prometheus::Histogram>* prometheus_;
-  std::vector<std::unique_ptr<Histogram>> wrappers_;
+  std::mutex wrappers_mutex_;
+  std::unordered_map<::prometheus::Histogram*, std::unique_ptr<Histogram>>
+      wrappers_;
   const BucketBoundaries boundaries_;
 };
 

cartographer/metrics/family_factory.h

@@ -27,14 +27,31 @@
 namespace cartographer {
 namespace metrics {
 
+template <typename MetricType>
+class NullFamily;
+
 template <typename MetricType>
 class Family {
- public:
+ public:  // Family instance that does nothing. Safe for use in static
+          // initializers.
+  static Family<MetricType>* Null() {
+    static NullFamily<MetricType> null_family;
+    return &null_family;
+  }
+
   virtual ~Family() = default;
 
   virtual MetricType* Add(const std::map<std::string, std::string>& labels) = 0;
 };
 
+template <typename MetricType>
+class NullFamily : public Family<MetricType> {
+ public:
+  MetricType* Add(const std::map<std::string, std::string>& labels) override {
+    return MetricType::Null();
+  }
+};
+
 class FamilyFactory {
  public:
   virtual ~FamilyFactory() = default;