subscription_topic_statistics.hpp

// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved.
//
// 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 RCLCPP__TOPIC_STATISTICS__SUBSCRIPTION_TOPIC_STATISTICS_HPP_
#define RCLCPP__TOPIC_STATISTICS__SUBSCRIPTION_TOPIC_STATISTICS_HPP_
 
#include <memory>
#include <string>
#include <utility>
#include <vector>
 
#include "libstatistics_collector/collector/generate_statistics_message.hpp"
#include "libstatistics_collector/moving_average_statistics/types.hpp"
#include "libstatistics_collector/topic_statistics_collector/constants.hpp"
#include "libstatistics_collector/topic_statistics_collector/received_message_age.hpp"
#include "libstatistics_collector/topic_statistics_collector/received_message_period.hpp"
 
#include "rcl/time.h"
#include "rclcpp/time.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp/timer.hpp"
 
#include "statistics_msgs/msg/metrics_message.hpp"
 
namespace rclcpp
{
namespace topic_statistics
{
 
constexpr const char kDefaultPublishTopicName[]{"/statistics"};
constexpr const std::chrono::milliseconds kDefaultPublishingPeriod{std::chrono::seconds(1)};
 
using libstatistics_collector::collector::GenerateStatisticMessage;
using statistics_msgs::msg::MetricsMessage;
using libstatistics_collector::moving_average_statistics::StatisticData;
 
class SubscriptionTopicStatistics
{
  using TopicStatsCollector = libstatistics_collector::TopicStatisticsCollector;
  using ReceivedMessageAge = libstatistics_collector::ReceivedMessageAgeCollector;
  using ReceivedMessagePeriod = libstatistics_collector::ReceivedMessagePeriodCollector;
 
public:
 
  SubscriptionTopicStatistics(
    const std::string & node_name,
    rclcpp::Publisher<statistics_msgs::msg::MetricsMessage>::SharedPtr publisher)
  : node_name_(node_name),
    publisher_(std::move(publisher))
  {
    // TODO(dbbonnie): ros-tooling/aws-roadmap/issues/226, received message age
 
    if (nullptr == publisher_) {
      throw std::invalid_argument("publisher pointer is nullptr");
    }
 
    bring_up();
  }
 
  virtual ~SubscriptionTopicStatistics()
  {
    tear_down();
  }
 
 
  virtual void handle_message(
    const rmw_message_info_t & message_info,
    const rclcpp::Time now_nanoseconds) const
  {
    std::lock_guard<std::mutex> lock(mutex_);
    for (const auto & collector : subscriber_statistics_collectors_) {
      collector->OnMessageReceived(message_info, now_nanoseconds.nanoseconds());
    }
  }
 
 
  void set_publisher_timer(rclcpp::TimerBase::SharedPtr publisher_timer)
  {
    publisher_timer_ = publisher_timer;
  }
 
 
  virtual void publish_message_and_reset_measurements()
  {
    std::vector<MetricsMessage> msgs;
    rclcpp::Time window_end{get_current_nanoseconds_since_epoch()};
 
    {
      std::lock_guard<std::mutex> lock(mutex_);
      for (auto & collector : subscriber_statistics_collectors_) {
        const auto collected_stats = collector->GetStatisticsResults();
        collector->ClearCurrentMeasurements();
 
        auto message = libstatistics_collector::collector::GenerateStatisticMessage(
          node_name_,
          collector->GetMetricName(),
          collector->GetMetricUnit(),
          window_start_,
          window_end,
          collected_stats);
        msgs.push_back(message);
      }
    }
 
    for (auto & msg : msgs) {
      publisher_->publish(msg);
    }
    window_start_ = window_end;
  }
 
protected:
 
  std::vector<StatisticData> get_current_collector_data() const
  {
    std::vector<StatisticData> data;
    std::lock_guard<std::mutex> lock(mutex_);
    for (const auto & collector : subscriber_statistics_collectors_) {
      data.push_back(collector->GetStatisticsResults());
    }
    return data;
  }
 
private:
 
  void bring_up()
  {
    auto received_message_age = std::make_unique<ReceivedMessageAge>();
    received_message_age->Start();
    subscriber_statistics_collectors_.emplace_back(std::move(received_message_age));
 
    auto received_message_period = std::make_unique<ReceivedMessagePeriod>();
    received_message_period->Start();
    {
      std::lock_guard<std::mutex> lock(mutex_);
      subscriber_statistics_collectors_.emplace_back(std::move(received_message_period));
    }
 
    window_start_ = rclcpp::Time(get_current_nanoseconds_since_epoch());
  }
 
 
  void tear_down()
  {
    {
      std::lock_guard<std::mutex> lock(mutex_);
      for (auto & collector : subscriber_statistics_collectors_) {
        collector->Stop();
      }
 
      subscriber_statistics_collectors_.clear();
    }
 
    if (publisher_timer_) {
      publisher_timer_->cancel();
      publisher_timer_.reset();
    }
 
    publisher_.reset();
  }
 
 
  int64_t get_current_nanoseconds_since_epoch() const
  {
    const auto now = std::chrono::system_clock::now();
    return std::chrono::duration_cast<std::chrono::nanoseconds>(now.time_since_epoch()).count();
  }
 
  mutable std::mutex mutex_;
  std::vector<std::unique_ptr<TopicStatsCollector>> subscriber_statistics_collectors_{};
  const std::string node_name_;
  rclcpp::Publisher<statistics_msgs::msg::MetricsMessage>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr publisher_timer_;
  rclcpp::Time window_start_;
};
}  // namespace topic_statistics
}  // namespace rclcpp
 
#endif  // RCLCPP__TOPIC_STATISTICS__SUBSCRIPTION_TOPIC_STATISTICS_HPP_