Add internal metric families. (#914)
- minimal counter, gauge and histogram implementations - metric family interfaces as defined in libcartographer - serializable to ROS messages RFC: https://github.com/googlecartographer/rfcs/pull/26master
parent
f5b583fde8
commit
237dd83b60
|
@ -0,0 +1,51 @@
|
||||||
|
/*
|
||||||
|
* 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_INTERNAL_COUNTER_H
|
||||||
|
#define CARTOGRAPHER_ROS_METRICS_INTERNAL_COUNTER_H
|
||||||
|
|
||||||
|
#include "cartographer/metrics/counter.h"
|
||||||
|
#include "cartographer_ros/metrics/internal/gauge.h"
|
||||||
|
#include "cartographer_ros_msgs/Metric.h"
|
||||||
|
|
||||||
|
namespace cartographer_ros {
|
||||||
|
namespace metrics {
|
||||||
|
|
||||||
|
class Counter : public ::cartographer::metrics::Counter {
|
||||||
|
public:
|
||||||
|
explicit Counter(const std::map<std::string, std::string>& labels)
|
||||||
|
: gauge_(labels) {}
|
||||||
|
|
||||||
|
void Increment(const double by_value) override { gauge_.Increment(by_value); }
|
||||||
|
|
||||||
|
void Increment() override { gauge_.Increment(); }
|
||||||
|
|
||||||
|
double Value() const { return gauge_.Value(); }
|
||||||
|
|
||||||
|
cartographer_ros_msgs::Metric ToRosMessage() {
|
||||||
|
cartographer_ros_msgs::Metric msg = gauge_.ToRosMessage();
|
||||||
|
msg.type = cartographer_ros_msgs::Metric::TYPE_COUNTER;
|
||||||
|
return msg;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
Gauge gauge_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace metrics
|
||||||
|
} // namespace cartographer_ros
|
||||||
|
|
||||||
|
#endif // CARTOGRAPHER_ROS_METRICS_INTERNAL_COUNTER_H
|
|
@ -0,0 +1,83 @@
|
||||||
|
/*
|
||||||
|
* 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/internal/family.h"
|
||||||
|
|
||||||
|
#include "cartographer/common/make_unique.h"
|
||||||
|
#include "cartographer_ros/metrics/internal/counter.h"
|
||||||
|
#include "cartographer_ros/metrics/internal/gauge.h"
|
||||||
|
#include "cartographer_ros/metrics/internal/histogram.h"
|
||||||
|
|
||||||
|
namespace cartographer_ros {
|
||||||
|
namespace metrics {
|
||||||
|
|
||||||
|
using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries;
|
||||||
|
|
||||||
|
Counter* CounterFamily::Add(const std::map<std::string, std::string>& labels) {
|
||||||
|
auto wrapper = ::cartographer::common::make_unique<Counter>(labels);
|
||||||
|
auto* ptr = wrapper.get();
|
||||||
|
wrappers_.emplace_back(std::move(wrapper));
|
||||||
|
return ptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
cartographer_ros_msgs::MetricFamily CounterFamily::ToRosMessage() {
|
||||||
|
cartographer_ros_msgs::MetricFamily family_msg;
|
||||||
|
family_msg.name = name_;
|
||||||
|
family_msg.description = description_;
|
||||||
|
for (const auto& wrapper : wrappers_) {
|
||||||
|
family_msg.metrics.push_back(wrapper->ToRosMessage());
|
||||||
|
}
|
||||||
|
return family_msg;
|
||||||
|
}
|
||||||
|
|
||||||
|
Gauge* GaugeFamily::Add(const std::map<std::string, std::string>& labels) {
|
||||||
|
auto wrapper = ::cartographer::common::make_unique<Gauge>(labels);
|
||||||
|
auto* ptr = wrapper.get();
|
||||||
|
wrappers_.emplace_back(std::move(wrapper));
|
||||||
|
return ptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
cartographer_ros_msgs::MetricFamily GaugeFamily::ToRosMessage() {
|
||||||
|
cartographer_ros_msgs::MetricFamily family_msg;
|
||||||
|
family_msg.name = name_;
|
||||||
|
family_msg.description = description_;
|
||||||
|
for (const auto& wrapper : wrappers_) {
|
||||||
|
family_msg.metrics.push_back(wrapper->ToRosMessage());
|
||||||
|
}
|
||||||
|
return family_msg;
|
||||||
|
}
|
||||||
|
|
||||||
|
Histogram* HistogramFamily::Add(
|
||||||
|
const std::map<std::string, std::string>& labels) {
|
||||||
|
auto wrapper =
|
||||||
|
::cartographer::common::make_unique<Histogram>(labels, boundaries_);
|
||||||
|
auto* ptr = wrapper.get();
|
||||||
|
wrappers_.emplace_back(std::move(wrapper));
|
||||||
|
return ptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
cartographer_ros_msgs::MetricFamily HistogramFamily::ToRosMessage() {
|
||||||
|
cartographer_ros_msgs::MetricFamily family_msg;
|
||||||
|
family_msg.name = name_;
|
||||||
|
family_msg.description = description_;
|
||||||
|
for (const auto& wrapper : wrappers_) {
|
||||||
|
family_msg.metrics.push_back(wrapper->ToRosMessage());
|
||||||
|
}
|
||||||
|
return family_msg;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace metrics
|
||||||
|
} // namespace cartographer_ros
|
|
@ -0,0 +1,81 @@
|
||||||
|
/*
|
||||||
|
* 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_INTERNAL_FAMILY_H
|
||||||
|
#define CARTOGRAPHER_ROS_METRICS_INTERNAL_FAMILY_H
|
||||||
|
|
||||||
|
#include <memory>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#include "cartographer/metrics/family_factory.h"
|
||||||
|
#include "cartographer_ros/metrics/internal/counter.h"
|
||||||
|
#include "cartographer_ros/metrics/internal/gauge.h"
|
||||||
|
#include "cartographer_ros/metrics/internal/histogram.h"
|
||||||
|
#include "cartographer_ros_msgs/MetricFamily.h"
|
||||||
|
|
||||||
|
namespace cartographer_ros {
|
||||||
|
namespace metrics {
|
||||||
|
class CounterFamily
|
||||||
|
: public ::cartographer::metrics::Family<::cartographer::metrics::Counter> {
|
||||||
|
public:
|
||||||
|
CounterFamily(const std::string& name, const std::string& description)
|
||||||
|
: name_(name), description_(description) {}
|
||||||
|
Counter* Add(const std::map<std::string, std::string>& labels) override;
|
||||||
|
cartographer_ros_msgs::MetricFamily ToRosMessage();
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::string name_;
|
||||||
|
std::string description_;
|
||||||
|
std::vector<std::unique_ptr<Counter>> wrappers_;
|
||||||
|
};
|
||||||
|
|
||||||
|
class GaugeFamily
|
||||||
|
: public ::cartographer::metrics::Family<::cartographer::metrics::Gauge> {
|
||||||
|
public:
|
||||||
|
GaugeFamily(const std::string& name, const std::string& description)
|
||||||
|
: name_(name), description_(description) {}
|
||||||
|
Gauge* Add(const std::map<std::string, std::string>& labels) override;
|
||||||
|
|
||||||
|
cartographer_ros_msgs::MetricFamily ToRosMessage();
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::string name_;
|
||||||
|
std::string description_;
|
||||||
|
std::vector<std::unique_ptr<Gauge>> wrappers_;
|
||||||
|
};
|
||||||
|
|
||||||
|
class HistogramFamily : public ::cartographer::metrics::Family<
|
||||||
|
::cartographer::metrics::Histogram> {
|
||||||
|
public:
|
||||||
|
HistogramFamily(const std::string& name, const std::string& description,
|
||||||
|
const BucketBoundaries& boundaries)
|
||||||
|
: boundaries_(boundaries) {}
|
||||||
|
|
||||||
|
Histogram* Add(const std::map<std::string, std::string>& labels) override;
|
||||||
|
|
||||||
|
cartographer_ros_msgs::MetricFamily ToRosMessage();
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::string name_;
|
||||||
|
std::string description_;
|
||||||
|
std::vector<std::unique_ptr<Histogram>> wrappers_;
|
||||||
|
const BucketBoundaries boundaries_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace metrics
|
||||||
|
} // namespace cartographer_ros
|
||||||
|
|
||||||
|
#endif // CARTOGRAPHER_ROS_METRICS_INTERNAL_FAMILY_H
|
|
@ -0,0 +1,77 @@
|
||||||
|
/*
|
||||||
|
* 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_INTERNAL_GAUGE_H
|
||||||
|
#define CARTOGRAPHER_ROS_METRICS_INTERNAL_GAUGE_H
|
||||||
|
|
||||||
|
#include <atomic>
|
||||||
|
#include <map>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#include "cartographer/metrics/gauge.h"
|
||||||
|
#include "cartographer_ros_msgs/Metric.h"
|
||||||
|
|
||||||
|
namespace cartographer_ros {
|
||||||
|
namespace metrics {
|
||||||
|
|
||||||
|
class Gauge : public ::cartographer::metrics::Gauge {
|
||||||
|
public:
|
||||||
|
explicit Gauge(const std::map<std::string, std::string>& labels)
|
||||||
|
: labels_(labels) {}
|
||||||
|
|
||||||
|
void Decrement(const double by_value) override { Add(-1. * by_value); }
|
||||||
|
|
||||||
|
void Decrement() override { Decrement(1.); }
|
||||||
|
|
||||||
|
void Increment(const double by_value) override { Add(by_value); }
|
||||||
|
|
||||||
|
void Increment() override { Increment(1.); }
|
||||||
|
|
||||||
|
void Set(double value) override {
|
||||||
|
double expected = value_.load();
|
||||||
|
while (!value_.compare_exchange_weak(expected, value)) {
|
||||||
|
;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
double Value() const { return value_.load(); }
|
||||||
|
|
||||||
|
cartographer_ros_msgs::Metric ToRosMessage() {
|
||||||
|
cartographer_ros_msgs::Metric msg;
|
||||||
|
msg.type = cartographer_ros_msgs::Metric::TYPE_GAUGE;
|
||||||
|
for (const auto& label : labels_) {
|
||||||
|
cartographer_ros_msgs::MetricLabel label_msg;
|
||||||
|
label_msg.key = label.first;
|
||||||
|
label_msg.value = label.second;
|
||||||
|
msg.labels.push_back(label_msg);
|
||||||
|
}
|
||||||
|
msg.value = Value();
|
||||||
|
return msg;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
void Add(const double value) {
|
||||||
|
double current = Value();
|
||||||
|
Set(current + value);
|
||||||
|
}
|
||||||
|
|
||||||
|
const std::map<std::string, std::string> labels_;
|
||||||
|
std::atomic<double> value_{0.};
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace metrics
|
||||||
|
} // namespace cartographer_ros
|
||||||
|
|
||||||
|
#endif // CARTOGRAPHER_ROS_METRICS_INTERNAL_GAUGE_H
|
|
@ -0,0 +1,94 @@
|
||||||
|
/*
|
||||||
|
* 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/internal/histogram.h"
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <numeric>
|
||||||
|
|
||||||
|
#include "glog/logging.h"
|
||||||
|
|
||||||
|
namespace cartographer_ros {
|
||||||
|
namespace metrics {
|
||||||
|
|
||||||
|
using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries;
|
||||||
|
|
||||||
|
Histogram::Histogram(const std::map<std::string, std::string>& labels,
|
||||||
|
const BucketBoundaries& bucket_boundaries)
|
||||||
|
: labels_(labels),
|
||||||
|
bucket_boundaries_(bucket_boundaries),
|
||||||
|
bucket_counts_(bucket_boundaries.size() + 1) {
|
||||||
|
::cartographer::common::MutexLocker lock(&mutex_);
|
||||||
|
CHECK(std::is_sorted(std::begin(bucket_boundaries_),
|
||||||
|
std::end(bucket_boundaries_)));
|
||||||
|
}
|
||||||
|
|
||||||
|
Histogram::Histogram(const BucketBoundaries& bucket_boundaries)
|
||||||
|
: Histogram({}, bucket_boundaries) {}
|
||||||
|
|
||||||
|
void Histogram::Observe(double value) {
|
||||||
|
auto bucket_index =
|
||||||
|
std::distance(bucket_boundaries_.begin(),
|
||||||
|
std::upper_bound(bucket_boundaries_.begin(),
|
||||||
|
bucket_boundaries_.end(), value));
|
||||||
|
// TODO: check lock contention and potential for atomic operations.
|
||||||
|
::cartographer::common::MutexLocker lock(&mutex_);
|
||||||
|
sum_ += value;
|
||||||
|
bucket_counts_[bucket_index] += 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::map<double, double> Histogram::CountsByBucket() {
|
||||||
|
::cartographer::common::MutexLocker lock(&mutex_);
|
||||||
|
std::map<double, double> counts_by_bucket;
|
||||||
|
// Add the finite buckets.
|
||||||
|
for (size_t i = 0; i < bucket_boundaries_.size(); ++i) {
|
||||||
|
counts_by_bucket[bucket_boundaries_.at(i)] = bucket_counts_.at(i);
|
||||||
|
}
|
||||||
|
// Add the "infinite" bucket.
|
||||||
|
counts_by_bucket[kInfiniteBoundary] = bucket_counts_.back();
|
||||||
|
return counts_by_bucket;
|
||||||
|
}
|
||||||
|
|
||||||
|
double Histogram::Sum() {
|
||||||
|
::cartographer::common::MutexLocker lock(&mutex_);
|
||||||
|
return sum_;
|
||||||
|
}
|
||||||
|
|
||||||
|
double Histogram::CumulativeCount() {
|
||||||
|
::cartographer::common::MutexLocker lock(&mutex_);
|
||||||
|
return std::accumulate(bucket_counts_.begin(), bucket_counts_.end(), 0.);
|
||||||
|
}
|
||||||
|
|
||||||
|
cartographer_ros_msgs::Metric Histogram::ToRosMessage() {
|
||||||
|
cartographer_ros_msgs::Metric msg;
|
||||||
|
msg.type = cartographer_ros_msgs::Metric::TYPE_HISTOGRAM;
|
||||||
|
for (const auto& label : labels_) {
|
||||||
|
cartographer_ros_msgs::MetricLabel label_msg;
|
||||||
|
label_msg.key = label.first;
|
||||||
|
label_msg.value = label.second;
|
||||||
|
msg.labels.push_back(label_msg);
|
||||||
|
}
|
||||||
|
for (const auto& bucket : CountsByBucket()) {
|
||||||
|
cartographer_ros_msgs::HistogramBucket bucket_msg;
|
||||||
|
bucket_msg.bucket_boundary = bucket.first;
|
||||||
|
bucket_msg.count = bucket.second;
|
||||||
|
msg.counts_by_bucket.push_back(bucket_msg);
|
||||||
|
}
|
||||||
|
return msg;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace metrics
|
||||||
|
} // namespace cartographer_ros
|
|
@ -0,0 +1,62 @@
|
||||||
|
/*
|
||||||
|
* 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_INTERNAL_HISTOGRAM_H
|
||||||
|
#define CARTOGRAPHER_ROS_METRICS_INTERNAL_HISTOGRAM_H
|
||||||
|
|
||||||
|
#include <map>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "cartographer/common/mutex.h"
|
||||||
|
#include "cartographer/metrics/histogram.h"
|
||||||
|
#include "cartographer_ros_msgs/Metric.h"
|
||||||
|
|
||||||
|
namespace cartographer_ros {
|
||||||
|
namespace metrics {
|
||||||
|
|
||||||
|
constexpr double kInfiniteBoundary = std::numeric_limits<double>::infinity();
|
||||||
|
|
||||||
|
using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries;
|
||||||
|
|
||||||
|
class Histogram : public ::cartographer::metrics::Histogram {
|
||||||
|
public:
|
||||||
|
explicit Histogram(const std::map<std::string, std::string>& labels,
|
||||||
|
const BucketBoundaries& bucket_boundaries);
|
||||||
|
|
||||||
|
explicit Histogram(const BucketBoundaries& bucket_boundaries);
|
||||||
|
|
||||||
|
void Observe(double value) override;
|
||||||
|
|
||||||
|
std::map<double, double> CountsByBucket();
|
||||||
|
|
||||||
|
double Sum();
|
||||||
|
|
||||||
|
double CumulativeCount();
|
||||||
|
|
||||||
|
cartographer_ros_msgs::Metric ToRosMessage();
|
||||||
|
|
||||||
|
private:
|
||||||
|
cartographer::common::Mutex mutex_;
|
||||||
|
const std::map<std::string, std::string> labels_;
|
||||||
|
const BucketBoundaries bucket_boundaries_;
|
||||||
|
std::vector<double> bucket_counts_ GUARDED_BY(mutex_);
|
||||||
|
double sum_ GUARDED_BY(mutex_);
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace metrics
|
||||||
|
} // namespace cartographer_ros
|
||||||
|
|
||||||
|
#endif // CARTOGRAPHER_ROS_METRICS_INTERNAL_HISTOGRAM_H
|
|
@ -0,0 +1,104 @@
|
||||||
|
/*
|
||||||
|
* 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 <algorithm>
|
||||||
|
#include <array>
|
||||||
|
#include <numeric>
|
||||||
|
|
||||||
|
#include "cartographer/metrics/histogram.h"
|
||||||
|
#include "cartographer_ros/metrics/internal/counter.h"
|
||||||
|
#include "cartographer_ros/metrics/internal/gauge.h"
|
||||||
|
#include "cartographer_ros/metrics/internal/histogram.h"
|
||||||
|
#include "gtest/gtest.h"
|
||||||
|
|
||||||
|
namespace cartographer_ros {
|
||||||
|
namespace metrics {
|
||||||
|
|
||||||
|
TEST(Metrics, GaugeTest) {
|
||||||
|
Gauge gauge({});
|
||||||
|
EXPECT_EQ(gauge.Value(), 0.);
|
||||||
|
gauge.Increment(1.2);
|
||||||
|
EXPECT_EQ(gauge.Value(), 1.2);
|
||||||
|
gauge.Increment();
|
||||||
|
EXPECT_EQ(gauge.Value(), 2.2);
|
||||||
|
gauge.Decrement(2.2);
|
||||||
|
EXPECT_EQ(gauge.Value(), 0.);
|
||||||
|
gauge.Decrement();
|
||||||
|
EXPECT_EQ(gauge.Value(), -1.);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(Metrics, CounterTest) {
|
||||||
|
Counter counter({});
|
||||||
|
EXPECT_EQ(counter.Value(), 0.);
|
||||||
|
counter.Increment(1.2);
|
||||||
|
EXPECT_EQ(counter.Value(), 1.2);
|
||||||
|
counter.Increment(0.8);
|
||||||
|
EXPECT_EQ(counter.Value(), 2.);
|
||||||
|
counter.Increment();
|
||||||
|
EXPECT_EQ(counter.Value(), 3.);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(Metrics, HistogramFixedWidthTest) {
|
||||||
|
auto boundaries = ::cartographer::metrics::Histogram::FixedWidth(1, 3);
|
||||||
|
Histogram histogram(boundaries);
|
||||||
|
|
||||||
|
// Observe some values that fit in finite buckets.
|
||||||
|
std::array<double, 3> values = {{0., 2, 2.5}};
|
||||||
|
for (const auto& value : values) {
|
||||||
|
histogram.Observe(value);
|
||||||
|
}
|
||||||
|
// 1 2 3 inf
|
||||||
|
// 1 | 0 | 2 | 0 |
|
||||||
|
EXPECT_EQ(histogram.CountsByBucket()[1], 1);
|
||||||
|
EXPECT_EQ(histogram.CountsByBucket()[2], 0);
|
||||||
|
EXPECT_EQ(histogram.CountsByBucket()[3], 2);
|
||||||
|
EXPECT_EQ(histogram.CumulativeCount(), values.size());
|
||||||
|
EXPECT_EQ(histogram.Sum(), std::accumulate(values.begin(), values.end(), 0.));
|
||||||
|
|
||||||
|
// Values above the last bucket boundary should go to the "infinite" bucket.
|
||||||
|
histogram.Observe(3.5);
|
||||||
|
// 1 2 3 inf
|
||||||
|
// 1 | 0 | 2 | 1 |
|
||||||
|
EXPECT_EQ(histogram.CountsByBucket()[kInfiniteBoundary], 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(Metrics, HistogramScaledPowersOfTest) {
|
||||||
|
auto boundaries =
|
||||||
|
::cartographer::metrics::Histogram::ScaledPowersOf(2, 1, 2048);
|
||||||
|
Histogram histogram(boundaries);
|
||||||
|
|
||||||
|
// Observe some values that fit in finite buckets.
|
||||||
|
std::array<double, 3> values = {{256, 512, 666}};
|
||||||
|
for (const auto& value : values) {
|
||||||
|
histogram.Observe(value);
|
||||||
|
}
|
||||||
|
// ... 256 512 1024 inf
|
||||||
|
// ... | 1 | 2 | 0 |
|
||||||
|
EXPECT_EQ(histogram.CountsByBucket()[256], 0);
|
||||||
|
EXPECT_EQ(histogram.CountsByBucket()[512], 1);
|
||||||
|
EXPECT_EQ(histogram.CountsByBucket()[1024], 2);
|
||||||
|
EXPECT_EQ(histogram.CumulativeCount(), values.size());
|
||||||
|
EXPECT_EQ(histogram.Sum(), std::accumulate(values.begin(), values.end(), 0.));
|
||||||
|
|
||||||
|
// Values above the last bucket boundary should go to the "infinite" bucket.
|
||||||
|
histogram.Observe(2048);
|
||||||
|
// ... 256 512 1024 inf
|
||||||
|
// ... | 1 | 2 | 1 |
|
||||||
|
EXPECT_EQ(histogram.CountsByBucket()[kInfiniteBoundary], 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace metrics
|
||||||
|
} // namespace cartographer_ros
|
|
@ -24,26 +24,18 @@ set(PACKAGE_DEPENDENCIES
|
||||||
find_package(catkin REQUIRED COMPONENTS message_generation ${PACKAGE_DEPENDENCIES})
|
find_package(catkin REQUIRED COMPONENTS message_generation ${PACKAGE_DEPENDENCIES})
|
||||||
|
|
||||||
add_message_files(
|
add_message_files(
|
||||||
FILES
|
DIRECTORY
|
||||||
LandmarkEntry.msg
|
msg
|
||||||
LandmarkList.msg
|
)
|
||||||
StatusCode.msg
|
|
||||||
StatusResponse.msg
|
add_message_files(
|
||||||
SubmapList.msg
|
DIRECTORY
|
||||||
SubmapEntry.msg
|
msg/metrics
|
||||||
SubmapTexture.msg
|
|
||||||
SensorTopics.msg
|
|
||||||
TrajectoryOptions.msg
|
|
||||||
TrajectoryStates.msg
|
|
||||||
)
|
)
|
||||||
|
|
||||||
add_service_files(
|
add_service_files(
|
||||||
FILES
|
DIRECTORY
|
||||||
FinishTrajectory.srv
|
srv
|
||||||
GetTrajectoryStates.srv
|
|
||||||
StartTrajectory.srv
|
|
||||||
SubmapQuery.srv
|
|
||||||
WriteState.srv
|
|
||||||
)
|
)
|
||||||
|
|
||||||
generate_messages(
|
generate_messages(
|
||||||
|
|
|
@ -0,0 +1,19 @@
|
||||||
|
# 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.
|
||||||
|
|
||||||
|
# A histogram bucket counts values x for which:
|
||||||
|
# previous_boundary < x <= bucket_boundary
|
||||||
|
# holds.
|
||||||
|
float64 bucket_boundary
|
||||||
|
float64 count
|
|
@ -0,0 +1,26 @@
|
||||||
|
# 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.
|
||||||
|
|
||||||
|
uint8 TYPE_COUNTER=0
|
||||||
|
uint8 TYPE_GAUGE=1
|
||||||
|
uint8 TYPE_HISTOGRAM=2
|
||||||
|
|
||||||
|
uint8 type
|
||||||
|
cartographer_ros_msgs/MetricLabel[] labels
|
||||||
|
|
||||||
|
# TYPE_COUNTER or TYPE_GAUGE
|
||||||
|
float64 value
|
||||||
|
|
||||||
|
# TYPE_HISTOGRAM
|
||||||
|
cartographer_ros_msgs/HistogramBucket[] counts_by_bucket
|
|
@ -0,0 +1,17 @@
|
||||||
|
# 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.
|
||||||
|
|
||||||
|
string name
|
||||||
|
string description
|
||||||
|
cartographer_ros_msgs/Metric[] metrics
|
|
@ -0,0 +1,16 @@
|
||||||
|
# 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.
|
||||||
|
|
||||||
|
string key
|
||||||
|
string value
|
Loading…
Reference in New Issue