/home/toby/moos-ivp/MOOS-2375-Oct0611/Core/MOOSLIB/MOOSCommPkt.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 Core 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. 
//
// MOOSCommPkt.cpp: implementation of the MOOSCommPkt class.
//
#ifdef _WIN32
    #pragma warning(disable : 4786)
    #pragma warning(disable : 4503)
#endif


#include <MOOSGenLib/MOOSGenLib.h>
#include "MOOSCommPkt.h"
#include "MOOSMsg.h"
#include "MOOSGlobalHelper.h"
#include <assert.h>
#include <cstring>
#include <iostream>
#include "MOOSException.h"


#define DEFAULT_ASSUMMED_MAX_MOOS_MSG_SIZE 40000


#ifdef COMPRESSED_MOOS_PROTOCOL
        
        #define MAX_BELIEVABLE_ZLIB_COMPRESSION 256  //if we look like we are getting a compression ration of more than this then bail - there is an error.

        #define COMPRESSION_PACKET_SIZE_THRESHOLD 1024 //packets below this size won't be compressed.

        #include <zlib.h>
#endif

using namespace std;

// Construction/Destruction

CMOOSCommPkt::CMOOSCommPkt()
{
    m_pStream = DefaultStream;
    m_nStreamSpace = MOOS_PKT_DEFAULT_SPACE;
    m_pNextData = m_pStream;
    m_bAllocated = false;
    m_nByteCount = 0;
    m_nMsgLen = 0;
        m_dfCompression = 1.0;


}

CMOOSCommPkt::~CMOOSCommPkt()
{
    if(m_bAllocated)
    {
        delete [] m_pStream;
    }
}

double CMOOSCommPkt::GetCompression()
{
        return m_dfCompression;
}

int CMOOSCommPkt::GetBytesRequired()
{
    if(m_nByteCount==0)
    {
        return sizeof(int);
    }
    else
    {
        return m_nMsgLen-m_nByteCount;
    }
}

bool CMOOSCommPkt::Fill(unsigned char *InData, int nData)
{
    
    if(    m_nByteCount ==0)
    {
        //here we figure out how many bytes we are expecting
        assert(nData==sizeof(int));

        memcpy((void*)(&m_nMsgLen),(void*)InData,sizeof(int));

        //look to swap byte order if this machine is Big End in
        if(!IsLittleEndian())
        {
            m_nMsgLen = SwapByteOrder<int>(m_nMsgLen);
        }

    }

    if(m_nByteCount+nData>=m_nStreamSpace)
    {
        InflateTo(2*(m_nStreamSpace+nData));
    }
    memcpy(m_pNextData,InData,nData);
    m_pNextData+=nData;
    m_nByteCount+=nData;

    return true;
}


int CMOOSCommPkt::GetStreamLength()
{
    return m_nMsgLen;
}


bool CMOOSCommPkt::Serialize(MOOSMSG_LIST &List, bool bToStream, bool bNoNULL, double * pdfPktTime)
{
        //note +1 is for indicator regarding compressed or not compressed
        unsigned int nHeaderSize = 2*sizeof(int)+1;
        
    if(bToStream)
    {

        m_nMsgLen=0;
        m_pNextData = m_pStream;
        m_nByteCount = 0;

        //we need to leave space at the start of the packet for total
        //length and number of include messages
                
        m_pNextData+=nHeaderSize; 
        m_nByteCount+= nHeaderSize;


        //assume to start with that mesages are reasonably sized -if they aren't we'll make an adjustment
        unsigned int nWorkingMemoryCurrentSize = DEFAULT_ASSUMMED_MAX_MOOS_MSG_SIZE;
        unsigned char * pTmpBuffer =new unsigned char[nWorkingMemoryCurrentSize] ;



        MOOSMSG_LIST::iterator p;
        int nCount = 0;
        for(p = List.begin();p!=List.end();p++,nCount++)
        {
        
            unsigned int nRequiredSize = p->GetSizeInBytesWhenSerialised();

            if(nRequiredSize>nWorkingMemoryCurrentSize)
            {
                //std::cerr<<"making more space "<<nWorkingMemoryCurrentSize<<" -> "<<nRequiredSize<<std::endl;
                nWorkingMemoryCurrentSize = nRequiredSize;
                delete [] pTmpBuffer;

                pTmpBuffer = new unsigned  char [nWorkingMemoryCurrentSize];
            }

            //MOOSTrace("Sending %s \n",p->m_sKey.c_str());
            //int nTmpSize = MAX_MOOS_MSG_SIZE;

            int nCopied = (*p).Serialize(pTmpBuffer,nWorkingMemoryCurrentSize);

            if(nCopied !=nRequiredSize )
            {
                std::cerr<<"bad news expected "<<nWorkingMemoryCurrentSize<<" but serialisation took "<<nCopied<<std::endl;
                p->Trace();
            }

            if(nCopied!=-1)
            {
                //now copy to our stream...
                CopyToStream(pTmpBuffer,nCopied);
            }
            else
            {
                delete [] pTmpBuffer;
                return false;
            }

        }

        delete [] pTmpBuffer;





                unsigned char bCompressed = 0;
#ifdef COMPRESSED_MOOS_PROTOCOL
                
                //we only compress if it is worth it
                if(m_nByteCount>COMPRESSION_PACKET_SIZE_THRESHOLD)
                {
                        
                        unsigned long int nDataLength = m_nByteCount-nHeaderSize;
                        
                        //we require .1% extra and 12 bytes
                        unsigned long int nZBSize = (unsigned int) ((float)nDataLength*1.01+13);
                        unsigned char * ZipBuffer = new unsigned char[nZBSize];         
                        
                        int nZipResult = compress(ZipBuffer,&nZBSize,m_pStream+nHeaderSize,nDataLength);
                        
                        
                        switch (nZipResult) {
                                case Z_OK:
                                        break;
                                case Z_BUF_ERROR:
                                        throw CMOOSException("failed to compress outgoing data - looks like compression actually expanded data!");
                                        break;
                                case Z_MEM_ERROR:
                                        throw CMOOSException("zlib memory error!");
                                        break;
                                default:
                                        break;
                        }
                        
                        //MOOSTrace("Compressed out going buffer is %d bytes and original is %d\n",nZBSize,nDataLength);
                        
                        memcpy(m_pStream+nHeaderSize,ZipBuffer,nZBSize);

                        //maybe useful to record what compression ration we are getting
                        m_dfCompression = (double)m_nByteCount/nZBSize;

                        //now we have a new byte count                  
                        m_nByteCount = nZBSize+nHeaderSize;
                        
                        delete ZipBuffer;
                        
                        bCompressed = 1;
                        
                }
                
#endif

        //finally write how many bytes we have written at the start
        //look for need to swap byte order if required
        m_pNextData = m_pStream;
        int nBC = IsLittleEndian() ? m_nByteCount : SwapByteOrder<int>(m_nByteCount);
        memcpy((void*)m_pNextData,(void*)(&nBC),sizeof(m_nByteCount));
        m_pNextData+=sizeof(m_nByteCount);


        //and then how many messages are included
        //look for need to swap byte order if required
        int nMessages = List.size();
        nMessages = IsLittleEndian() ? nMessages : SwapByteOrder<int>(nMessages);
        memcpy((void*)m_pNextData,(void*)(&nMessages),sizeof(nMessages));
        m_pNextData+=sizeof(nMessages);
                
                //and is this a compressed message or not?
                *m_pNextData = bCompressed;
                m_pNextData+=1;


    }
    else
    {
        
        m_pNextData = m_pStream;
        m_nMsgLen = 0;
        m_nByteCount = 0;

        //first figure out the length of the message
        //look to swap byte order as required
        memcpy((void*)(&m_nMsgLen),(void*)m_pNextData,sizeof(m_nMsgLen));
        m_nMsgLen = IsLittleEndian() ? m_nMsgLen : SwapByteOrder<int>(m_nMsgLen);
        m_pNextData+=sizeof(m_nMsgLen);
        m_nByteCount+=sizeof(m_nMsgLen);

        int nSpaceFree=m_nMsgLen - sizeof(m_nMsgLen);

        //now figure out how many messages are packed in this packet
        //look to swap byte order as required
        int nMessages = 0;
        memcpy((void*)(&nMessages),(void*)m_pNextData,sizeof(nMessages));
        nMessages = IsLittleEndian() ? nMessages : SwapByteOrder<int>(nMessages);
        m_pNextData+=sizeof(nMessages);
        nSpaceFree-=sizeof(nMessages);
        m_nByteCount+=sizeof(nMessages);
                
                //now account for one byet of compression indication
                m_pNextData+=sizeof(unsigned char);
        nSpaceFree-=sizeof(unsigned char);
        m_nByteCount+=sizeof(unsigned char);
                
                
                
#ifdef COMPRESSED_MOOS_PROTOCOL         
                unsigned char bCompressed = m_pStream[nHeaderSize-1];
                //we will only have compressed if it is worth it
                
                //MOOSTrace("Message is %s compressed\n",bCompressed? "":"not");
                if(bCompressed)
                {
                        
                        unsigned long int nDataLength = m_nMsgLen-nHeaderSize;
                        
                        //we'd be surprised if we had more tha 90% compression
                        int nCompressionFactor = 10;
                        unsigned char * ZipBuffer = NULL;
                        unsigned long int nZBSize;
                        
                        int nZipResult = -1;
                        
                        do
                        {
                                
                                nZBSize = nCompressionFactor*nDataLength;
                                
                                ZipBuffer = new unsigned char[nZBSize];
                                
                                nZipResult = uncompress(ZipBuffer,&nZBSize,m_pStream+nHeaderSize,nDataLength);
                                
                                switch (nZipResult) 
                                {
                                        case Z_MEM_ERROR:
                                                throw CMOOSException("ZLIB out of memory");
                                                break;
                                                
                                        case Z_BUF_ERROR:
                                        {
                                                //if we get here we need more space
                                                delete ZipBuffer;
                                                nCompressionFactor *=8;
                                                
                                                if(nCompressionFactor>MAX_BELIEVABLE_ZLIB_COMPRESSION)
                                                {
                                                        //this is very suspcious
                                                        throw CMOOSException("error in decompressing CMOOSPkt stream");
                                                }
                                        }
                                                break;
                                                
                                        case Z_DATA_ERROR:
                                                throw CMOOSException("ZLIB received corrupted data - this is really bad news.");
                                                break;
                                                
                                                
                                        default:
                                                break;
                                }
                        }while(nZipResult!=Z_OK);
                        
                        
                        //MOOSTrace("received %d data bytes and uncompressed them to %d  bytes\n",nDataLength,nZBSize);
                        
                        //maybe useful to record what compression ration we are getting
                        m_dfCompression = (double)nZBSize/nDataLength;

                        
                        //we have a new message length
                        m_nMsgLen = nZBSize+nHeaderSize;
                        
                        //check we have allocated enough memory
                        if(m_nMsgLen>m_nStreamSpace)
                        {
                                //interesting case we are out of memory - we need some more
                                InflateTo(m_nMsgLen);
                        }
                        
                        //copy the uncompressed data into our stream
                        memcpy(m_pStream+nHeaderSize,ZipBuffer,nZBSize);
                        
                        //recalculate space free
                        nSpaceFree= m_nMsgLen-nHeaderSize;
                        
                        //be a good citizen
                        delete ZipBuffer;
                }
#endif
                

        for(int i = 0; i<nMessages;i++)
        {
                    
            CMOOSMsg Msg;
            int nUsed = Msg.Serialize(m_pNextData,nSpaceFree,false);

            if(nUsed!=-1) 
            {
                //allows us to not store NULL messages
                bool bOmit = bNoNULL && (Msg.m_cMsgType==MOOS_NULL_MSG);

                if(Msg.m_cMsgType==MOOS_NULL_MSG && pdfPktTime!=NULL && i == 0 )
                {
                    *pdfPktTime     = Msg.GetDouble();                    
                }

                if(!bOmit)
                {
                    List.push_front(Msg);
                }


                m_pNextData+=nUsed;
                m_nByteCount+=nUsed;
                nSpaceFree-=nUsed;

            }
            else
            {
                //bad news...
                break;
            }        
        }
    }

    //here at the last moment we can fill in our totalm length for safe keeping
    m_nMsgLen = m_nByteCount;

    return true;
}

bool CMOOSCommPkt::CopyToStream(unsigned char *pData, int nBytes)
{
    //well do we have enough space to do this?
    if(m_nByteCount+nBytes>=m_nStreamSpace)
    {
        //no..better inflate ourselves...
        InflateTo(2*(m_nStreamSpace+nBytes));
    }

    //by this point we are guaranteed enough space..
    //unless (new has failed which is really the end of the world and we will be dead :-(  )

    //copy temporary buffer to our main buffer
    memcpy(m_pNextData,pData,nBytes);

    //increment hot spot
    m_pNextData+=nBytes;

    //increment byte count
    m_nByteCount+=nBytes;

    return true;
}

bool CMOOSCommPkt::InflateTo(int nNewStreamSize)
{
    //make more space then..
    m_nStreamSpace = nNewStreamSize;

    unsigned char * pNew = new unsigned char[m_nStreamSpace];

    if(pNew==NULL)
    {
        //absolute disaster..nothing we can do ...whole process must exit
        MOOSTrace("memory allocation failed in CMOOSCommPkt::CopyToStream");
        return false;
    }

    //copy what we have already assembled into new space
    memcpy(pNew,m_pStream,m_nByteCount);

    
    if(m_bAllocated)
    {
        delete [] m_pStream;
    }
    else
    {
        m_bAllocated = true;
    }

    //switch m_pStream to point to the new space;
    m_pStream = pNew;

    //and set the next data point to be m_nMsgLen bytes from start
    //so we will start writin after are existing stuff
    m_pNextData=m_pStream+m_nByteCount;

    return true;
}