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_;
  std::atomic<bool> interrupt_thread_;
  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