From be7ecf2e0d8bbdf43e11fb407e95ac9dd3f2ff98 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Wed, 4 Jul 2018 17:38:17 +0200 Subject: [PATCH] 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_grpc/node_grpc_main.cc | 7 +- .../metrics/family_factory.cc | 71 +++++++++++++++++++ .../cartographer_ros/metrics/family_factory.h | 61 ++++++++++++++++ cartographer_ros/cartographer_ros/node.cc | 27 ++++++- cartographer_ros/cartographer_ros/node.h | 10 ++- .../cartographer_ros/node_constants.h | 1 + .../cartographer_ros/node_main.cc | 9 ++- .../cartographer_ros/offline_node.cc | 6 +- cartographer_ros_msgs/srv/ReadMetrics.srv | 18 +++++ 9 files changed, 204 insertions(+), 6 deletions(-) create mode 100644 cartographer_ros/cartographer_ros/metrics/family_factory.cc create mode 100644 cartographer_ros/cartographer_ros/metrics/family_factory.h create mode 100644 cartographer_ros_msgs/srv/ReadMetrics.srv diff --git a/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc b/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc index 0e33f99..1ddb0ef 100644 --- a/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc +++ b/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 */); diff --git a/cartographer_ros/cartographer_ros/metrics/family_factory.cc b/cartographer_ros/cartographer_ros/metrics/family_factory.cc new file mode 100644 index 0000000..cb0133a --- /dev/null +++ b/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(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(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( + 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 diff --git a/cartographer_ros/cartographer_ros/metrics/family_factory.h b/cartographer_ros/cartographer_ros/metrics/family_factory.h new file mode 100644 index 0000000..a05fbd5 --- /dev/null +++ b/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 +#include + +#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> counter_families_; + std::vector> gauge_families_; + std::vector> histogram_families_; +}; + +} // namespace metrics +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_METRICS_FAMILY_FACTORY_H diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 8cc4852..ef38adc 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/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 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(); + 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( @@ -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()) { diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 1ea0608..2b4bbc1 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/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 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 metrics_registry_; }; } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/node_constants.h b/cartographer_ros/cartographer_ros/node_constants.h index b658d16..a0e3f3e 100644 --- a/cartographer_ros/cartographer_ros/node_constants.h +++ b/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"; diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 62d8181..247feb3 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/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::common::make_unique( 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); } diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc index e5f40e7..08bbef9 100644 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ b/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); } diff --git a/cartographer_ros_msgs/srv/ReadMetrics.srv b/cartographer_ros_msgs/srv/ReadMetrics.srv new file mode 100644 index 0000000..5070e38 --- /dev/null +++ b/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