Program Listing for File pal_statistics.hpp

↰ Return to documentation for file (include/pal_statistics/pal_statistics.hpp)

// Copyright 2020 PAL Robotics S.L.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright
//      notice, this list of conditions and the following disclaimer.
//
//    * Redistributions in binary form must reproduce the above copyright
//      notice, this list of conditions and the following disclaimer in the
//      documentation and/or other materials provided with the distribution.
//
//    * Neither the name of the PAL Robotics S.L. nor the names of its
//      contributors may be used to endorse or promote products derived from
//      this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.


#ifndef PAL_STATISTICS__PAL_STATISTICS_HPP_
#define PAL_STATISTICS__PAL_STATISTICS_HPP_

#include <memory>
#include <mutex>
#include <string>

#include "pal_statistics/pal_statistics_utils.hpp"

#include "pal_statistics_msgs/msg/statistics.hpp"
#include "pal_statistics_msgs/msg/statistics_names.hpp"
#include "pal_statistics_msgs/msg/statistics_values.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"

// fordward declaration to make it friend of StatisticsRegistry
template<typename NodeT>
class PalStatisticsTestHelperClass;

namespace pal_statistics
{

// forward declarations
template<typename T>
class LockFreeQueue;
struct EnabledId;
class RegistrationList;

class StatisticsRegistry : public std::enable_shared_from_this<StatisticsRegistry>
{
public:
  StatisticsRegistry(
    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface,
    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface,
    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_interface,
    const rclcpp::node_interfaces::NodeClockInterface::SharedPtr & clock_interface,
    const std::string & topic);

  StatisticsRegistry(const std::shared_ptr<rclcpp::Node> & node, const std::string & topic);
  StatisticsRegistry(
    const rclcpp_lifecycle::LifecycleNode::SharedPtr & node,
    const std::string & topic);

  virtual ~StatisticsRegistry();

  IdType registerVariable(
    const std::string & name, const double * variable, RegistrationsRAII * bookkeeping = NULL,
    bool enabled = true);

  IdType registerFunction(
    const std::string & name, const std::function<double()> & funct,
    RegistrationsRAII * bookkeeping = NULL, bool enabled = true);


  void unregisterVariable(const std::string & name, RegistrationsRAII * bookkeeping = NULL);
  void unregisterVariable(IdType id, RegistrationsRAII * bookkeeping = NULL);

  void publish();

  bool publishAsync();

  void startPublishThread();

  pal_statistics_msgs::msg::Statistics createMsg();

  template<typename T>
  void publishCustomStatistic(const std::string & name, T value)
  {
    pal_statistics_msgs::msg::Statistics msg;
    pal_statistics_msgs::msg::Statistic stat;
    stat.name = name;
    stat.value = static_cast<double>(value);
    msg.statistics.push_back(stat);
    msg.header.stamp = clock_->now();
    pub_->publish(msg);
  }

  void publishCustomStatistics(const pal_statistics_msgs::msg::Statistics & msg)
  {
    pub_->publish(msg);
  }

  bool enable(const IdType & id);
  bool disable(const IdType & id);

protected:
  void joinPublisherThread();

private:
  void updateMsgUnsafe();

  bool updateMsg(
    pal_statistics_msgs::msg::StatisticsNames & names,
    pal_statistics_msgs::msg::StatisticsValues & values, bool smart_fill = false);

  void publisherThreadCycle();

  IdType registerInternal(
    const std::string & name, VariableHolder && variable,
    RegistrationsRAII * bookkeeping, bool enabled);

  bool setEnabledmpl(const IdType & id, bool enabled);

  void handlePendingDisables(const std::unique_lock<std::mutex> & data_lock);

  void doPublish(bool publish_names_msg = true);

  const rclcpp::Logger & getLogger() const;

  rclcpp::Logger logger_;
  rclcpp::Clock::SharedPtr clock_;

  std::mutex data_mutex_;
  std::unique_ptr<RegistrationList> registration_list_;

  std::unique_ptr<LockFreeQueue<EnabledId>> enabled_ids_;


  // To avoid deadlocks, should always be acquired after data_mutex_
  std::mutex pub_mutex_;
  rclcpp::Publisher<pal_statistics_msgs::msg::Statistics>::SharedPtr pub_;
  rclcpp::Publisher<pal_statistics_msgs::msg::StatisticsNames>::SharedPtr pub_names_;
  rclcpp::Publisher<pal_statistics_msgs::msg::StatisticsValues>::SharedPtr pub_values_;

  std::atomic<bool> is_data_ready_;
  std::atomic<bool> interrupt_thread_;
  std::shared_ptr<std::thread> publisher_thread_;

  struct GeneratedStatistics
  {
    GeneratedStatistics()
    : last_names_version_(-1)
    {
    }
    void update(
      const pal_statistics_msgs::msg::StatisticsNames & names,
      const pal_statistics_msgs::msg::StatisticsValues & values);

    pal_statistics_msgs::msg::Statistics msg_;
    unsigned int last_names_version_;
  };

  pal_statistics_msgs::msg::StatisticsNames names_msg_;
  pal_statistics_msgs::msg::StatisticsValues values_msg_;
  GeneratedStatistics generated_statistics_;
  // Internal stats
  unsigned int publish_async_attempts_;
  unsigned int publish_async_failures_;
  double last_async_pub_duration_;
  RegistrationsRAII internal_stats_raii_;

  template<typename NodeT>
  friend class ::PalStatisticsTestHelperClass;
};
}  // namespace pal_statistics
#endif  // PAL_STATISTICS__PAL_STATISTICS_HPP_