<commit_before>/* -*-c++-*- */
/**
* OsgAL - OpenSceneGraph Audio Library
* Copyright (C) 2004 VRlab, Ume University
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library 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
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA.
*/

#include "openalpp/Audiobase"
#include "stdio.h"
#include <sstream>
using namespace openalpp;

AudioBase::AudioBase(int frequency,int refresh,int synchronous)
  throw (InitError)
{
  if(!instances_) {
    ALboolean success = alutInitWithoutContext (0L, 0L);
    if (success != AL_TRUE) {
      ALenum error = alutGetError ();
      const char *err_str = alutGetErrorString(error);
      std::ostringstream str;
      str << "Couldn't open device: " << std::string(err_str);
      throw InitError(str.str().c_str());
    }
    // Open a write (output) device. This should (in theory) make it possible
    // to open a read (input) device later..
#ifdef _WIN32
    char *initString = 0L;//"DirectSound", "DirectSound3D", ;
    device_ =alcOpenDevice((ALCchar *)initString);

#else
    device_=alcOpenDevice((/*const */ALubyte *)"'((direction \"write\")) '((devices '(alsa sdl native null)))");
#endif
    if(!device_)
      throw InitError("Couldn't open device.");
    int attributes[7],i=0;
    attributes[0]=0;
    if(frequency>0) {
      attributes[i++]=ALC_FREQUENCY;
      attributes[i++]=frequency;
      attributes[i]=0;
    }
    if(refresh>0) {
      attributes[i++]=ALC_REFRESH;
      attributes[i++]=refresh;
      attributes[i]=0;
    }
    if(synchronous>0) {
      attributes[i++]=ALC_SYNC;
      attributes[i++]=synchronous;
      attributes[i]=0;
    }
    context_=alcCreateContext(device_,attributes);
    if(!context_ || alcGetError(device_)!=ALC_NO_ERROR) {
      if(context_)
	      alcDestroyContext(context_);
      alcCloseDevice(device_);
      throw InitError("Couldn't create context");
    }
    alcMakeContextCurrent(context_);
    reverbinitiated_=false;

    // Check for EAX 2.0 support
    unsigned char szFnName[256];
    ALboolean g_bEAX = alIsExtensionPresent((ALCchar*)"EAX2.0");
    if (g_bEAX == AL_TRUE)
    {
      sprintf((char*)szFnName, "EAXSet");
      ALvoid *eaxSet = alGetProcAddress((ALCchar *)szFnName);
      if (eaxSet == NULL) g_bEAX = AL_FALSE;
    }
    if (g_bEAX == AL_TRUE)
    {
      sprintf((char*)szFnName,"EAXGet");
      ALvoid *eaxGet = alGetProcAddress((ALCchar *)szFnName);
      if (eaxGet == NULL) g_bEAX = AL_FALSE;
    }
    if (g_bEAX == AL_TRUE)
      std::cerr << "Using OpenAL EAX2.0 extension" << std::endl;
    else
      std::cerr << "No OpenAL EAX2.0 extensions available" << std::endl;
  }

  instances_++;
}

AudioBase::~AudioBase() {
  instances_--;
  if(!instances_) {
//    std::cerr << "All instances removed, closing OpenAL context&device" << std::endl;
    alutExit ();
    alcMakeContextCurrent(NULL);
    alcDestroyContext(context_);
    alcCloseDevice(device_);
  }
}

const char *openalpp::alGetErrorString(ALenum error)
{
  switch (error) {
    case (AL_INVALID_NAME):
      return "Invalid Name parameter passed to AL call.";
    case (AL_INVALID_ENUM):
      return "Invalid parameter passed to AL call.";
    case (AL_INVALID_VALUE):
      return "Invalid enum parameter value.";
    case (AL_INVALID_OPERATION):
      return "Illegal call.";
    case (AL_OUT_OF_MEMORY):
      return "ut of memory.";
    default:
      return "No message associated wth error code.";

  }
  return "";
}

// Static members
int AudioBase::instances_=0;
ALCdevice *AudioBase::device_=NULL;
#ifndef WIN32
void *AudioBase::context_=NULL;
#else
struct ALCcontext_struct *AudioBase::context_=NULL;
#endif

<commit_msg>Case typo in header files<commit_after>/* -*-c++-*- */
/**
* OsgAL - OpenSceneGraph Audio Library
* Copyright (C) 2004 VRlab, Ume University
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library 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
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA.
*/

#include "openalpp/AudioBase"
#include "stdio.h"
#include <sstream>
using namespace openalpp;

AudioBase::AudioBase(int frequency,int refresh,int synchronous)
  throw (InitError)
{
  if(!instances_) {
#if OPENAL_VERSION >= 2007
    ALboolean success = alutInitWithoutContext (0L, 0L);
    if (success != AL_TRUE) {
      ALenum error = alutGetError ();
      const char *err_str = alutGetErrorString(error);
      std::ostringstream str;
      str << "Couldn't open device: " << std::string(err_str);
      throw InitError(str.str().c_str());
    }
#endif
    // Open a write (output) device. This should (in theory) make it possible
    // to open a read (input) device later..
#ifdef _WIN32
    //char *initString = 0L;//"DirectSound", "DirectSound3D", ;
    char *initStringList = (char*)alcGetString(NULL, ALC_DEVICE_SPECIFIER);
		if (initStringList)
		{
			std::cout << "sound device " << initStringList << std::endl << "found" << std::endl;
			std::string initStringList2(initStringList);
			std::string deviceAsked = "Generic Hardware";
			if (initStringList2.find(deviceAsked) == std::string::npos)
				device_ = 0;
			else
			{
				device_ =alcOpenDevice((const ALCchar *)deviceAsked.c_str());
				std::cout << "device_ =alcOpenDevice((const ALCchar *)initStringList);" << std::endl;
			}
		}
		else
		{
			std::cout << "alcGetString return NULL" << std::endl;
			device_ = 0;
		}

#else
#if OPENAL_VERSION < 2005
    device_=alcOpenDevice((/*const */ALubyte *)"'((direction \"write\")) '((devices '(alsa sdl native null)))");
#else // OPENAL_VERSION < 2005
    device_=alcOpenDevice("'((direction \"write\")) '((devices '(alsa sdl native null)))");
#endif // OPENAL_VERSION < 2005
#endif
    if(!device_)
      throw InitError("Couldn't open device.");
    int attributes[7],i=0;
    attributes[0]=0;
    if(frequency>0) {
      attributes[i++]=ALC_FREQUENCY;
      attributes[i++]=frequency;
      attributes[i]=0;
    }
    if(refresh>0) {
      attributes[i++]=ALC_REFRESH;
      attributes[i++]=refresh;
      attributes[i]=0;
    }
    if(synchronous>0) {
      attributes[i++]=ALC_SYNC;
      attributes[i++]=synchronous;
      attributes[i]=0;
    }
    context_=alcCreateContext(device_,attributes);
    if(!context_ || alcGetError(device_)!=ALC_NO_ERROR) {
      if(context_)
	      alcDestroyContext(context_);
      alcCloseDevice(device_);
      throw InitError("Couldn't create context");
    }
    alcMakeContextCurrent(context_);
    reverbinitiated_=false;

    // Check for EAX 2.0 support
#if OPENAL_VERSION < 2005
    unsigned char szFnName[256];
    ALboolean g_bEAX = alIsExtensionPresent((ALubyte*)"EAX2.0");
#else // OPENAL_VERSION < 2005
    char szFnName[256];
    ALboolean g_bEAX = alIsExtensionPresent("EAX2.0");
#endif // OPENAL_VERSION < 2005
    if (g_bEAX == AL_TRUE)
    {
      sprintf((char*)szFnName, "EAXSet");
      ALvoid *eaxSet = alGetProcAddress(szFnName);
      if (eaxSet == NULL) g_bEAX = AL_FALSE;
    }
    if (g_bEAX == AL_TRUE)
    {
      sprintf((char*)szFnName,"EAXGet");
      ALvoid *eaxGet = alGetProcAddress(szFnName);
      if (eaxGet == NULL) g_bEAX = AL_FALSE;
    }
    if (g_bEAX == AL_TRUE)
      std::cerr << "Using OpenAL EAX2.0 extension" << std::endl;
    else
      std::cerr << "No OpenAL EAX2.0 extensions available" << std::endl;
  }

  instances_++;
}

AudioBase::~AudioBase() {
  instances_--;
  if(!instances_) {
//    std::cerr << "All instances removed, closing OpenAL context&device" << std::endl;
    alutExit ();
    alcMakeContextCurrent(NULL);
    alcDestroyContext(context_);
    alcCloseDevice(device_);
  }
}

const char *openalpp::alGetErrorString(ALenum error)
{
  switch (error) {
    case (AL_INVALID_NAME):
      return "Invalid Name parameter passed to AL call.";
    case (AL_INVALID_ENUM):
      return "Invalid parameter passed to AL call.";
    case (AL_INVALID_VALUE):
      return "Invalid enum parameter value.";
    case (AL_INVALID_OPERATION):
      return "Illegal call.";
    case (AL_OUT_OF_MEMORY):
      return "ut of memory.";
    default:
      return "No message associated wth error code.";

  }
  return "";
}

// Static members
int AudioBase::instances_=0;
ALCdevice *AudioBase::device_=NULL;
#ifndef WIN32
#if OPENAL_VERSION < 2007
void *AudioBase::context_=NULL;
#else // OPENAL_VERSION < 2007
ALCcontext *AudioBase::context_=NULL;
#endif // OPENAL_VERSION < 2007
#else
struct ALCcontext_struct *AudioBase::context_=NULL;
#endif

<|endoftext|>


<commit_before>#include <osg/GL>
#include <osg/GLU>

#include <osg/Notify>
#include <osgUtil/Tesselator>

using namespace osg;
using namespace osgUtil;

static Tesselator* s_currentTesselator=0;

Tesselator::Tesselator()
{
    _tobj = 0;
    _errorCode = 0;
}

Tesselator::~Tesselator()
{
    if (_tobj) gluDeleteTess(_tobj);
}

void Tesselator::beginTesselation()
{
    reset();

    if (!_tobj) _tobj = gluNewTess();

    gluTessCallback(_tobj, GLU_TESS_VERTEX_DATA, (GLvoid (CALLBACK*)()) vertexCallback);
    gluTessCallback(_tobj, GLU_TESS_BEGIN_DATA,  (GLvoid (CALLBACK*)()) beginCallback);
    gluTessCallback(_tobj, GLU_TESS_END_DATA,    (GLvoid (CALLBACK*)()) endCallback);
    gluTessCallback(_tobj, GLU_TESS_COMBINE,     (GLvoid (CALLBACK*)()) combineCallback);
    gluTessCallback(_tobj, GLU_TESS_ERROR_DATA,  (GLvoid (CALLBACK*)()) errorCallback);

    gluTessBeginPolygon(_tobj,this);

    s_currentTesselator = this;

}

void Tesselator::beginContour()
{
    if (_tobj)
    {
        gluTessBeginContour(_tobj);
    }
}

void Tesselator::addVertex(osg::Vec3* vertex)
{
    if (_tobj)
    {
        Vec3d* data = new Vec3d;
        _coordData.push_back(data);
        (*data)._v[0]=(*vertex)[0];
        (*data)._v[1]=(*vertex)[1];
        (*data)._v[2]=(*vertex)[2];
        gluTessVertex(_tobj,data->_v,vertex);
    }
}

void Tesselator::endContour()
{
    if (_tobj)
    {
        gluTessEndContour(_tobj);
    }
}

void Tesselator::endTesselation()
{
    if (_tobj)
    {
        gluTessEndPolygon(_tobj);
        gluDeleteTess(_tobj);
        _tobj = 0;

        if (_errorCode!=0)
        {
           const GLubyte *estring = gluErrorString((GLenum)_errorCode);
           osg::notify(osg::WARN)<<"Tessellation Error: "<<estring<< std::endl;
        }
    }
}

void Tesselator::reset()
{
    if (_tobj)
    {
        gluDeleteTess(_tobj);
        _tobj = 0;
    }
    _primList.clear();
    _coordData.clear();
    _newVertexList.clear();
    _errorCode = 0;
}

class InsertNewVertices : public osg::ArrayVisitor
{
    public:

        float _f1,_f2,_f3,_f4;
        unsigned int _i1,_i2,_i3,_i4;

        InsertNewVertices(float f1,unsigned int i1,
                          float f2,unsigned int i2,
                          float f3,unsigned int i3,
                          float f4,unsigned int i4):
                            _f1(f1),_f2(f2),_f3(f3),_f4(f4),
                            _i1(i1),_i2(i2),_i3(i3),_i4(i4){}

        template <class ARRAY,class TYPE>
        void apply_imp(ARRAY& array,TYPE initialValue)
        {
            TYPE val = initialValue;
            if (_f1) val += static_cast<TYPE>(array[_i1] * _f1);
            if (_f2) val += static_cast<TYPE>(array[_i2] * _f2);
            if (_f3) val += static_cast<TYPE>(array[_i3] * _f3);
            if (_f4) val += static_cast<TYPE>(array[_i4] * _f4);

            array.push_back(val);
        }

        virtual void apply(osg::ByteArray& ba) { apply_imp(ba,GLbyte(0)); }
        virtual void apply(osg::ShortArray& ba) { apply_imp(ba,GLshort(0)); }
        virtual void apply(osg::IntArray& ba) { apply_imp(ba,GLint(0)); }
        virtual void apply(osg::UByteArray& ba) { apply_imp(ba,GLubyte(0)); }
        virtual void apply(osg::UShortArray& ba) { apply_imp(ba,GLushort(0)); }
        virtual void apply(osg::UIntArray& ba) { apply_imp(ba,GLuint(0)); }
        virtual void apply(osg::UByte4Array& ba) { apply_imp(ba,UByte4()); }
        virtual void apply(osg::FloatArray& ba) { apply_imp(ba,float(0)); }
        virtual void apply(osg::Vec2Array& ba) { apply_imp(ba,Vec2()); }
        virtual void apply(osg::Vec3Array& ba) { apply_imp(ba,Vec3()); }
        virtual void apply(osg::Vec4Array& ba) { apply_imp(ba,Vec4()); }

};

void Tesselator::retesselatePolygons(osg::Geometry& geom)
{
    Vec3Array* vertices = geom.getVertexArray();
    if (!vertices || vertices->empty() || geom.getPrimitiveList().empty()) return;

    int noPrimitiveAtStart = geom.getPrimitiveList().size();
    for(int primNo=0;primNo<noPrimitiveAtStart;++primNo)
    {
        osg::Primitive* primitive = geom.getPrimitiveList()[primNo].get();
        if (primitive->getMode()==osg::Primitive::POLYGON)
        {
            beginTesselation();
            beginContour();

            switch(primitive->getType())
            {
                case(osg::Primitive::DrawArraysPrimitiveType):
                {
                    osg::DrawArrays* drawArray = static_cast<osg::DrawArrays*>(primitive);
                    unsigned int first = drawArray->getFirst();
                    unsigned int last = first+drawArray->getCount();
                    for(unsigned int i=first;i<last;++i)
                    {
                        addVertex(&((*vertices)[i]));
                    }
                    break;
                }
                case(osg::Primitive::DrawElementsUBytePrimitiveType):
                {
                    osg::DrawElementsUByte* drawElements = static_cast<osg::DrawElementsUByte*>(primitive);
                    for(osg::DrawElementsUByte::iterator indexItr=drawElements->begin();
                        indexItr!=drawElements->end();
                        ++indexItr)
                    {
                        addVertex(&((*vertices)[*indexItr]));
                    }
                    break;
                }
                case(osg::Primitive::DrawElementsUShortPrimitiveType):
                {
                    osg::DrawElementsUShort* drawElements = static_cast<osg::DrawElementsUShort*>(primitive);
                    for(osg::DrawElementsUShort::iterator indexItr=drawElements->begin();
                        indexItr!=drawElements->end();
                        ++indexItr)
                    {
                        addVertex(&((*vertices)[*indexItr]));
                    }
                    break;
                }
                case(osg::Primitive::DrawElementsUIntPrimitiveType):
                {
                    osg::DrawElementsUInt* drawElements = static_cast<osg::DrawElementsUInt*>(primitive);
                    for(osg::DrawElementsUInt::iterator indexItr=drawElements->begin();
                        indexItr!=drawElements->end();
                        ++indexItr)
                    {
                        addVertex(&((*vertices)[*indexItr]));
                    }
                    break;
                }
                default:
                    break;
            }

            endContour();
            endTesselation();

            typedef std::map<osg::Vec3*,unsigned int> VertexPtrToIndexMap;
            VertexPtrToIndexMap vertexPtrToIndexMap;

            // populate the VertexPtrToIndexMap.
            for(unsigned int vi=0;vi<vertices->size();++vi)
            {
                vertexPtrToIndexMap[&((*vertices)[vi])] = vi;
            }

            if (!_newVertexList.empty())
            {

                osg::Vec3Array* normals = NULL;
                if (geom.getNormalBinding()==osg::Geometry::BIND_PER_VERTEX)
                {
                    normals = geom.getNormalArray();
                }

                typedef std::vector<osg::Array*> ArrayList;
                ArrayList arrays;

                if (geom.getColorBinding()==osg::Geometry::BIND_PER_VERTEX)
                {
                    arrays.push_back(geom.getColorArray());
                }

                osg::Geometry::TexCoordArrayList& tcal = geom.getTexCoordArrayList();
                for(osg::Geometry::TexCoordArrayList::iterator tcalItr=tcal.begin();
                    tcalItr!=tcal.end();
                    ++tcalItr)
                {
                    if (tcalItr->valid())
                    {
                        arrays.push_back(tcalItr->get());
                    }
                }

                // now add any new vertices that are required.
                for(NewVertexList::iterator itr=_newVertexList.begin();
                    itr!=_newVertexList.end();
                    ++itr)
                {
                    osg::Vec3* vertex = itr->first;
                    NewVertex& newVertex = itr->second;

                    // assign vertex.
                    vertexPtrToIndexMap[vertex]=vertices->size();
                    vertices->push_back(*vertex);

                    // assign normals
                    if (normals)
                    {
                        osg::Vec3 norm(0.0f,0.0f,0.0f);
                        if (newVertex._v1) norm += (*normals)[vertexPtrToIndexMap[newVertex._v1]] * newVertex._f1;
                        if (newVertex._v2) norm += (*normals)[vertexPtrToIndexMap[newVertex._v2]] * newVertex._f2;
                        if (newVertex._v3) norm += (*normals)[vertexPtrToIndexMap[newVertex._v3]] * newVertex._f3;
                        if (newVertex._v4) norm += (*normals)[vertexPtrToIndexMap[newVertex._v4]] * newVertex._f4;
                        norm.normalize();
                        normals->push_back(norm);
                    }

                    if (!arrays.empty())
                    {
                        InsertNewVertices inv(newVertex._f1,vertexPtrToIndexMap[newVertex._v1],
                                              newVertex._f2,vertexPtrToIndexMap[newVertex._v2],
                                              newVertex._f3,vertexPtrToIndexMap[newVertex._v3],
                                              newVertex._f4,vertexPtrToIndexMap[newVertex._v4]);

                                                  // assign the rest of the attributes.
                        for(ArrayList::iterator aItr=arrays.begin();
                            aItr!=arrays.end();
                            ++aItr)
                        {
                            (*aItr)->accept(inv);
                        }
                    }
                }

            }

            for(PrimList::iterator primItr=_primList.begin();
                primItr!=_primList.end();
                ++primItr)
            {
                Prim* prim = primItr->get();

                osg::DrawElementsUShort* elements = new osg::DrawElementsUShort(prim->_mode);
                for(Prim::VecList::iterator vitr=prim->_vertices.begin();
                    vitr!=prim->_vertices.end();
                    ++vitr)
                {
                    elements->push_back(vertexPtrToIndexMap[*vitr]);
                }

                if (primItr==_primList.begin())
                {
                    // first new primitive so overwrite the previous polygon.
                    geom.getPrimitiveList()[primNo] = elements;
                }
                else
                {
                    // subsequence primtives add to the back of the primitive list.
                    geom.addPrimitive(elements);
                }
            }

        }

    }
}

void Tesselator::begin(GLenum mode)
{
    _primList.push_back(new Prim(mode));
}

void Tesselator::vertex(osg::Vec3* vertex)
{
    if (!_primList.empty())
    {
        Prim* prim = _primList.back().get();
        prim->_vertices.push_back(vertex);

    }
}

void Tesselator::combine(osg::Vec3* vertex,void* vertex_data[4],GLfloat weight[4])
{
    _newVertexList[vertex]=NewVertex(weight[0],(Vec3*)vertex_data[0],
                                     weight[1],(Vec3*)vertex_data[1],
                                     weight[2],(Vec3*)vertex_data[2],
                                     weight[3],(Vec3*)vertex_data[3]);
}

void Tesselator::end()
{
    // no need to do anything right now...
}

void Tesselator::error(GLenum errorCode)
{
    _errorCode = errorCode;
}

void CALLBACK Tesselator::beginCallback(GLenum which, void* userData)
{
    ((Tesselator*)userData)->begin(which);
}

void CALLBACK Tesselator::endCallback(void* userData)
{
    ((Tesselator*)userData)->end();
}

void CALLBACK Tesselator::vertexCallback(GLvoid *data, void* userData)
{
    ((Tesselator*)userData)->vertex((Vec3*)data);
}

void CALLBACK Tesselator::combineCallback(GLdouble coords[3], void* vertex_data[4],
                              GLfloat weight[4], void** outData,
                              void* /*userData*/)
{
    Vec3* newData = new osg::Vec3(coords[0],coords[1],coords[2]);
    *outData = newData;
    //((Tesselator*)userData)->combine(newData);
    s_currentTesselator->combine(newData,vertex_data,weight);
}

void CALLBACK Tesselator::errorCallback(GLenum errorCode, void* userData)
{
    ((Tesselator*)userData)->error(errorCode);
}
<commit_msg>Fixed the glTessCallback type to GLU_TESS_ERROR_DATA.<commit_after>#include <osg/GL>
#include <osg/GLU>

#include <osg/Notify>
#include <osgUtil/Tesselator>

using namespace osg;
using namespace osgUtil;

Tesselator::Tesselator()
{
    _tobj = 0;
    _errorCode = 0;
}

Tesselator::~Tesselator()
{
    if (_tobj) gluDeleteTess(_tobj);
}

void Tesselator::beginTesselation()
{
    reset();

    if (!_tobj) _tobj = gluNewTess();

    gluTessCallback(_tobj, GLU_TESS_VERTEX_DATA, (GLvoid (CALLBACK*)()) vertexCallback);
    gluTessCallback(_tobj, GLU_TESS_BEGIN_DATA,  (GLvoid (CALLBACK*)()) beginCallback);
    gluTessCallback(_tobj, GLU_TESS_END_DATA,    (GLvoid (CALLBACK*)()) endCallback);
    gluTessCallback(_tobj, GLU_TESS_COMBINE_DATA,(GLvoid (CALLBACK*)()) combineCallback);
    gluTessCallback(_tobj, GLU_TESS_ERROR_DATA,  (GLvoid (CALLBACK*)()) errorCallback);

    gluTessBeginPolygon(_tobj,this);
}

void Tesselator::beginContour()
{
    if (_tobj)
    {
        gluTessBeginContour(_tobj);
    }
}

void Tesselator::addVertex(osg::Vec3* vertex)
{
    if (_tobj)
    {
        Vec3d* data = new Vec3d;
        _coordData.push_back(data);
        (*data)._v[0]=(*vertex)[0];
        (*data)._v[1]=(*vertex)[1];
        (*data)._v[2]=(*vertex)[2];
        gluTessVertex(_tobj,data->_v,vertex);
    }
}

void Tesselator::endContour()
{
    if (_tobj)
    {
        gluTessEndContour(_tobj);
    }
}

void Tesselator::endTesselation()
{
    if (_tobj)
    {
        gluTessEndPolygon(_tobj);
        gluDeleteTess(_tobj);
        _tobj = 0;

        if (_errorCode!=0)
        {
           const GLubyte *estring = gluErrorString((GLenum)_errorCode);
           osg::notify(osg::WARN)<<"Tessellation Error: "<<estring<< std::endl;
        }
    }
}

void Tesselator::reset()
{
    if (_tobj)
    {
        gluDeleteTess(_tobj);
        _tobj = 0;
    }
    _primList.clear();
    _coordData.clear();
    _newVertexList.clear();
    _errorCode = 0;
}

class InsertNewVertices : public osg::ArrayVisitor
{
    public:

        float _f1,_f2,_f3,_f4;
        unsigned int _i1,_i2,_i3,_i4;

        InsertNewVertices(float f1,unsigned int i1,
                          float f2,unsigned int i2,
                          float f3,unsigned int i3,
                          float f4,unsigned int i4):
                            _f1(f1),_f2(f2),_f3(f3),_f4(f4),
                            _i1(i1),_i2(i2),_i3(i3),_i4(i4){}

        template <class ARRAY,class TYPE>
        void apply_imp(ARRAY& array,TYPE initialValue)
        {
            TYPE val = initialValue;
            if (_f1) val += static_cast<TYPE>(array[_i1] * _f1);
            if (_f2) val += static_cast<TYPE>(array[_i2] * _f2);
            if (_f3) val += static_cast<TYPE>(array[_i3] * _f3);
            if (_f4) val += static_cast<TYPE>(array[_i4] * _f4);

            array.push_back(val);
        }

        virtual void apply(osg::ByteArray& ba) { apply_imp(ba,GLbyte(0)); }
        virtual void apply(osg::ShortArray& ba) { apply_imp(ba,GLshort(0)); }
        virtual void apply(osg::IntArray& ba) { apply_imp(ba,GLint(0)); }
        virtual void apply(osg::UByteArray& ba) { apply_imp(ba,GLubyte(0)); }
        virtual void apply(osg::UShortArray& ba) { apply_imp(ba,GLushort(0)); }
        virtual void apply(osg::UIntArray& ba) { apply_imp(ba,GLuint(0)); }
        virtual void apply(osg::UByte4Array& ba) { apply_imp(ba,UByte4()); }
        virtual void apply(osg::FloatArray& ba) { apply_imp(ba,float(0)); }
        virtual void apply(osg::Vec2Array& ba) { apply_imp(ba,Vec2()); }
        virtual void apply(osg::Vec3Array& ba) { apply_imp(ba,Vec3()); }
        virtual void apply(osg::Vec4Array& ba) { apply_imp(ba,Vec4()); }

};

void Tesselator::retesselatePolygons(osg::Geometry& geom)
{
    Vec3Array* vertices = geom.getVertexArray();
    if (!vertices || vertices->empty() || geom.getPrimitiveList().empty()) return;

    int noPrimitiveAtStart = geom.getPrimitiveList().size();
    for(int primNo=0;primNo<noPrimitiveAtStart;++primNo)
    {
        osg::Primitive* primitive = geom.getPrimitiveList()[primNo].get();
        if (primitive->getMode()==osg::Primitive::POLYGON)
        {
            beginTesselation();
            beginContour();

            switch(primitive->getType())
            {
                case(osg::Primitive::DrawArraysPrimitiveType):
                {
                    osg::DrawArrays* drawArray = static_cast<osg::DrawArrays*>(primitive);
                    unsigned int first = drawArray->getFirst();
                    unsigned int last = first+drawArray->getCount();
                    for(unsigned int i=first;i<last;++i)
                    {
                        addVertex(&((*vertices)[i]));
                    }
                    break;
                }
                case(osg::Primitive::DrawElementsUBytePrimitiveType):
                {
                    osg::DrawElementsUByte* drawElements = static_cast<osg::DrawElementsUByte*>(primitive);
                    for(osg::DrawElementsUByte::iterator indexItr=drawElements->begin();
                        indexItr!=drawElements->end();
                        ++indexItr)
                    {
                        addVertex(&((*vertices)[*indexItr]));
                    }
                    break;
                }
                case(osg::Primitive::DrawElementsUShortPrimitiveType):
                {
                    osg::DrawElementsUShort* drawElements = static_cast<osg::DrawElementsUShort*>(primitive);
                    for(osg::DrawElementsUShort::iterator indexItr=drawElements->begin();
                        indexItr!=drawElements->end();
                        ++indexItr)
                    {
                        addVertex(&((*vertices)[*indexItr]));
                    }
                    break;
                }
                case(osg::Primitive::DrawElementsUIntPrimitiveType):
                {
                    osg::DrawElementsUInt* drawElements = static_cast<osg::DrawElementsUInt*>(primitive);
                    for(osg::DrawElementsUInt::iterator indexItr=drawElements->begin();
                        indexItr!=drawElements->end();
                        ++indexItr)
                    {
                        addVertex(&((*vertices)[*indexItr]));
                    }
                    break;
                }
                default:
                    break;
            }

            endContour();
            endTesselation();

            typedef std::map<osg::Vec3*,unsigned int> VertexPtrToIndexMap;
            VertexPtrToIndexMap vertexPtrToIndexMap;

            // populate the VertexPtrToIndexMap.
            for(unsigned int vi=0;vi<vertices->size();++vi)
            {
                vertexPtrToIndexMap[&((*vertices)[vi])] = vi;
            }

            if (!_newVertexList.empty())
            {

                osg::Vec3Array* normals = NULL;
                if (geom.getNormalBinding()==osg::Geometry::BIND_PER_VERTEX)
                {
                    normals = geom.getNormalArray();
                }

                typedef std::vector<osg::Array*> ArrayList;
                ArrayList arrays;

                if (geom.getColorBinding()==osg::Geometry::BIND_PER_VERTEX)
                {
                    arrays.push_back(geom.getColorArray());
                }

                osg::Geometry::TexCoordArrayList& tcal = geom.getTexCoordArrayList();
                for(osg::Geometry::TexCoordArrayList::iterator tcalItr=tcal.begin();
                    tcalItr!=tcal.end();
                    ++tcalItr)
                {
                    if (tcalItr->valid())
                    {
                        arrays.push_back(tcalItr->get());
                    }
                }

                // now add any new vertices that are required.
                for(NewVertexList::iterator itr=_newVertexList.begin();
                    itr!=_newVertexList.end();
                    ++itr)
                {
                    osg::Vec3* vertex = itr->first;
                    NewVertex& newVertex = itr->second;

                    // assign vertex.
                    vertexPtrToIndexMap[vertex]=vertices->size();
                    vertices->push_back(*vertex);

                    // assign normals
                    if (normals)
                    {
                        osg::Vec3 norm(0.0f,0.0f,0.0f);
                        if (newVertex._v1) norm += (*normals)[vertexPtrToIndexMap[newVertex._v1]] * newVertex._f1;
                        if (newVertex._v2) norm += (*normals)[vertexPtrToIndexMap[newVertex._v2]] * newVertex._f2;
                        if (newVertex._v3) norm += (*normals)[vertexPtrToIndexMap[newVertex._v3]] * newVertex._f3;
                        if (newVertex._v4) norm += (*normals)[vertexPtrToIndexMap[newVertex._v4]] * newVertex._f4;
                        norm.normalize();
                        normals->push_back(norm);
                    }

                    if (!arrays.empty())
                    {
                        InsertNewVertices inv(newVertex._f1,vertexPtrToIndexMap[newVertex._v1],
                                              newVertex._f2,vertexPtrToIndexMap[newVertex._v2],
                                              newVertex._f3,vertexPtrToIndexMap[newVertex._v3],
                                              newVertex._f4,vertexPtrToIndexMap[newVertex._v4]);

                                                  // assign the rest of the attributes.
                        for(ArrayList::iterator aItr=arrays.begin();
                            aItr!=arrays.end();
                            ++aItr)
                        {
                            (*aItr)->accept(inv);
                        }
                    }
                }

            }

            for(PrimList::iterator primItr=_primList.begin();
                primItr!=_primList.end();
                ++primItr)
            {
                Prim* prim = primItr->get();

                osg::DrawElementsUShort* elements = new osg::DrawElementsUShort(prim->_mode);
                for(Prim::VecList::iterator vitr=prim->_vertices.begin();
                    vitr!=prim->_vertices.end();
                    ++vitr)
                {
                    elements->push_back(vertexPtrToIndexMap[*vitr]);
                }

                if (primItr==_primList.begin())
                {
                    // first new primitive so overwrite the previous polygon.
                    geom.getPrimitiveList()[primNo] = elements;
                }
                else
                {
                    // subsequence primtives add to the back of the primitive list.
                    geom.addPrimitive(elements);
                }
            }

        }

    }
}

void Tesselator::begin(GLenum mode)
{
    _primList.push_back(new Prim(mode));
}

void Tesselator::vertex(osg::Vec3* vertex)
{
    if (!_primList.empty())
    {
        Prim* prim = _primList.back().get();
        prim->_vertices.push_back(vertex);

    }
}

void Tesselator::combine(osg::Vec3* vertex,void* vertex_data[4],GLfloat weight[4])
{
    _newVertexList[vertex]=NewVertex(weight[0],(Vec3*)vertex_data[0],
                                     weight[1],(Vec3*)vertex_data[1],
                                     weight[2],(Vec3*)vertex_data[2],
                                     weight[3],(Vec3*)vertex_data[3]);
}

void Tesselator::end()
{
    // no need to do anything right now...
}

void Tesselator::error(GLenum errorCode)
{
    _errorCode = errorCode;
}

void CALLBACK Tesselator::beginCallback(GLenum which, void* userData)
{
    ((Tesselator*)userData)->begin(which);
}

void CALLBACK Tesselator::endCallback(void* userData)
{
    ((Tesselator*)userData)->end();
}

void CALLBACK Tesselator::vertexCallback(GLvoid *data, void* userData)
{
    ((Tesselator*)userData)->vertex((Vec3*)data);
}

void CALLBACK Tesselator::combineCallback(GLdouble coords[3], void* vertex_data[4],
                              GLfloat weight[4], void** outData,
                              void* userData)
{
    Vec3* newData = new osg::Vec3(coords[0],coords[1],coords[2]);
    *outData = newData;
    ((Tesselator*)userData)->combine(newData,vertex_data,weight);
}

void CALLBACK Tesselator::errorCallback(GLenum errorCode, void* userData)
{
    ((Tesselator*)userData)->error(errorCode);
}
<|endoftext|>


<commit_before>#include "balancer.hpp"
#include <net/tcp/stream.hpp>

#define READQ_PER_CLIENT        4096
#define MAX_READQ_PER_NODE      8192
#define READQ_FOR_NODES         8192
#define MAX_OUTGOING_ATTEMPTS    100
// checking if nodes are dead or not
#define ACTIVE_INITIAL_PERIOD     8s
#define ACTIVE_CHECK_PERIOD      30s
// connection attempt timeouts
#define CONNECT_TIMEOUT          10s
#define CONNECT_THROW_PERIOD     20s

#define LB_VERBOSE 0
#if LB_VERBOSE
#define LBOUT(fmt, ...) printf(fmt, ##__VA_ARGS__)
#else
#define LBOUT(fmt, ...) /** **/
#endif

using namespace std::chrono;

namespace microLB
{
  Balancer::Balancer(
         netstack_t& incoming, uint16_t in_port,
         netstack_t& outgoing, bool active_check)
    : nodes(active_check),
      netin(incoming), netout(outgoing),
      signal({this, &Balancer::handle_queue})
  {
    netin.tcp().listen(in_port,
    [this] (auto conn) {
      if (conn != nullptr) {
        this->incoming(std::make_unique<net::tcp::Stream> (conn));
      }
    });

    this->init_liveupdate();
  }
  int Balancer::wait_queue() const {
    return this->queue.size();
  }
  int Balancer::connect_throws() const {
    return this->throw_counter;
  }
  netstack_t& Balancer::get_client_network() noexcept
  {
    return this->netin;
  }
  netstack_t& Balancer::get_nodes_network() noexcept
  {
    return this->netout;
  }
  const pool_signal_t& Balancer::get_pool_signal() const
  {
    return this->signal;
  }
  void Balancer::incoming(net::Stream_ptr conn)
  {
      assert(conn != nullptr);
      queue.emplace_back(std::move(conn));
      LBOUT("Queueing connection (q=%lu)\n", queue.size());
      // IMPORTANT: try to handle queue, in case its ready
      // don't directly call handle_connections() from here!
      this->handle_queue();
  }
  void Balancer::handle_queue()
  {
    // check waitq
    while (nodes.pool_size() > 0 && queue.empty() == false)
    {
      auto& client = queue.front();
      assert(client.conn != nullptr);
      if (client.conn->is_connected()) {
        // NOTE: explicitly want to copy buffers
        net::Stream_ptr rval =
            nodes.assign(std::move(client.conn), client.readq);
        if (rval == nullptr) {
          // done with this queue item
          queue.pop_front();
        }
        else {
          // put connection back in queue item
          client.conn = std::move(rval);
        }
      }
      else {
        queue.pop_front();
      }
    } // waitq check
    // check if we need to create more connections
    this->handle_connections();
  }
  void Balancer::handle_connections()
  {
    LBOUT("Handle_connections. %i waiting \n", queue.size());
    // stop any rethrow timer since this is a de-facto retry
    if (this->throw_retry_timer != Timers::UNUSED_ID) {
        Timers::stop(this->throw_retry_timer);
        this->throw_retry_timer = Timers::UNUSED_ID;
    }

    // prune dead clients because the "number of clients" is being
    // used in a calculation right after this to determine how many
    // nodes to connect to
    auto new_end = std::remove_if(queue.begin(), queue.end(),
        [](Waiting& client) {
          return client.conn == nullptr || client.conn->is_connected() == false;
        });
    queue.erase(new_end, queue.end());

    // calculating number of connection attempts to create
    int np_connecting = nodes.pool_connecting();
    int estimate = queue.size() - (np_connecting + nodes.pool_size());
    estimate = std::min(estimate, MAX_OUTGOING_ATTEMPTS);
    estimate = std::max(0, estimate - np_connecting);
    // create more outgoing connections
    LBOUT("Estimated connections needed: %d\n", estimate);
    if (estimate > 0)
    {
      try {
        nodes.create_connections(estimate);
      }
      catch (std::exception& e)
      {
        this->throw_counter++;
        // assuming the failure is due to not enough eph. ports
        this->throw_retry_timer = Timers::oneshot(CONNECT_THROW_PERIOD,
        [this] (int) {
            this->throw_retry_timer = Timers::UNUSED_ID;
            this->handle_connections();
        });
      }
    } // estimate
  } // handle_connections()

  Waiting::Waiting(net::Stream_ptr incoming)
    : conn(std::move(incoming)), total(0)
  {
    assert(this->conn != nullptr);
    assert(this->conn->is_connected());

    // Release connection if it closes before it's assigned to a node.
    this->conn->on_close([this](){
        if (this->conn != nullptr)
          this->conn->reset_callbacks();
        this->conn = nullptr;
      });

    // queue incoming data from clients not yet
    // assigned to a node
    this->conn->on_read(READQ_PER_CLIENT,
    [this] (auto buf) {
      // prevent buffer bloat attack
      this->total += buf->size();
      if (this->total > MAX_READQ_PER_NODE) {
        this->conn->close();
      }
      else {
        LBOUT("*** Queued %lu bytes\n", buf->size());
        readq.push_back(buf);
      }
    });
  }

  void Nodes::create_connections(int total)
  {
    // temporary iterator
    for (int i = 0; i < total; i++)
    {
      bool dest_found = false;
      // look for next active node up to *size* times
      for (size_t i = 0; i < nodes.size(); i++)
      {
        const int iter = conn_iterator;
        conn_iterator = (conn_iterator + 1) % nodes.size();
        // if the node is active, connect immediately
        auto& dest_node = nodes[iter];
        if (dest_node.is_active()) {
          dest_node.connect();
          dest_found = true;
          break;
        }
      }
      // if no active node found, simply delegate to the next node
      if (dest_found == false)
      {
        // with active-checks we can return here later when we get a connection
        if (this->do_active_check) return;
        const int iter = conn_iterator;
        conn_iterator = (conn_iterator + 1) % nodes.size();
        nodes[iter].connect();
      }
    }
  }
  net::Stream_ptr Nodes::assign(net::Stream_ptr conn, queue_vector_t& readq)
  {
    for (size_t i = 0; i < nodes.size(); i++)
    {
      auto outgoing = nodes[algo_iterator].get_connection();
      // algorithm here //
      algo_iterator = (algo_iterator + 1) % nodes.size();
      // check if connection was retrieved
      if (outgoing != nullptr)
      {
        assert(outgoing->is_connected());
        LBOUT("Assigning client to node %d (%s)\n",
              algo_iterator, outgoing->to_string().c_str());
        auto& session = this->create_session(
              std::move(conn), std::move(outgoing));
        // flush readq to session.outgoing
        for (auto buffer : readq) {
          LBOUT("*** Flushing %lu bytes\n", buffer->size());
          session.outgoing->write(buffer);
        }
        return nullptr;
      }
    }
    return conn;
  }
  size_t Nodes::size() const noexcept {
    return nodes.size();
  }
  Nodes::const_iterator Nodes::begin() const {
    return nodes.cbegin();
  }
  Nodes::const_iterator Nodes::end() const {
    return nodes.cend();
  }
  int Nodes::pool_connecting() const {
    int count = 0;
    for (auto& node : nodes) count += node.connection_attempts();
    return count;
  }
  int Nodes::pool_size() const {
    int count = 0;
    for (auto& node : nodes) count += node.pool_size();
    return count;
  }
  int32_t Nodes::open_sessions() const {
    return session_cnt;
  }
  int64_t Nodes::total_sessions() const {
    return session_total;
  }
  int32_t Nodes::timed_out_sessions() const {
    return 0;
  }
  Session& Nodes::create_session(net::Stream_ptr client, net::Stream_ptr outgoing)
  {
    int idx = -1;
    if (free_sessions.empty()) {
      idx = sessions.size();
      sessions.emplace_back(*this, idx, std::move(client), std::move(outgoing));
    } else {
      idx = free_sessions.back();
      new (&sessions[idx]) Session(*this, idx, std::move(client), std::move(outgoing));
      free_sessions.pop_back();
    }
    session_total++;
    session_cnt++;
    LBOUT("New session %d  (current = %d, total = %ld)\n",
          idx, session_cnt, session_total);
    return sessions[idx];
  }
  Session& Nodes::get_session(int idx)
  {
    auto& session = sessions.at(idx);
    assert(session.is_alive());
    return session;
  }
  void Nodes::close_session(int idx)
  {
    auto& session = get_session(idx);
    // remove connections
    session.incoming->reset_callbacks();
    session.incoming = nullptr;
    session.outgoing->reset_callbacks();
    session.outgoing = nullptr;
    // free session
    free_sessions.push_back(session.self);
    session_cnt--;
    LBOUT("Session %d closed  (total = %d)\n", session.self, session_cnt);
  }

  Node::Node(netstack_t& stk, net::Socket a, const pool_signal_t& sig, bool da)
    : stack(stk), pool_signal(sig), addr(a), do_active_check(da)
  {
    if (this->do_active_check)
    {
      // periodically connect to node and determine if active
      // however, perform first check immediately
      this->active_timer = Timers::periodic(0s, ACTIVE_CHECK_PERIOD,
            {this, &Node::perform_active_check});
    }
  }
  void Node::perform_active_check(int)
  {
    assert(this->do_active_check);
    try {
      this->stack.tcp().connect(this->addr,
      [this] (auto conn) {
        this->active = (conn != nullptr);
        // if we are connected, its alive
        if (conn != nullptr)
        {
          // hopefully put this to good use
          pool.push_back(std::make_unique<net::tcp::Stream>(conn));
          // stop any active check
          this->stop_active_check();
          // signal change in pool
          this->pool_signal();
        }
        else {
          // if no periodic check is being done right now,
          // start doing it (after initial delay)
          this->restart_active_check();
        }
      });
    } catch (std::exception& e) {
      // do nothing, because might just be eph.ports used up
    }
  }
  void Node::restart_active_check()
  {
    // set as inactive
    this->active = false;
    if (this->do_active_check)
    {
      // begin checking active again
      if (this->active_timer == Timers::UNUSED_ID)
      {
        this->active_timer = Timers::periodic(
          ACTIVE_INITIAL_PERIOD, ACTIVE_CHECK_PERIOD,
          {this, &Node::perform_active_check});
        LBOUT("Node %s restarting active check (and is inactive)\n",
              this->addr.to_string().c_str());
      }
      else
      {
        LBOUT("Node %s still trying to connect...\n",
              this->addr.to_string().c_str());
      }
    }
  }
  void Node::stop_active_check()
  {
    // set as active
    this->active = true;
    if (this->do_active_check)
    {
      // stop active checking for now
      if (this->active_timer != Timers::UNUSED_ID) {
        Timers::stop(this->active_timer);
        this->active_timer = Timers::UNUSED_ID;
      }
    }
  }
  void Node::connect()
  {
    auto outgoing = this->stack.tcp().connect(this->addr);
    // connecting to node atm.
    this->connecting++;
    // retry timer when connect takes too long
    int fail_timer = Timers::oneshot(CONNECT_TIMEOUT,
    [this, outgoing] (int)
    {
      // close connection
      outgoing->abort();
      // no longer connecting
      assert(this->connecting > 0);
      this->connecting --;
      // restart active check
      this->restart_active_check();
      // signal change in pool
      this->pool_signal();
    });
    // add connection to pool on success, otherwise.. retry
    outgoing->on_connect(
    [this, fail_timer] (auto conn)
    {
      // stop retry timer
      Timers::stop(fail_timer);
      // no longer connecting
      assert(this->connecting > 0);
      this->connecting --;
      // connection may be null, apparently
      if (conn != nullptr && conn->is_connected())
      {
        LBOUT("Connected to %s  (%ld total)\n",
                addr.to_string().c_str(), pool.size());
        this->pool.push_back(std::make_unique<net::tcp::Stream>(conn));
        // stop any active check
        this->stop_active_check();
        // signal change in pool
        this->pool_signal();
      }
      else {
        this->restart_active_check();
      }
    });
  }
  net::Stream_ptr Node::get_connection()
  {
    while (pool.empty() == false) {
        auto conn = std::move(pool.back());
        assert(conn != nullptr);
        pool.pop_back();
        if (conn->is_connected()) return conn;
        else conn->close();
    }
    return nullptr;
  }

  // use indexing to access Session because std::vector
  Session::Session(Nodes& n, int idx,
                   net::Stream_ptr inc, net::Stream_ptr out)
      : parent(n), self(idx), incoming(std::move(inc)),
                              outgoing(std::move(out))
  {
    incoming->on_read(READQ_PER_CLIENT,
    [this] (auto buf) {
        assert(this->is_alive());
        this->outgoing->write(buf);
    });
    incoming->on_close(
    [&nodes = n, idx] () {
        nodes.close_session(idx);
    });
    outgoing->on_read(READQ_FOR_NODES,
    [this] (auto buf) {
        assert(this->is_alive());
        this->incoming->write(buf);
    });
    outgoing->on_close(
    [&nodes = n, idx] () {
        nodes.close_session(idx);
    });

  }
  bool Session::is_alive() const {
    return incoming != nullptr;
  }
}
<commit_msg>microlb: removed code that closed connection once buffer limit of 8k was reached<commit_after>#include "balancer.hpp"
#include <net/tcp/stream.hpp>

#define READQ_PER_CLIENT        4096
#define READQ_FOR_NODES         8192
#define MAX_OUTGOING_ATTEMPTS    100
// checking if nodes are dead or not
#define ACTIVE_INITIAL_PERIOD     8s
#define ACTIVE_CHECK_PERIOD      30s
// connection attempt timeouts
#define CONNECT_TIMEOUT          10s
#define CONNECT_THROW_PERIOD     20s

#define LB_VERBOSE 0
#if LB_VERBOSE
#define LBOUT(fmt, ...) printf(fmt, ##__VA_ARGS__)
#else
#define LBOUT(fmt, ...) /** **/
#endif

using namespace std::chrono;

namespace microLB
{
  Balancer::Balancer(
         netstack_t& incoming, uint16_t in_port,
         netstack_t& outgoing, bool active_check)
    : nodes(active_check),
      netin(incoming), netout(outgoing),
      signal({this, &Balancer::handle_queue})
  {
    netin.tcp().listen(in_port,
    [this] (auto conn) {
      if (conn != nullptr) {
        this->incoming(std::make_unique<net::tcp::Stream> (conn));
      }
    });

    this->init_liveupdate();
  }
  int Balancer::wait_queue() const {
    return this->queue.size();
  }
  int Balancer::connect_throws() const {
    return this->throw_counter;
  }
  netstack_t& Balancer::get_client_network() noexcept
  {
    return this->netin;
  }
  netstack_t& Balancer::get_nodes_network() noexcept
  {
    return this->netout;
  }
  const pool_signal_t& Balancer::get_pool_signal() const
  {
    return this->signal;
  }
  void Balancer::incoming(net::Stream_ptr conn)
  {
      assert(conn != nullptr);
      queue.emplace_back(std::move(conn));
      LBOUT("Queueing connection (q=%lu)\n", queue.size());
      // IMPORTANT: try to handle queue, in case its ready
      // don't directly call handle_connections() from here!
      this->handle_queue();
  }
  void Balancer::handle_queue()
  {
    // check waitq
    while (nodes.pool_size() > 0 && queue.empty() == false)
    {
      auto& client = queue.front();
      assert(client.conn != nullptr);
      if (client.conn->is_connected()) {
        // NOTE: explicitly want to copy buffers
        net::Stream_ptr rval =
            nodes.assign(std::move(client.conn), client.readq);
        if (rval == nullptr) {
          // done with this queue item
          queue.pop_front();
        }
        else {
          // put connection back in queue item
          client.conn = std::move(rval);
        }
      }
      else {
        queue.pop_front();
      }
    } // waitq check
    // check if we need to create more connections
    this->handle_connections();
  }
  void Balancer::handle_connections()
  {
    LBOUT("Handle_connections. %i waiting \n", queue.size());
    // stop any rethrow timer since this is a de-facto retry
    if (this->throw_retry_timer != Timers::UNUSED_ID) {
        Timers::stop(this->throw_retry_timer);
        this->throw_retry_timer = Timers::UNUSED_ID;
    }

    // prune dead clients because the "number of clients" is being
    // used in a calculation right after this to determine how many
    // nodes to connect to
    auto new_end = std::remove_if(queue.begin(), queue.end(),
        [](Waiting& client) {
          return client.conn == nullptr || client.conn->is_connected() == false;
        });
    queue.erase(new_end, queue.end());

    // calculating number of connection attempts to create
    int np_connecting = nodes.pool_connecting();
    int estimate = queue.size() - (np_connecting + nodes.pool_size());
    estimate = std::min(estimate, MAX_OUTGOING_ATTEMPTS);
    estimate = std::max(0, estimate - np_connecting);
    // create more outgoing connections
    LBOUT("Estimated connections needed: %d\n", estimate);
    if (estimate > 0)
    {
      try {
        nodes.create_connections(estimate);
      }
      catch (std::exception& e)
      {
        this->throw_counter++;
        // assuming the failure is due to not enough eph. ports
        this->throw_retry_timer = Timers::oneshot(CONNECT_THROW_PERIOD,
        [this] (int) {
            this->throw_retry_timer = Timers::UNUSED_ID;
            this->handle_connections();
        });
      }
    } // estimate
  } // handle_connections()

  Waiting::Waiting(net::Stream_ptr incoming)
    : conn(std::move(incoming)), total(0)
  {
    assert(this->conn != nullptr);
    assert(this->conn->is_connected());

    // Release connection if it closes before it's assigned to a node.
    this->conn->on_close([this](){
        if (this->conn != nullptr)
          this->conn->reset_callbacks();
        this->conn = nullptr;
      });

    // queue incoming data from clients not yet
    // assigned to a node
    this->conn->on_read(READQ_PER_CLIENT,
    [this] (auto buf) {
      // prevent buffer bloat attack
      this->total += buf->size();
      LBOUT("*** Queued %lu bytes\n", buf->size());
      readq.push_back(buf);
    });
  }

  void Nodes::create_connections(int total)
  {
    // temporary iterator
    for (int i = 0; i < total; i++)
    {
      bool dest_found = false;
      // look for next active node up to *size* times
      for (size_t i = 0; i < nodes.size(); i++)
      {
        const int iter = conn_iterator;
        conn_iterator = (conn_iterator + 1) % nodes.size();
        // if the node is active, connect immediately
        auto& dest_node = nodes[iter];
        if (dest_node.is_active()) {
          dest_node.connect();
          dest_found = true;
          break;
        }
      }
      // if no active node found, simply delegate to the next node
      if (dest_found == false)
      {
        // with active-checks we can return here later when we get a connection
        if (this->do_active_check) return;
        const int iter = conn_iterator;
        conn_iterator = (conn_iterator + 1) % nodes.size();
        nodes[iter].connect();
      }
    }
  }
  net::Stream_ptr Nodes::assign(net::Stream_ptr conn, queue_vector_t& readq)
  {
    for (size_t i = 0; i < nodes.size(); i++)
    {
      auto outgoing = nodes[algo_iterator].get_connection();
      // algorithm here //
      algo_iterator = (algo_iterator + 1) % nodes.size();
      // check if connection was retrieved
      if (outgoing != nullptr)
      {
        assert(outgoing->is_connected());
        LBOUT("Assigning client to node %d (%s)\n",
              algo_iterator, outgoing->to_string().c_str());
        auto& session = this->create_session(
              std::move(conn), std::move(outgoing));
        // flush readq to session.outgoing
        for (auto buffer : readq) {
          LBOUT("*** Flushing %lu bytes\n", buffer->size());
          session.outgoing->write(buffer);
        }
        return nullptr;
      }
    }
    return conn;
  }
  size_t Nodes::size() const noexcept {
    return nodes.size();
  }
  Nodes::const_iterator Nodes::begin() const {
    return nodes.cbegin();
  }
  Nodes::const_iterator Nodes::end() const {
    return nodes.cend();
  }
  int Nodes::pool_connecting() const {
    int count = 0;
    for (auto& node : nodes) count += node.connection_attempts();
    return count;
  }
  int Nodes::pool_size() const {
    int count = 0;
    for (auto& node : nodes) count += node.pool_size();
    return count;
  }
  int32_t Nodes::open_sessions() const {
    return session_cnt;
  }
  int64_t Nodes::total_sessions() const {
    return session_total;
  }
  int32_t Nodes::timed_out_sessions() const {
    return 0;
  }
  Session& Nodes::create_session(net::Stream_ptr client, net::Stream_ptr outgoing)
  {
    int idx = -1;
    if (free_sessions.empty()) {
      idx = sessions.size();
      sessions.emplace_back(*this, idx, std::move(client), std::move(outgoing));
    } else {
      idx = free_sessions.back();
      new (&sessions[idx]) Session(*this, idx, std::move(client), std::move(outgoing));
      free_sessions.pop_back();
    }
    session_total++;
    session_cnt++;
    LBOUT("New session %d  (current = %d, total = %ld)\n",
          idx, session_cnt, session_total);
    return sessions[idx];
  }
  Session& Nodes::get_session(int idx)
  {
    auto& session = sessions.at(idx);
    assert(session.is_alive());
    return session;
  }
  void Nodes::close_session(int idx)
  {
    auto& session = get_session(idx);
    // remove connections
    session.incoming->reset_callbacks();
    session.incoming = nullptr;
    session.outgoing->reset_callbacks();
    session.outgoing = nullptr;
    // free session
    free_sessions.push_back(session.self);
    session_cnt--;
    LBOUT("Session %d closed  (total = %d)\n", session.self, session_cnt);
  }

  Node::Node(netstack_t& stk, net::Socket a, const pool_signal_t& sig, bool da)
    : stack(stk), pool_signal(sig), addr(a), do_active_check(da)
  {
    if (this->do_active_check)
    {
      // periodically connect to node and determine if active
      // however, perform first check immediately
      this->active_timer = Timers::periodic(0s, ACTIVE_CHECK_PERIOD,
            {this, &Node::perform_active_check});
    }
  }
  void Node::perform_active_check(int)
  {
    assert(this->do_active_check);
    try {
      this->stack.tcp().connect(this->addr,
      [this] (auto conn) {
        this->active = (conn != nullptr);
        // if we are connected, its alive
        if (conn != nullptr)
        {
          // hopefully put this to good use
          pool.push_back(std::make_unique<net::tcp::Stream>(conn));
          // stop any active check
          this->stop_active_check();
          // signal change in pool
          this->pool_signal();
        }
        else {
          // if no periodic check is being done right now,
          // start doing it (after initial delay)
          this->restart_active_check();
        }
      });
    } catch (std::exception& e) {
      // do nothing, because might just be eph.ports used up
    }
  }
  void Node::restart_active_check()
  {
    // set as inactive
    this->active = false;
    if (this->do_active_check)
    {
      // begin checking active again
      if (this->active_timer == Timers::UNUSED_ID)
      {
        this->active_timer = Timers::periodic(
          ACTIVE_INITIAL_PERIOD, ACTIVE_CHECK_PERIOD,
          {this, &Node::perform_active_check});
        LBOUT("Node %s restarting active check (and is inactive)\n",
              this->addr.to_string().c_str());
      }
      else
      {
        LBOUT("Node %s still trying to connect...\n",
              this->addr.to_string().c_str());
      }
    }
  }
  void Node::stop_active_check()
  {
    // set as active
    this->active = true;
    if (this->do_active_check)
    {
      // stop active checking for now
      if (this->active_timer != Timers::UNUSED_ID) {
        Timers::stop(this->active_timer);
        this->active_timer = Timers::UNUSED_ID;
      }
    }
  }
  void Node::connect()
  {
    auto outgoing = this->stack.tcp().connect(this->addr);
    // connecting to node atm.
    this->connecting++;
    // retry timer when connect takes too long
    int fail_timer = Timers::oneshot(CONNECT_TIMEOUT,
    [this, outgoing] (int)
    {
      // close connection
      outgoing->abort();
      // no longer connecting
      assert(this->connecting > 0);
      this->connecting --;
      // restart active check
      this->restart_active_check();
      // signal change in pool
      this->pool_signal();
    });
    // add connection to pool on success, otherwise.. retry
    outgoing->on_connect(
    [this, fail_timer] (auto conn)
    {
      // stop retry timer
      Timers::stop(fail_timer);
      // no longer connecting
      assert(this->connecting > 0);
      this->connecting --;
      // connection may be null, apparently
      if (conn != nullptr && conn->is_connected())
      {
        LBOUT("Connected to %s  (%ld total)\n",
                addr.to_string().c_str(), pool.size());
        this->pool.push_back(std::make_unique<net::tcp::Stream>(conn));
        // stop any active check
        this->stop_active_check();
        // signal change in pool
        this->pool_signal();
      }
      else {
        this->restart_active_check();
      }
    });
  }
  net::Stream_ptr Node::get_connection()
  {
    while (pool.empty() == false) {
        auto conn = std::move(pool.back());
        assert(conn != nullptr);
        pool.pop_back();
        if (conn->is_connected()) return conn;
        else conn->close();
    }
    return nullptr;
  }

  // use indexing to access Session because std::vector
  Session::Session(Nodes& n, int idx,
                   net::Stream_ptr inc, net::Stream_ptr out)
      : parent(n), self(idx), incoming(std::move(inc)),
                              outgoing(std::move(out))
  {
    incoming->on_read(READQ_PER_CLIENT,
    [this] (auto buf) {
        assert(this->is_alive());
        this->outgoing->write(buf);
    });
    incoming->on_close(
    [&nodes = n, idx] () {
        nodes.close_session(idx);
    });
    outgoing->on_read(READQ_FOR_NODES,
    [this] (auto buf) {
        assert(this->is_alive());
        this->incoming->write(buf);
    });
    outgoing->on_close(
    [&nodes = n, idx] () {
        nodes.close_session(idx);
    });

  }
  bool Session::is_alive() const {
    return incoming != nullptr;
  }
}
<|endoftext|>


<commit_before>// Copyright (c) 2014-2015, Nicolas François
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <io.h>
#include <fcntl.h>
#include <stdio.h>
#include <windows.h>
#include "serial_port.h"

int selectBaudrate(int baudrate_speed){
  if(baudrate_speed<0){
    return -1;
  }
  return baudrate_speed;
}

int selectDataBits(int databits_nb) {
  switch(databits_nb){
    case 5:
      return DATABITS_5;
    case 6:
      return DATABITS_6;
    case 7:
      return DATABITS_7;
    case 8:
      return DATABITS_8;
    default:
      return -1;
  }
}

bool testSerialPort(const char* port_name){
  HANDLE handlePort = CreateFile(port_name, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING,  0, NULL);
  if(handlePort == INVALID_HANDLE_VALUE){
     return false;
  }
  CloseHandle(handlePort);
  return true;
}

int convertParity(parity_t parity){
    // MARKPARITY, SPACEPARITY not supported
    switch(parity){
        default:
        case NONE:
            return NOPARITY;
         case ODD:
            return ODDPARITY;
         case EVEN:
            return EVENPARITY;
    }
}

int convertStopBits(stopbits_t stopbits){
    switch(stopbits){
        default:
        case ONE:
            return ONESTOPBIT;
         case TWO:
            return TWOSTOPBITS;
            /*
         case TWOSTOPBITS:
            return TWOSTOPBITS;
            */
    }
}

int openSerialPort(const char* port_name, int baudrate, int databits, parity_t parity, stopbits_t stopbits){
  HANDLE handlePort = CreateFile(port_name, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
  int tty_fd = _open_osfhandle(reinterpret_cast<intptr_t>(handlePort), _O_TEXT);
  if(tty_fd > 0){
    DCB config = {0};
    config.DCBlength = sizeof(config);
    config.fBinary = true;
    config.BaudRate = baudrate;
    config.ByteSize = databits;
    config.Parity = convertParity(parity);
    config.StopBits = convertStopBits(stopbits);
    config.fDtrControl = 0;
    config.fRtsControl = 0;
    SetCommState(handlePort, &config);

    COMMTIMEOUTS commTimeouts = {0};
    commTimeouts.ReadIntervalTimeout = 1;
    commTimeouts.ReadTotalTimeoutMultiplier  = 0;
    commTimeouts.ReadTotalTimeoutConstant    = 1000;
    commTimeouts.WriteTotalTimeoutConstant   = 1000;
    commTimeouts.WriteTotalTimeoutMultiplier = 1;
    SetCommTimeouts(handlePort, &commTimeouts);

    PurgeComm(handlePort, PURGE_RXCLEAR);
    PurgeComm(handlePort, PURGE_TXCLEAR);
  }
  return tty_fd;
}

bool closeSerialPort(int tty_fd){
  HANDLE handlePort =  reinterpret_cast<HANDLE>(_get_osfhandle(tty_fd));
  return CloseHandle(handlePort);
}

int readFromSerialPort(int tty_fd, uint8_t* data, int buffer_size){
  DWORD bytes_read = -1;
  HANDLE handlePort =  reinterpret_cast<HANDLE>(_get_osfhandle(tty_fd));
  ReadFile(handlePort, data, buffer_size, &bytes_read, NULL);
  return bytes_read;
}

int writeToSerialPort(int tty_fd, const char* data){
  HANDLE handlePort =  reinterpret_cast<HANDLE>(_get_osfhandle(tty_fd));
  DWORD length = -1;
  WriteFile(handlePort, data,  strlen(data), &length, NULL);
  return length;
}

int writeToSerialPort(int tty_fd, uint8_t byte){
  HANDLE handlePort =  reinterpret_cast<HANDLE>(_get_osfhandle(tty_fd));
  DWORD length = -1;
  WriteFile(handlePort, &byte, sizeof(uint8_t), &length,NULL);
  return length;
}
<commit_msg>Quick fix for #6 (without params)<commit_after>// Copyright (c) 2014-2015, Nicolas François
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <io.h>
#include <fcntl.h>
#include <stdio.h>
#include <windows.h>
#include "serial_port.h"

int selectBaudrate(int baudrate_speed){
  if(baudrate_speed<0){
    return -1;
  }
  return baudrate_speed;
}

int selectDataBits(int databits_nb) {
  switch(databits_nb){
    case 5:
      return DATABITS_5;
    case 6:
      return DATABITS_6;
    case 7:
      return DATABITS_7;
    case 8:
      return DATABITS_8;
    default:
      return -1;
  }
}

bool testSerialPort(const char* port_name){
  HANDLE handlePort = CreateFile(port_name, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING,  0, NULL);
  if(handlePort == INVALID_HANDLE_VALUE){
     return false;
  }
  CloseHandle(handlePort);
  return true;
}

int convertParity(parity_t parity){
    // MARKPARITY, SPACEPARITY not supported
    switch(parity){
        default:
        case NONE:
            return NOPARITY;
         case ODD:
            return ODDPARITY;
         case EVEN:
            return EVENPARITY;
    }
}

int convertStopBits(stopbits_t stopbits){
    switch(stopbits){
        default:
        case ONE:
            return ONESTOPBIT;
         case TWO:
            return TWOSTOPBITS;
            /*
         case TWOSTOPBITS:
            return TWOSTOPBITS;
            */
    }
}

int openSerialPort(const char* port_name, int baudrate, int databits, parity_t parity, stopbits_t stopbits){
  HANDLE handlePort = CreateFile(port_name, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
  int tty_fd = _open_osfhandle(reinterpret_cast<intptr_t>(handlePort), _O_TEXT);
  if(tty_fd > 0){
    DCB config = {0};
    config.DCBlength = sizeof(config);
    config.fBinary = true;
    config.BaudRate = baudrate;
    config.ByteSize = databits;
    config.Parity = convertParity(parity);
    config.StopBits = convertStopBits(stopbits);
    config.fDtrControl = 0;
    config.fRtsControl = 0;
    SetCommState(handlePort, &config);

    COMMTIMEOUTS commTimeouts = {0};
    commTimeouts.ReadIntervalTimeout = 1;
    commTimeouts.ReadTotalTimeoutMultiplier  = 0;
    commTimeouts.ReadTotalTimeoutConstant    = 100;
    commTimeouts.WriteTotalTimeoutConstant   = 100;
    commTimeouts.WriteTotalTimeoutMultiplier = 1;
    SetCommTimeouts(handlePort, &commTimeouts);

    PurgeComm(handlePort, PURGE_RXCLEAR);
    PurgeComm(handlePort, PURGE_TXCLEAR);
  }
  return tty_fd;
}

bool closeSerialPort(int tty_fd){
  HANDLE handlePort =  reinterpret_cast<HANDLE>(_get_osfhandle(tty_fd));
  return CloseHandle(handlePort);
}

int readFromSerialPort(int tty_fd, uint8_t* data, int buffer_size){
  DWORD bytes_read = -1;
  HANDLE handlePort =  reinterpret_cast<HANDLE>(_get_osfhandle(tty_fd));
  ReadFile(handlePort, data, buffer_size, &bytes_read, NULL);
  return bytes_read;
}

int writeToSerialPort(int tty_fd, const char* data){
  HANDLE handlePort =  reinterpret_cast<HANDLE>(_get_osfhandle(tty_fd));
  DWORD length = -1;
  WriteFile(handlePort, data,  strlen(data), &length, NULL);
  return length;
}

int writeToSerialPort(int tty_fd, uint8_t byte){
  HANDLE handlePort =  reinterpret_cast<HANDLE>(_get_osfhandle(tty_fd));
  DWORD length = -1;
  WriteFile(handlePort, &byte, sizeof(uint8_t), &length,NULL);
  return length;
}
<|endoftext|>


<commit_before>// Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <memory>
#include <string>
#include <utility>
#include <vector>

#include "builtin_interfaces/msg/time.hpp"

#include "rcl/time.h"

#include "rclcpp/clock.hpp"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/parameter_client.hpp"
#include "rclcpp/parameter_events_filter.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp/time_source.hpp"

namespace rclcpp
{

TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> node)
: logger_(rclcpp::get_logger("rclcpp")),
  clock_subscription_(nullptr),
  ros_time_active_(false)
{
  this->attachNode(node);
}

TimeSource::TimeSource()
: logger_(rclcpp::get_logger("rclcpp")),
  ros_time_active_(false)
{
}

void TimeSource::attachNode(rclcpp::Node::SharedPtr node)
{
  attachNode(
    node->get_node_base_interface(),
    node->get_node_topics_interface(),
    node->get_node_graph_interface(),
    node->get_node_services_interface(),
    node->get_node_logging_interface(),
    node->get_node_clock_interface(),
    node->get_node_parameters_interface());
}

void TimeSource::attachNode(
  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface,
  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,
  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface)
{
  node_base_ = node_base_interface;
  node_topics_ = node_topics_interface;
  node_graph_ = node_graph_interface;
  node_services_ = node_services_interface;
  node_logging_ = node_logging_interface;
  node_clock_ = node_clock_interface;
  node_parameters_ = node_parameters_interface;
  // TODO(tfoote): Update QOS

  logger_ = node_logging_->get_logger();

  // Though this defaults to false, it can be overridden by initial parameter values for the node,
  // which may be given by the user at the node's construction or even by command-line arguments.
  rclcpp::ParameterValue use_sim_time_param;
  const char * use_sim_time_name = "use_sim_time";
  if (!node_parameters_->has_parameter(use_sim_time_name)) {
    use_sim_time_param = node_parameters_->declare_parameter(
      use_sim_time_name,
      rclcpp::ParameterValue(false),
      rcl_interfaces::msg::ParameterDescriptor());
  } else {
    use_sim_time_param = node_parameters_->get_parameter(use_sim_time_name).get_parameter_value();
  }
  if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) {
    if (use_sim_time_param.get<bool>()) {
      parameter_state_ = SET_TRUE;
      enable_ros_time();
      create_clock_sub();
    }
  } else {
    // TODO(wjwwood): use set_on_parameters_set_callback to catch the type mismatch,
    //   before the use_sim_time parameter can ever be set to an invalid value
    RCLCPP_ERROR(logger_, "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'",
      rclcpp::to_string(use_sim_time_param.get_type()).c_str());
  }

  // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609
  parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event(
    node_topics_,
    std::bind(&TimeSource::on_parameter_event, this, std::placeholders::_1));
}

void TimeSource::detachNode()
{
  this->ros_time_active_ = false;
  clock_subscription_.reset();
  parameter_subscription_.reset();
  node_base_.reset();
  node_topics_.reset();
  node_graph_.reset();
  node_services_.reset();
  node_logging_.reset();
  node_clock_.reset();
  node_parameters_.reset();
  disable_ros_time();
}

void TimeSource::attachClock(std::shared_ptr<rclcpp::Clock> clock)
{
  if (clock->get_clock_type() != RCL_ROS_TIME) {
    throw std::invalid_argument("Cannot attach clock to a time source that's not a ROS clock");
  }

  std::lock_guard<std::mutex> guard(clock_list_lock_);
  associated_clocks_.push_back(clock);
  // Set the clock to zero unless there's a recently received message
  auto time_msg = std::make_shared<builtin_interfaces::msg::Time>();
  if (last_msg_set_) {
    time_msg = std::make_shared<builtin_interfaces::msg::Time>(last_msg_set_->clock);
  }
  set_clock(time_msg, ros_time_active_, clock);
}

void TimeSource::detachClock(std::shared_ptr<rclcpp::Clock> clock)
{
  std::lock_guard<std::mutex> guard(clock_list_lock_);
  auto result = std::find(associated_clocks_.begin(), associated_clocks_.end(), clock);
  if (result != associated_clocks_.end()) {
    associated_clocks_.erase(result);
  } else {
    RCLCPP_ERROR(logger_, "failed to remove clock");
  }
}

TimeSource::~TimeSource()
{
  if (node_base_ || node_topics_ || node_graph_ || node_services_ ||
    node_logging_ || node_clock_ || node_parameters_)
  {
    this->detachNode();
  }
}

void TimeSource::set_clock(
  const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled,
  std::shared_ptr<rclcpp::Clock> clock)
{
  // Do change
  if (!set_ros_time_enabled && clock->ros_time_is_active()) {
    disable_ros_time(clock);
  } else if (set_ros_time_enabled && !clock->ros_time_is_active()) {
    enable_ros_time(clock);
  }

  auto ret = rcl_set_ros_time_override(&(clock->rcl_clock_), rclcpp::Time(*msg).nanoseconds());
  if (ret != RCL_RET_OK) {
    rclcpp::exceptions::throw_from_rcl_error(
      ret, "Failed to set ros_time_override_status");
  }
}

void TimeSource::clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg)
{
  if (!this->ros_time_active_ && SET_TRUE == this->parameter_state_) {
    enable_ros_time();
  }
  // Cache the last message in case a new clock is attached.
  last_msg_set_ = msg;
  auto time_msg = std::make_shared<builtin_interfaces::msg::Time>(msg->clock);

  if (SET_TRUE == this->parameter_state_) {
    std::lock_guard<std::mutex> guard(clock_list_lock_);
    for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
      set_clock(time_msg, true, *it);
    }
  }
}

void TimeSource::create_clock_sub()
{
  std::lock_guard<std::mutex> guard(clock_sub_lock_);
  if (clock_subscription_) {
    // Subscription already created.
    return;
  }

  clock_subscription_ = rclcpp::create_subscription<rosgraph_msgs::msg::Clock>(
    node_topics_,
    "/clock",
    rclcpp::QoS(QoSInitialization::from_rmw(rmw_qos_profile_default)),
    std::bind(&TimeSource::clock_cb, this, std::placeholders::_1)
  );
}

void TimeSource::destroy_clock_sub()
{
  std::lock_guard<std::mutex> guard(clock_sub_lock_);
  clock_subscription_.reset();
}

void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
{
  // Filter for only 'use_sim_time' being added or changed.
  rclcpp::ParameterEventsFilter filter(event, {"use_sim_time"},
    {rclcpp::ParameterEventsFilter::EventType::NEW,
      rclcpp::ParameterEventsFilter::EventType::CHANGED});
  for (auto & it : filter.get_events()) {
    if (it.second->value.type != ParameterType::PARAMETER_BOOL) {
      RCLCPP_ERROR(logger_, "use_sim_time parameter set to something besides a bool");
      continue;
    }
    if (it.second->value.bool_value) {
      parameter_state_ = SET_TRUE;
      enable_ros_time();
      create_clock_sub();
    } else {
      parameter_state_ = SET_FALSE;
      disable_ros_time();
      destroy_clock_sub();
    }
  }
  // Handle the case that use_sim_time was deleted.
  rclcpp::ParameterEventsFilter deleted(event, {"use_sim_time"},
    {rclcpp::ParameterEventsFilter::EventType::DELETED});
  for (auto & it : deleted.get_events()) {
    (void) it;  // if there is a match it's already matched, don't bother reading it.
    // If the parameter is deleted mark it as unset but dont' change state.
    parameter_state_ = UNSET;
  }
}

void TimeSource::enable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
{
  auto ret = rcl_enable_ros_time_override(&clock->rcl_clock_);
  if (ret != RCL_RET_OK) {
    rclcpp::exceptions::throw_from_rcl_error(
      ret, "Failed to enable ros_time_override_status");
  }
}

void TimeSource::disable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
{
  auto ret = rcl_disable_ros_time_override(&clock->rcl_clock_);
  if (ret != RCL_RET_OK) {
    rclcpp::exceptions::throw_from_rcl_error(
      ret, "Failed to enable ros_time_override_status");
  }
}

void TimeSource::enable_ros_time()
{
  if (ros_time_active_) {
    // already enabled no-op
    return;
  }

  // Local storage
  ros_time_active_ = true;

  // Update all attached clocks to zero or last recorded time
  std::lock_guard<std::mutex> guard(clock_list_lock_);
  auto time_msg = std::make_shared<builtin_interfaces::msg::Time>();
  if (last_msg_set_) {
    time_msg = std::make_shared<builtin_interfaces::msg::Time>(last_msg_set_->clock);
  }
  for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
    set_clock(time_msg, true, *it);
  }
}

void TimeSource::disable_ros_time()
{
  if (!ros_time_active_) {
    // already disabled no-op
    return;
  }

  // Local storage
  ros_time_active_ = false;

  // Update all attached clocks
  std::lock_guard<std::mutex> guard(clock_list_lock_);
  for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
    auto msg = std::make_shared<builtin_interfaces::msg::Time>();
    set_clock(msg, false, *it);
  }
}

}  // namespace rclcpp
<commit_msg>Make TimeSource ignore use_sim_time events coming from other nodes. (#799)<commit_after>// Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <memory>
#include <string>
#include <utility>
#include <vector>

#include "builtin_interfaces/msg/time.hpp"

#include "rcl/time.h"

#include "rclcpp/clock.hpp"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/parameter_client.hpp"
#include "rclcpp/parameter_events_filter.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp/time_source.hpp"

namespace rclcpp
{

TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> node)
: logger_(rclcpp::get_logger("rclcpp")),
  clock_subscription_(nullptr),
  ros_time_active_(false)
{
  this->attachNode(node);
}

TimeSource::TimeSource()
: logger_(rclcpp::get_logger("rclcpp")),
  ros_time_active_(false)
{
}

void TimeSource::attachNode(rclcpp::Node::SharedPtr node)
{
  attachNode(
    node->get_node_base_interface(),
    node->get_node_topics_interface(),
    node->get_node_graph_interface(),
    node->get_node_services_interface(),
    node->get_node_logging_interface(),
    node->get_node_clock_interface(),
    node->get_node_parameters_interface());
}

void TimeSource::attachNode(
  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface,
  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,
  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface)
{
  node_base_ = node_base_interface;
  node_topics_ = node_topics_interface;
  node_graph_ = node_graph_interface;
  node_services_ = node_services_interface;
  node_logging_ = node_logging_interface;
  node_clock_ = node_clock_interface;
  node_parameters_ = node_parameters_interface;
  // TODO(tfoote): Update QOS

  logger_ = node_logging_->get_logger();

  // Though this defaults to false, it can be overridden by initial parameter values for the node,
  // which may be given by the user at the node's construction or even by command-line arguments.
  rclcpp::ParameterValue use_sim_time_param;
  const char * use_sim_time_name = "use_sim_time";
  if (!node_parameters_->has_parameter(use_sim_time_name)) {
    use_sim_time_param = node_parameters_->declare_parameter(
      use_sim_time_name,
      rclcpp::ParameterValue(false),
      rcl_interfaces::msg::ParameterDescriptor());
  } else {
    use_sim_time_param = node_parameters_->get_parameter(use_sim_time_name).get_parameter_value();
  }
  if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) {
    if (use_sim_time_param.get<bool>()) {
      parameter_state_ = SET_TRUE;
      enable_ros_time();
      create_clock_sub();
    }
  } else {
    // TODO(wjwwood): use set_on_parameters_set_callback to catch the type mismatch,
    //   before the use_sim_time parameter can ever be set to an invalid value
    RCLCPP_ERROR(logger_, "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'",
      rclcpp::to_string(use_sim_time_param.get_type()).c_str());
  }

  // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609
  parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event(
    node_topics_,
    std::bind(&TimeSource::on_parameter_event, this, std::placeholders::_1));
}

void TimeSource::detachNode()
{
  this->ros_time_active_ = false;
  clock_subscription_.reset();
  parameter_subscription_.reset();
  node_base_.reset();
  node_topics_.reset();
  node_graph_.reset();
  node_services_.reset();
  node_logging_.reset();
  node_clock_.reset();
  node_parameters_.reset();
  disable_ros_time();
}

void TimeSource::attachClock(std::shared_ptr<rclcpp::Clock> clock)
{
  if (clock->get_clock_type() != RCL_ROS_TIME) {
    throw std::invalid_argument("Cannot attach clock to a time source that's not a ROS clock");
  }

  std::lock_guard<std::mutex> guard(clock_list_lock_);
  associated_clocks_.push_back(clock);
  // Set the clock to zero unless there's a recently received message
  auto time_msg = std::make_shared<builtin_interfaces::msg::Time>();
  if (last_msg_set_) {
    time_msg = std::make_shared<builtin_interfaces::msg::Time>(last_msg_set_->clock);
  }
  set_clock(time_msg, ros_time_active_, clock);
}

void TimeSource::detachClock(std::shared_ptr<rclcpp::Clock> clock)
{
  std::lock_guard<std::mutex> guard(clock_list_lock_);
  auto result = std::find(associated_clocks_.begin(), associated_clocks_.end(), clock);
  if (result != associated_clocks_.end()) {
    associated_clocks_.erase(result);
  } else {
    RCLCPP_ERROR(logger_, "failed to remove clock");
  }
}

TimeSource::~TimeSource()
{
  if (node_base_ || node_topics_ || node_graph_ || node_services_ ||
    node_logging_ || node_clock_ || node_parameters_)
  {
    this->detachNode();
  }
}

void TimeSource::set_clock(
  const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled,
  std::shared_ptr<rclcpp::Clock> clock)
{
  // Do change
  if (!set_ros_time_enabled && clock->ros_time_is_active()) {
    disable_ros_time(clock);
  } else if (set_ros_time_enabled && !clock->ros_time_is_active()) {
    enable_ros_time(clock);
  }

  auto ret = rcl_set_ros_time_override(&(clock->rcl_clock_), rclcpp::Time(*msg).nanoseconds());
  if (ret != RCL_RET_OK) {
    rclcpp::exceptions::throw_from_rcl_error(
      ret, "Failed to set ros_time_override_status");
  }
}

void TimeSource::clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg)
{
  if (!this->ros_time_active_ && SET_TRUE == this->parameter_state_) {
    enable_ros_time();
  }
  // Cache the last message in case a new clock is attached.
  last_msg_set_ = msg;
  auto time_msg = std::make_shared<builtin_interfaces::msg::Time>(msg->clock);

  if (SET_TRUE == this->parameter_state_) {
    std::lock_guard<std::mutex> guard(clock_list_lock_);
    for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
      set_clock(time_msg, true, *it);
    }
  }
}

void TimeSource::create_clock_sub()
{
  std::lock_guard<std::mutex> guard(clock_sub_lock_);
  if (clock_subscription_) {
    // Subscription already created.
    return;
  }

  clock_subscription_ = rclcpp::create_subscription<rosgraph_msgs::msg::Clock>(
    node_topics_,
    "/clock",
    rclcpp::QoS(QoSInitialization::from_rmw(rmw_qos_profile_default)),
    std::bind(&TimeSource::clock_cb, this, std::placeholders::_1)
  );
}

void TimeSource::destroy_clock_sub()
{
  std::lock_guard<std::mutex> guard(clock_sub_lock_);
  clock_subscription_.reset();
}

void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
{
  // Filter out events on 'use_sim_time' parameter instances in other nodes.
  if (event->node != node_base_->get_fully_qualified_name()) {
    return;
  }
  // Filter for only 'use_sim_time' being added or changed.
  rclcpp::ParameterEventsFilter filter(event, {"use_sim_time"},
    {rclcpp::ParameterEventsFilter::EventType::NEW,
      rclcpp::ParameterEventsFilter::EventType::CHANGED});
  for (auto & it : filter.get_events()) {
    if (it.second->value.type != ParameterType::PARAMETER_BOOL) {
      RCLCPP_ERROR(logger_, "use_sim_time parameter cannot be set to anything but a bool");
      continue;
    }
    if (it.second->value.bool_value) {
      parameter_state_ = SET_TRUE;
      enable_ros_time();
      create_clock_sub();
    } else {
      parameter_state_ = SET_FALSE;
      disable_ros_time();
      destroy_clock_sub();
    }
  }
  // Handle the case that use_sim_time was deleted.
  rclcpp::ParameterEventsFilter deleted(event, {"use_sim_time"},
    {rclcpp::ParameterEventsFilter::EventType::DELETED});
  for (auto & it : deleted.get_events()) {
    (void) it;  // if there is a match it's already matched, don't bother reading it.
    // If the parameter is deleted mark it as unset but dont' change state.
    parameter_state_ = UNSET;
  }
}

void TimeSource::enable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
{
  auto ret = rcl_enable_ros_time_override(&clock->rcl_clock_);
  if (ret != RCL_RET_OK) {
    rclcpp::exceptions::throw_from_rcl_error(
      ret, "Failed to enable ros_time_override_status");
  }
}

void TimeSource::disable_ros_time(std::shared_ptr<rclcpp::Clock> clock)
{
  auto ret = rcl_disable_ros_time_override(&clock->rcl_clock_);
  if (ret != RCL_RET_OK) {
    rclcpp::exceptions::throw_from_rcl_error(
      ret, "Failed to enable ros_time_override_status");
  }
}

void TimeSource::enable_ros_time()
{
  if (ros_time_active_) {
    // already enabled no-op
    return;
  }

  // Local storage
  ros_time_active_ = true;

  // Update all attached clocks to zero or last recorded time
  std::lock_guard<std::mutex> guard(clock_list_lock_);
  auto time_msg = std::make_shared<builtin_interfaces::msg::Time>();
  if (last_msg_set_) {
    time_msg = std::make_shared<builtin_interfaces::msg::Time>(last_msg_set_->clock);
  }
  for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
    set_clock(time_msg, true, *it);
  }
}

void TimeSource::disable_ros_time()
{
  if (!ros_time_active_) {
    // already disabled no-op
    return;
  }

  // Local storage
  ros_time_active_ = false;

  // Update all attached clocks
  std::lock_guard<std::mutex> guard(clock_list_lock_);
  for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
    auto msg = std::make_shared<builtin_interfaces::msg::Time>();
    set_clock(msg, false, *it);
  }
}

}  // namespace rclcpp
<|endoftext|>