pal_statistics.h

Go to the documentation of this file.

#ifndef _PAL_STATISTICS_H_
#define _PAL_STATISTICS_H_

#include <ros/ros.h>
#include <boost/thread/lock_guard.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/condition_variable.hpp>
#include <boost/thread/thread.hpp>
#include <pal_statistics_msgs/Statistics.h>
#include <pal_statistics/pal_statistics_utils.h>
#include <gtest/gtest_prod.h>

namespace pal_statistics
{
class StatisticsRegistry : public boost::enable_shared_from_this<StatisticsRegistry>
{
public:
  StatisticsRegistry(const std::string &topic);

  virtual ~StatisticsRegistry();

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

  IdType registerFunction(const std::string &name, const boost::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::Statistics createMsg();

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

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

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

private:
  void updateMsgUnsafe();

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

  void publisherThreadCycle();

  void startPublishThreadImpl();

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

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

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

  void doPublish(bool publish_names_msg = true);

  ros::NodeHandle nh_;

  boost::mutex data_mutex_;
  RegistrationList registration_list_;

  struct EnabledId
  {
    //Can't use a pair because it's not trivially copiable
    IdType id;
    bool enabled;
  };

  LockFreeQueue<EnabledId> enabled_ids_;


  // To avoid deadlocks, should always be acquired after data_mutex_
  boost::mutex pub_mutex_;
  ros::Publisher pub_;
  ros::Publisher pub_names_;
  ros::Publisher pub_values_;

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

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

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

  pal_statistics_msgs::StatisticsNames names_msg_;
  pal_statistics_msgs::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_;

  FRIEND_TEST(PalStatisticsTest, stressAsync);
};
}  // namespace pal_statistics
#endif