/home/toby/moos-ivp/MOOS-2375-Oct0611/Tools/Graphical/uPlayBack/MOOSPlayBack.cpp

Go to the documentation of this file.

//
//   MOOS - Mission Oriented Operating Suite 
//  
//   A suit of Applications and Libraries for Mobile Robotics Research 
//   Copyright (C) 2001-2005 Massachusetts Institute of Technology and 
//   Oxford University. 
//    
//   This software was written by Paul Newman at MIT 2001-2002 and Oxford 
//   University 2003-2005. email: pnewman@robots.ox.ac.uk. 
//      
//   This file is part of a  MOOS Utility Component. 
//        
//   This program is free software; you can redistribute it and/or 
//   modify it under the terms of the GNU General Public License as 
//   published by the Free Software Foundation; either version 2 of the 
//   License, or (at your option) any later version. 
//          
//   This program is distributed in the hope that it will be useful, 
//   but WITHOUT ANY WARRANTY; without even the implied warranty of 
//   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
//   General Public License for more details. 
//            
//   You should have received a copy of the GNU General Public License 
//   along with this program; if not, write to the Free Software 
//   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 
//   02111-1307, USA. 
//
// MOOSPlayBack.cpp: implementation of the CMOOSPlayBack class.
//
#ifdef _WIN32
    #pragma warning(disable : 4786)
#endif

#include "MOOSPlayBack.h"
#include <algorithm>

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif


#define MAX_CHOKE_TIME 2.0

// Construction/Destruction

CMOOSPlayBack::CMOOSPlayBack()
{
    m_dfLastMessageTime = 0;
    m_nLastLine = 0;
    m_bEOF = true;
    m_dfTickTime = 0.01;
    m_dfLastClientProcessedTime = -1;
    m_bWaitingForClientCatchup = false;
}

CMOOSPlayBack::~CMOOSPlayBack()
{

}

bool CMOOSPlayBack::Initialise(const string &sFileName)
{
    m_sFileName = sFileName;
    if(!ParseFile())
        return false;

    return true;

}

bool CMOOSPlayBack::IsOpen()
{
    return m_InputFile.is_open();
}

bool CMOOSPlayBack::ParseFile()
{
    
    m_bEOF = false;

    //if open then close
    if(m_InputFile.is_open())
    {
        m_InputFile.close();
        m_InputFile.clear();
    }

    //now open
    m_InputFile.open(m_sFileName.c_str());

    //check
    if(!m_InputFile.is_open())
    {
        //bad news
        return false;
    }

    m_bEOF = false;

    m_Sources.clear();
    m_PlayBackList.clear();

    char Tmp[10000];
        
    string sLogStart;

    int nLine = 0;
    m_dfLogStart = 0.0;
    while(!m_InputFile.eof())
    {
        m_InputFile.getline(Tmp,sizeof(Tmp));

        string sLine = string(Tmp);

        //if we haven't found it look for start of log file...
        if(m_dfLogStart==0.0 && sLine.find("LOGSTART")!=string::npos)
        {
            MOOSChomp(sLine,"LOGSTART");
            m_dfLogStart = atof(sLine.c_str());
            continue;
        }

        MOOSTrimWhiteSpace(sLine);

        //is it a comment? or maybe there is nothing on the line!
        if(sLine[0]=='%' || sLine.size()==0)
            continue;
        
        if(nLine==0)
        {
            sLogStart = sLine;
        }
        else
        {
            CPlaybackEntry NewEntry;
            
            NewEntry.m_dfTime = atof(MOOSChomp(sLine," ").c_str())+m_dfLogStart;
            MOOSTrimWhiteSpace(sLine);
            
            NewEntry.m_sWhat  = MOOSChomp(sLine," ");
            MOOSTrimWhiteSpace(sLine);
            
            NewEntry.m_sWho  = MOOSChomp(sLine," ");
            MOOSTrimWhiteSpace(sLine);

            string sData = sLine;
            MOOSTrimWhiteSpace(sData);
            
            //is this a string or not?
            if(MOOSIsNumeric(sData))
            {
                NewEntry.m_dfVal = atof(sData.c_str());
                NewEntry.m_sVal = "";
                NewEntry.m_bNumeric = true;
            }
            else
            {
                NewEntry.m_sVal = sData;
                NewEntry.m_dfVal = 0;
                NewEntry.m_bNumeric = false;
            }

            m_PlayBackList.push_back(NewEntry);
            //MOOSTrace("size is %d\n",m_PlayBackList.size());

            m_Sources.insert(NewEntry.m_sWho);

        }        

        nLine++;
    


    }

    sort(m_PlayBackList.begin(),m_PlayBackList.end());
    //m_PlayBackList.sort();


    return true;
}

int CMOOSPlayBack::GetSize()
{
    return m_PlayBackList.size();
}

double CMOOSPlayBack::GetTimeNow()
{
    return m_dfLastMessageTime-m_dfLogStart;
}

int CMOOSPlayBack::GetCurrentLine()
{
    return m_nLastLine;
}

double CMOOSPlayBack::GetStartTime()
{
    if(m_PlayBackList.empty())
        return 0;

    return m_PlayBackList.front().m_dfTime;
}

double CMOOSPlayBack::GetFinishTime()
{
    if(m_PlayBackList.empty())
        return 0;

    return m_PlayBackList.back().m_dfTime;
}

bool CMOOSPlayBack::Iterate(MOOSMSG_LIST &Output)
{
    if(m_bEOF)
        return false;
    
    double dfStartTime = m_dfLastMessageTime+1e-6;
    double dfStopTime = m_dfLastMessageTime+m_dfTickTime;
    
    bool bDone = false;

    while(m_pListIterator!=m_PlayBackList.end() && !bDone)
    {    
        CPlaybackEntry & rEntry = *m_pListIterator;

        
        m_dfClientLagTime = MOOSTime() - m_dfLastClientProcessedTime;
        if (m_dfLastClientProcessedTime != -1 &&
             m_dfClientLagTime > MAX_CHOKE_TIME)
        {
            m_bWaitingForClientCatchup = true;    
            bDone = true;
            dfStopTime = rEntry.m_dfTime;
            continue;
        } 
        else
        {
            m_bWaitingForClientCatchup = false;
        }

        if(rEntry.m_dfTime>dfStopTime)
        {
            m_pListIterator;
            bDone = true;
            continue;
        }
        else
        {
            if(rEntry.m_dfTime>=dfStartTime)
            {
                CMOOSMsg Msg;

                Entry2Message(rEntry,Msg);

                if(m_SourceFilter.find(Msg.m_sSrc)==m_SourceFilter.end())
                {
                    Output.push_front(Msg);
                }
            }
            m_nLastLine++;
        }        
        m_pListIterator++;
    }

    //look for end of data..
    if(m_pListIterator==m_PlayBackList.end())
    {
        m_bEOF = true;
        return false;
    }
    
    m_dfLastMessageTime = dfStopTime;

    return true;
}

bool CMOOSPlayBack::IsEOF()
{
    return m_bEOF;
}

bool CMOOSPlayBack::Reset()
{
    if(m_PlayBackList.empty())
        return false;

    //reset important things..
    m_pListIterator = m_PlayBackList.begin();
    m_bEOF = false;
    m_nLastLine = 0;
    m_dfLastMessageTime = m_pListIterator->m_dfTime-1e-6;

    return true;
}

bool CMOOSPlayBack::SetTickInterval(double dfInterval)
{
    m_dfTickTime = dfInterval;
    return true;
}

bool CMOOSPlayBack::Filter(string sSrc, bool bWanted)
{
    if(bWanted)
    {
        //remove from our filter things that are wanted
        STRING_SET::iterator p = m_SourceFilter.find(sSrc);

        if(p!=m_SourceFilter.end())
        {
            m_SourceFilter.erase(p);
        }
    }
    else
    {
        //we don;t want this type of message (from thsinclient)
        //so add it to our filter
        m_SourceFilter.insert(sSrc);
        MOOSTrace("Filtering messages from %s\n",sSrc.c_str());
    }
    return true;
}

bool CMOOSPlayBack::ClearFilter()
{
    m_SourceFilter.clear();
    return true;
}

bool CMOOSPlayBack::Entry2Message(CPlaybackEntry Entry, CMOOSMsg &Msg)
{
    if(!CrackAndAdjustEntry(Entry))
        return false;

    Msg.m_dfTime = MOOSTime();
    Msg.m_dfVal = Entry.m_dfVal;
    Msg.m_sVal = Entry.m_sVal;
    Msg.m_sSrc = Entry.m_sWho;
    Msg.m_sKey = Entry.m_sWhat;
    Msg.m_cDataType = Entry.m_bNumeric ? MOOS_DOUBLE : MOOS_STRING;
    Msg.m_cMsgType = MOOS_NOTIFY;

    return true;
}

bool CMOOSPlayBack::CrackAndAdjustEntry(CPlaybackEntry &rEntry)
{
    //return true; //added Jan 2005 PMN - exisiting code was intolerable!
    //client must fix 
    
    //this is horrible but if the original message included timing information
    //then we have to adjust it here....
    /*
    if(rEntry.m_sWhat=="LBL_TOF")
    {
        string sOldStart = MOOSChomp(rEntry.m_sVal,",");
        MOOSChomp(sOldStart,"=");
        double dfOldTxTime = atof(sOldStart.c_str());
        double dfAdjustment = MOOSTime()-rEntry.m_dfTime;
        string sNewStart = MOOSFormat("Tx=%.3f,",dfOldTxTime+dfAdjustment);
        
        rEntry.m_sVal = sNewStart+rEntry.m_sVal;
    }*/

    return true;
}

double CMOOSPlayBack::GetLastMessageTime()
{
    return m_dfLastMessageTime;
}

bool CMOOSPlayBack::SetLastTimeProcessed(double dfTime)
{
    m_dfLastClientProcessedTime = dfTime;
    double diff = MOOSTime() - dfTime;
    return true;
}

bool CMOOSPlayBack::IsWaitingForClient()
{
    return m_bWaitingForClientCatchup;
}

string CMOOSPlayBack::GetStatusString()
{
    return MOOSFormat("%s Client Lag %g",
        m_bWaitingForClientCatchup ? "Waiting for Client CATCHUP":"Playing just fine", 
        m_dfClientLagTime);
}

bool CMOOSPlayBack::GotoTime(double dfT)
{
    int nTmp = 0;

    PLAYBACKLIST::iterator q = m_pListIterator;
    for(q=m_PlayBackList.begin();
        q!=m_PlayBackList.end();
        q++)
    {
        nTmp++;
        if(q->m_dfTime>dfT)
        {
            m_pListIterator=q;
            m_nLastLine = nTmp;
            m_bEOF = false;
            m_dfLastClientProcessedTime = -1;
            m_bWaitingForClientCatchup = false;
            m_dfLastMessageTime = m_pListIterator->m_dfTime-1e-6;    
            return true;
        }
    }
    return false;
}