Register internal metrics and provide a public interface. (#917)

[RFC 24](https://github.com/googlecartographer/rfcs/blob/master/text/0024-monitoring-ros.md)

Public API:
  - adds `cartographer_ros::metrics::FamilyFactory`
  - compatible with `::cartographer::metrics::RegisterAllMetrics`

Public RPC interface:
  - adds the ROS service `/read_metrics`
  - response contains the latest values of all available metric families

cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc

@@ -15,12 +15,16 @@
  */
 
 #include "cartographer/cloud/client/map_builder_stub.h"
+#include "cartographer/common/make_unique.h"
 #include "cartographer_ros/node.h"
 #include "cartographer_ros/node_options.h"
 #include "cartographer_ros/ros_log_sink.h"
 #include "gflags/gflags.h"
 #include "tf2_ros/transform_listener.h"
 
+DEFINE_bool(collect_metrics, false,
+            "Activates the collection of runtime metrics. If activated, the "
+            "metrics can be accessed via a ROS service.");
 DEFINE_string(configuration_directory, "",
               "First directory in which configuration files are searched, "
               "second is always the Cartographer installation to allow "
@@ -56,7 +60,8 @@ void Run() {
   auto map_builder =
       cartographer::common::make_unique<::cartographer::cloud::MapBuilderStub>(
           FLAGS_server_address);
-  Node node(node_options, std::move(map_builder), &tf_buffer);
+  Node node(node_options, std::move(map_builder), &tf_buffer,
+            FLAGS_collect_metrics);
 
   if (!FLAGS_load_state_filename.empty()) {
     node.LoadState(FLAGS_load_state_filename, true /* load_frozen_state */);

cartographer_ros/cartographer_ros/metrics/family_factory.cc

@@ -0,0 +1,71 @@
+/*
+ * 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.
+ */
+
+#include "cartographer_ros/metrics/family_factory.h"
+
+#include "cartographer/common/make_unique.h"
+
+namespace cartographer_ros {
+namespace metrics {
+
+using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries;
+
+::cartographer::metrics::Family<::cartographer::metrics::Counter>*
+FamilyFactory::NewCounterFamily(const std::string& name,
+                                const std::string& description) {
+  auto wrapper =
+      ::cartographer::common::make_unique<CounterFamily>(name, description);
+  auto* ptr = wrapper.get();
+  counter_families_.emplace_back(std::move(wrapper));
+  return ptr;
+}
+
+::cartographer::metrics::Family<::cartographer::metrics::Gauge>*
+FamilyFactory::NewGaugeFamily(const std::string& name,
+                              const std::string& description) {
+  auto wrapper =
+      ::cartographer::common::make_unique<GaugeFamily>(name, description);
+  auto* ptr = wrapper.get();
+  gauge_families_.emplace_back(std::move(wrapper));
+  return ptr;
+}
+
+::cartographer::metrics::Family<::cartographer::metrics::Histogram>*
+FamilyFactory::NewHistogramFamily(const std::string& name,
+                                  const std::string& description,
+                                  const BucketBoundaries& boundaries) {
+  auto wrapper = ::cartographer::common::make_unique<HistogramFamily>(
+      name, description, boundaries);
+  auto* ptr = wrapper.get();
+  histogram_families_.emplace_back(std::move(wrapper));
+  return ptr;
+}
+
+void FamilyFactory::ReadMetrics(
+    ::cartographer_ros_msgs::ReadMetrics::Response* response) const {
+  for (const auto& counter_family : counter_families_) {
+    response->metric_families.push_back(counter_family->ToRosMessage());
+  }
+  for (const auto& gauge_family : gauge_families_) {
+    response->metric_families.push_back(gauge_family->ToRosMessage());
+  }
+  for (const auto& histogram_family : histogram_families_) {
+    response->metric_families.push_back(histogram_family->ToRosMessage());
+  }
+}
+
+}  // namespace metrics
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/metrics/family_factory.h

@@ -0,0 +1,61 @@
+/*
+ * 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_ROS_METRICS_FAMILY_FACTORY_H
+#define CARTOGRAPHER_ROS_METRICS_FAMILY_FACTORY_H
+
+#include <memory>
+#include <string>
+
+#include "cartographer/metrics/family_factory.h"
+#include "cartographer_ros/metrics/internal/counter.h"
+#include "cartographer_ros/metrics/internal/family.h"
+#include "cartographer_ros/metrics/internal/gauge.h"
+#include "cartographer_ros/metrics/internal/histogram.h"
+#include "cartographer_ros_msgs/ReadMetrics.h"
+
+namespace cartographer_ros {
+namespace metrics {
+
+// Realizes the factory / registry interface for the metrics in libcartographer
+// and provides a wrapper to collect ROS messages from the metrics it owns.
+class FamilyFactory : public ::cartographer::metrics::FamilyFactory {
+ public:
+  ::cartographer::metrics::Family<::cartographer::metrics::Counter>*
+
+  NewCounterFamily(const std::string& name,
+                   const std::string& description) override;
+  ::cartographer::metrics::Family<::cartographer::metrics::Gauge>*
+  NewGaugeFamily(const std::string& name,
+                 const std::string& description) override;
+  ::cartographer::metrics::Family<::cartographer::metrics::Histogram>*
+  NewHistogramFamily(const std::string& name, const std::string& description,
+                     const ::cartographer::metrics::Histogram::BucketBoundaries&
+                         boundaries) override;
+
+  void ReadMetrics(
+      ::cartographer_ros_msgs::ReadMetrics::Response* response) const;
+
+ private:
+  std::vector<std::unique_ptr<CounterFamily>> counter_families_;
+  std::vector<std::unique_ptr<GaugeFamily>> gauge_families_;
+  std::vector<std::unique_ptr<HistogramFamily>> histogram_families_;
+};
+
+}  // namespace metrics
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_METRICS_FAMILY_FACTORY_H

cartographer_ros/cartographer_ros/node.cc

@@ -28,9 +28,11 @@
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/pose_graph_interface.h"
 #include "cartographer/mapping/proto/submap_visualization.pb.h"
+#include "cartographer/metrics/register.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
+#include "cartographer_ros/metrics/family_factory.h"
 #include "cartographer_ros/msg_conversion.h"
 #include "cartographer_ros/sensor_bridge.h"
 #include "cartographer_ros/tf_bridge.h"
@@ -88,10 +90,15 @@ using TrajectoryState =
 Node::Node(
     const NodeOptions& node_options,
     std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
-    tf2_ros::Buffer* const tf_buffer)
+    tf2_ros::Buffer* const tf_buffer, const bool collect_metrics)
     : node_options_(node_options),
       map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {
   carto::common::MutexLocker lock(&mutex_);
+  if (collect_metrics) {
+    metrics_registry_ = carto::common::make_unique<metrics::FamilyFactory>();
+    carto::metrics::RegisterAllMetrics(metrics_registry_.get());
+  }
+
   submap_list_publisher_ =
       node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
           kSubmapListTopic, kLatestOnlyPublisherQueueSize);
@@ -114,6 +121,8 @@ Node::Node(
       kWriteStateServiceName, &Node::HandleWriteState, this));
   service_servers_.push_back(node_handle_.advertiseService(
       kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this));
+  service_servers_.push_back(node_handle_.advertiseService(
+      kReadMetricsServiceName, &Node::HandleReadMetrics, this));
 
   scan_matched_point_cloud_publisher_ =
       node_handle_.advertise<sensor_msgs::PointCloud2>(
@@ -620,6 +629,22 @@ bool Node::HandleWriteState(
   return true;
 }
 
+bool Node::HandleReadMetrics(
+    ::cartographer_ros_msgs::ReadMetrics::Request& request,
+    ::cartographer_ros_msgs::ReadMetrics::Response& response) {
+  carto::common::MutexLocker lock(&mutex_);
+  response.timestamp = ros::Time::now();
+  if (!metrics_registry_) {
+    response.status.code = cartographer_ros_msgs::StatusCode::UNAVAILABLE;
+    response.status.message = "Collection of runtime metrics is not activated.";
+    return true;
+  }
+  metrics_registry_->ReadMetrics(&response);
+  response.status.code = cartographer_ros_msgs::StatusCode::OK;
+  response.status.message = "Successfully read metrics.";
+  return true;
+}
+
 void Node::FinishAllTrajectories() {
   carto::common::MutexLocker lock(&mutex_);
   for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {

cartographer_ros/cartographer_ros/node.h

@@ -29,11 +29,13 @@
 #include "cartographer/mapping/map_builder_interface.h"
 #include "cartographer/mapping/pose_extrapolator.h"
 #include "cartographer_ros/map_builder_bridge.h"
+#include "cartographer_ros/metrics/family_factory.h"
 #include "cartographer_ros/node_constants.h"
 #include "cartographer_ros/node_options.h"
 #include "cartographer_ros/trajectory_options.h"
 #include "cartographer_ros_msgs/FinishTrajectory.h"
 #include "cartographer_ros_msgs/GetTrajectoryStates.h"
+#include "cartographer_ros_msgs/ReadMetrics.h"
 #include "cartographer_ros_msgs/SensorTopics.h"
 #include "cartographer_ros_msgs/StartTrajectory.h"
 #include "cartographer_ros_msgs/StatusResponse.h"
@@ -58,7 +60,7 @@ class Node {
  public:
   Node(const NodeOptions& node_options,
        std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
-       tf2_ros::Buffer* tf_buffer);
+       tf2_ros::Buffer* tf_buffer, bool collect_metrics);
   ~Node();
 
   Node(const Node&) = delete;
@@ -142,6 +144,9 @@ class Node {
   bool HandleGetTrajectoryStates(
       ::cartographer_ros_msgs::GetTrajectoryStates::Request& request,
       ::cartographer_ros_msgs::GetTrajectoryStates::Response& response);
+  bool HandleReadMetrics(
+      cartographer_ros_msgs::ReadMetrics::Request& request,
+      cartographer_ros_msgs::ReadMetrics::Response& response);
 
   // Returns the set of SensorIds expected for a trajectory.
   // 'SensorId::id' is the expected ROS topic name.
@@ -212,11 +217,14 @@ class Node {
   // We have to keep the timer handles of ::ros::WallTimers around, otherwise
   // they do not fire.
   std::vector<::ros::WallTimer> wall_timers_;
+
   // The timer for publishing local trajectory data (i.e. pose transforms and
   // range data point clouds) is a regular timer which is not triggered when
   // simulation time is standing still. This prevents overflowing the transform
   // listener buffer by publishing the same transforms over and over again.
   ::ros::Timer publish_local_trajectory_data_timer_;
+
+  std::unique_ptr<cartographer_ros::metrics::FamilyFactory> metrics_registry_;
 };
 
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/node_constants.h

@@ -38,6 +38,7 @@ constexpr char kSubmapQueryServiceName[] = "submap_query";
 constexpr char kStartTrajectoryServiceName[] = "start_trajectory";
 constexpr char kWriteStateServiceName[] = "write_state";
 constexpr char kGetTrajectoryStatesServiceName[] = "get_trajectory_states";
+constexpr char kReadMetricsServiceName[] = "read_metrics";
 constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list";
 constexpr char kLandmarkPosesListTopic[] = "landmark_poses_list";
 constexpr char kConstraintListTopic[] = "constraint_list";

cartographer_ros/cartographer_ros/node_main.cc

@@ -14,6 +14,7 @@
  * limitations under the License.
  */
 
+#include "cartographer/common/make_unique.h"
 #include "cartographer/mapping/map_builder.h"
 #include "cartographer_ros/node.h"
 #include "cartographer_ros/node_options.h"
@@ -21,6 +22,9 @@
 #include "gflags/gflags.h"
 #include "tf2_ros/transform_listener.h"
 
+DEFINE_bool(collect_metrics, false,
+            "Activates the collection of runtime metrics. If activated, the "
+            "metrics can be accessed via a ROS service.");
 DEFINE_string(configuration_directory, "",
               "First directory in which configuration files are searched, "
               "second is always the Cartographer installation to allow "
@@ -53,9 +57,10 @@ void Run() {
       LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
 
   auto map_builder =
-      cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
+      ::cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
           node_options.map_builder_options);
-  Node node(node_options, std::move(map_builder), &tf_buffer);
+  Node node(node_options, std::move(map_builder), &tf_buffer,
+            FLAGS_collect_metrics);
   if (!FLAGS_load_state_filename.empty()) {
     node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
   }

cartographer_ros/cartographer_ros/offline_node.cc

@@ -32,6 +32,9 @@
 #include "tf2_ros/static_transform_broadcaster.h"
 #include "urdf/model.h"
 
+DEFINE_bool(collect_metrics, false,
+            "Activates the collection of runtime metrics. If activated, the "
+            "metrics can be accessed via a ROS service.");
 DEFINE_string(configuration_directory, "",
               "First directory in which configuration files are searched, "
               "second is always the Cartographer installation to allow "
@@ -130,7 +133,8 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
 
   tf_buffer.setUsingDedicatedThread(true);
 
-  Node node(node_options, std::move(map_builder), &tf_buffer);
+  Node node(node_options, std::move(map_builder), &tf_buffer,
+            FLAGS_collect_metrics);
   if (!FLAGS_load_state_filename.empty()) {
     node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
   }

cartographer_ros_msgs/srv/ReadMetrics.srv

@@ -0,0 +1,18 @@
+# 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.
+
+---
+cartographer_ros_msgs/StatusResponse status
+cartographer_ros_msgs/MetricFamily[] metric_families
+time timestamp