src/debug/real-time-logger.cpp

Go to the documentation of this file.

/*
 * Copyright 2018,
 * Joseph Mirabel
 *
 * LAAS-CNRS
 *
 */
 
#include <dynamic-graph/real-time-logger.h>
 
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/thread/thread.hpp>
 
namespace dynamicgraph {
RealTimeLogger::RealTimeLogger(const std::size_t &bufferSize)
    : buffer_(bufferSize, NULL),
      frontIdx_(0),
      backIdx_(0),
      oss_(NULL),
      nbDiscarded_(0) {
  for (std::size_t i = 0; i < buffer_.size(); ++i) buffer_[i] = new Data;
}
 
RealTimeLogger::~RealTimeLogger() {
  // Check that we are not spinning...
  for (std::size_t i = 0; i < buffer_.size(); ++i) delete buffer_[i];
}
 
bool RealTimeLogger::spinOnce() {
  if (empty()) return false;
  Data *data = buffer_[frontIdx_];
  frontIdx_ = (frontIdx_ + 1) % buffer_.size();
  std::string str = data->buf.str();
  // It is important to pass str.c_str() and not str
  // because the str object may contains a '\0' so
  // str.size() may be different from strlen(str.c_str())
  for (std::size_t i = 0; i < outputs_.size(); ++i)
    outputs_[i]->write(str.c_str());
  return true;
}
 
RTLoggerStream RealTimeLogger::front() {
  // If no output or if buffer is full, discard message.
  if (outputs_.empty() || full()) {
    nbDiscarded_++;
    return RTLoggerStream(NULL, oss_);
  }
  bool alone = wmutex.try_lock();
  // If someone is writting, discard message.
  if (!alone) {
    nbDiscarded_++;
    return RTLoggerStream(NULL, oss_);
  }
  Data *data = buffer_[backIdx_];
  // backIdx_ = (backIdx_+1) % buffer_.size();
  // Reset position of cursor
  data->buf.pubseekpos(0);
  oss_.rdbuf(&data->buf);
  return RTLoggerStream(this, oss_);
}
 
struct RealTimeLogger::thread {
  bool requestShutdown_;
  int threadPolicy_;
  int threadPriority_;
  bool changedThreadParams;
  boost::thread t_;
 
  explicit thread(RealTimeLogger *logger)
      : requestShutdown_(false),
        threadPolicy_(SCHED_OTHER),
        threadPriority_(0),
        changedThreadParams(true),
        t_(&thread::spin, this, logger) {}
 
  //  void setThreadPolicy(int policy) {
  //  threadPolicy_ = policy;
  //  changedThreadParams = true;
  // }
 
  // void setPriority(int priority) {
  //    threadPriority_ = priority;
  //    changedThreadParams = true;
  //  }
 
  //  int getThreadPolicy() { return threadPolicy_; }
  // int getThreadPriority() { return threadPriority_; }
 
  void changeThreadParams() {
    int threadPolicy;
    struct sched_param threadParam;
    if (pthread_getschedparam(pthread_self(), &threadPolicy, &threadParam) ==
        0) {
      threadPolicy = threadPolicy_;
      threadParam.sched_priority = threadPriority_;
      if (threadParam.sched_priority < sched_get_priority_min(threadPolicy))
        threadParam.sched_priority = sched_get_priority_min(threadPolicy);
 
      pthread_setschedparam(pthread_self(), threadPolicy, &threadParam);
      changedThreadParams = false;
    }
  }
 
  void spin(RealTimeLogger *logger) {
    // Change the thread's scheduler from real-time to normal
    // and reduce its priority
 
    while (!requestShutdown_ || !logger->empty()) {
      // If the logger did not write anything, it means the buffer is empty.
      // Do a pause
      if (!logger->spinOnce())
        boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
      if (changedThreadParams) changeThreadParams();
    }
  }
};
 
RealTimeLogger *RealTimeLogger::instance_ = NULL;
RealTimeLogger::thread *RealTimeLogger::thread_ = NULL;
 
RealTimeLogger &RealTimeLogger::instance() {
  if (instance_ == NULL) {
    instance_ = new RealTimeLogger(1000);
    thread_ = new thread(instance_);
  }
  return *instance_;
}
 
void RealTimeLogger::destroy() {
  if (instance_ == NULL) return;
  thread_->requestShutdown_ = true;
  thread_->t_.join();
  delete instance_;
  delete thread_;
}
}  // namespace dynamicgraph