LogManager.h
Go to the documentation of this file.
00001 #ifndef __LOG_MANAGER_H__
00002 #define __LOG_MANAGER_H__
00003 
00004 #include <iostream>
00005 #include <sys/time.h>
00006 #include <deque>
00007 #include <boost/thread/thread.hpp>
00008 #include "LogManagerBase.h"
00009 
00010 template<class T>
00011 class LogManager : public LogManagerBase
00012 {
00013 public:
00014     LogManager() : m_index(-1), m_isNewStateAdded(false), m_atLast(true),
00015         m_maxLogLength(0){
00016     }
00017     void add(const T& state){
00018         boost::mutex::scoped_lock lock(m_mutex);
00019         m_log.push_back(state);
00020         if (m_log.size() == 1) m_offsetT = state.time;
00021         if (m_maxLogLength > 0 && m_log.size() > m_maxLogLength) {
00022             m_log.pop_front();
00023             if (m_index >= 1) m_index--;
00024         }
00025         m_isNewStateAdded = true;
00026     }
00027     void clear(){
00028         boost::mutex::scoped_lock lock(m_mutex);
00029         m_isPlaying = false;
00030         m_log.clear();
00031         m_index = -1;
00032         m_atLast = true;
00033     }
00034     void prev(int delta=1){ 
00035         boost::mutex::scoped_lock lock(m_mutex);
00036         setIndex(m_index - delta); 
00037     }
00038     void next(int delta=1){ 
00039         boost::mutex::scoped_lock lock(m_mutex);
00040         setIndex(m_index + delta); 
00041     }
00042     void head(){ 
00043         boost::mutex::scoped_lock lock(m_mutex);
00044         setIndex(0); 
00045     }
00046     void tail(){ 
00047         boost::mutex::scoped_lock lock(m_mutex);
00048         if (!m_log.empty()) setIndex(m_log.size()-1); 
00049     } 
00050     void move(double ratio){
00051         boost::mutex::scoped_lock lock(m_mutex);
00052         if (m_log.size()) setIndex(ratio*(m_log.size()-1));
00053     }
00054     bool isNewStateAdded() { return m_isNewStateAdded; }
00055     double currentTime() { 
00056         boost::mutex::scoped_lock lock(m_mutex);
00057         if (!m_log.empty() && m_index>=0){
00058             return m_log[m_index].time - m_offsetT;
00059         }else{
00060             return -1;
00061         }
00062     }
00063     int index() { return m_index; }
00064     double time(int i) { 
00065         boost::mutex::scoped_lock lock(m_mutex);
00066         return m_log[i].time; 
00067     }
00068     void faster(){
00069         boost::mutex::scoped_lock lock(m_mutex);
00070         m_playRatio *= 2;
00071         if (m_isPlaying){
00072             m_initT = m_log[m_index].time;
00073             gettimeofday(&m_startT, NULL);
00074         }
00075     }
00076     void slower(){
00077         boost::mutex::scoped_lock lock(m_mutex);
00078         m_playRatio /= 2;
00079         if (m_isPlaying){
00080             m_initT = m_log[m_index].time;
00081             gettimeofday(&m_startT, NULL);
00082         }
00083     }
00084     bool record(double i_fps){
00085         boost::mutex::scoped_lock lock(m_mutex);
00086         if (m_log.empty()) return false;
00087 
00088         if (m_atLast) setIndex(0);
00089         m_initT = m_log[0].time;
00090         m_isRecording = true;
00091         m_fps = i_fps;
00092         return true;
00093     }
00094     bool isRecording() { return m_isRecording; }
00095     void play(){
00096         boost::mutex::scoped_lock lock(m_mutex);
00097         if (m_log.empty()) return;
00098 
00099         if (!m_isPlaying){
00100             m_isPlaying = true;
00101             if (m_atLast) setIndex(0);
00102             m_initT = m_log[m_index].time;
00103             gettimeofday(&m_startT, NULL);
00104         }else{
00105             m_isPlaying = false;
00106         }
00107     }
00108     int updateIndex(){
00109         boost::mutex::scoped_lock lock(m_mutex);
00110         if (m_isPlaying){
00111             // compute time to draw
00112             struct timeval tv;
00113             gettimeofday(&tv, NULL);
00114             double drawT = m_initT + ((tv.tv_sec - m_startT.tv_sec) + (tv.tv_usec - m_startT.tv_usec)*1e-6)*m_playRatio;
00115             //
00116             while(drawT > m_log[m_index].time){
00117                 setIndex(m_index+1);
00118                 if (m_atLast) {
00119                     m_isPlaying = false;
00120                     break;
00121                 }
00122             }
00123         } else if (m_isNewStateAdded && m_atLast){
00124             // draw newest state
00125             setIndex(m_log.size() - 1);
00126             m_isNewStateAdded = false;
00127         }
00128         if(m_isRecording){
00129             while(m_initT > m_log[m_index].time){
00130                 setIndex(m_index+1);
00131                 if (m_atLast) {
00132                     m_isRecording = false;
00133                     break;
00134                 }
00135             } 
00136             m_initT += 1.0/m_fps*m_playRatio;
00137         }
00138         return m_index;
00139     }
00140     T& state() { 
00141         boost::mutex::scoped_lock lock(m_mutex);
00142         if (m_index < 0 || m_index >= m_log.size()){
00143             std::cerr << "invalid index:" << m_index << "," << m_log.size()
00144                       << std::endl;
00145         }
00146         return m_log[m_index]; 
00147     }
00148     bool state(T& o_state){
00149         boost::mutex::scoped_lock lock(m_mutex);
00150         if (m_index < 0 || m_index >= m_log.size()){
00151             return false;
00152         }else{
00153             o_state = m_log[m_index]; 
00154             return true;
00155         }
00156     }
00157     void enableRingBuffer(int len) { m_maxLogLength = len; }
00158     unsigned int length() { 
00159         boost::mutex::scoped_lock lock(m_mutex);
00160         return m_log.size(); 
00161     }
00162     double time() {
00163         boost::mutex::scoped_lock lock(m_mutex);
00164         if (m_log.size() < m_index && m_index >= 0){
00165             return -1;
00166         }else{
00167             return m_log[m_index].time;
00168         }
00169     }
00170 protected:
00171     void setIndex(int i){
00172         if (m_log.empty()) return;
00173         
00174         m_index = i;
00175         if (m_index < 0) m_index = 0;
00176         if (m_index >= m_log.size()) m_index = m_log.size()-1;
00177         m_atLast = m_index == m_log.size()-1; 
00178     }
00179 
00180     std::deque<T> m_log;
00181     int m_index;
00182     bool m_isNewStateAdded, m_atLast;
00183     double m_initT;
00184     struct timeval m_startT;
00185     int m_maxLogLength;
00186     double m_offsetT;
00187     boost::mutex m_mutex; 
00188 };
00189 #endif


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55