ServiceLocatedObject.h

Go to the documentation of this file.

/**********************************************************************************************/
#ifndef SERVICELOCATEDOBJECT_H
#define SERVICELOCATEDOBJECT_H

#include "lo/LocatedObject.h"


#include <string>
#include <vector>
#include <map>

#define LO_TYPE_PHYSICAL 1


#define XML_NODE_SaveableLo "SaveableLo"
#define XML_ATTRIBUTE_LOID "loid"
#define XML_ATTRIBUTE_LOIDFATHER "loidfather"
#define XML_NODE_MATPOSE "Pose"
#define XML_NODE_COVARIANCE "Cov"
#ifndef ID_WORLD
#define ID_WORLD 1
#endif

/************************************************************************
                        XMLTag.h - Copyright klank


**************************************************************************/
typedef struct _xmlTextWriter xmlTextWriter;
typedef struct _xmlTextReader xmlTextReader;
#define XML_NODE_STD_VECTOR "std_vector"
#define XML_NODE_STD_PAIR "std_pair"
#define XML_NODE_INT "int"
#define XML_NODE_DOUBLE "double"
#define XML_NODE_STRING "std_string"
#define XML_NODE_MATRIX "newmat_Matrix"
#define XML_ATTRIBUTE_ROWS "rows"
#define XML_ATTRIBUTE_COLS "cols"

/**********************************************************************************************/
namespace jlo
{
class ServiceInterface;

class XMLTag
{
public:

  // Constructors/Destructors
  //


  XMLTag ( std::string Name);

  virtual ~XMLTag ( );
/****************************************************************************/
  void WriteToFile(const std::string& stFile, XMLTag** fileReference = NULL) const;
/****************************************************************************/
    char* WriteToString() const;
/****************************************************************************/
  static XMLTag* ReadFromFile(const std::string &stFile);
/****************************************************************************/
  XMLTag* Clone() const;

/****************************************************************************/
  virtual void Write(xmlTextWriter* pWriter) const;
/****************************************************************************/
  static XMLTag* Read(xmlTextReader* pReader);
/****************************************************************************/
  template<typename T> static XMLTag* Tag(std::vector<T> vector, std::string name = "")
  {
    XMLTag* tag = new XMLTag(name.compare("") == 0 ? XML_NODE_STD_VECTOR : name);
#ifdef WIN32
    std::vector<T>::const_iterator iter;
    for(iter = vector.begin(); iter != vector.end(); iter++)
    {
      tag->AddChild(XMLTag::Tag((*iter)));
    }
#else
    for(unsigned int i = 0; i < vector.size(); i++)
    {
      tag->AddChild(XMLTag::Tag(vector[i]));
    }
#endif
    return tag;
  }
  template<typename T1, typename T2> static XMLTag* Tag(std::pair<T1, T2> p, std::string name = "")
  {
    XMLTag* tag = new XMLTag(name.compare("") == 0 ? XML_NODE_STD_PAIR : name);
    tag->AddChild(XMLTag::Tag(p.first));
    tag->AddChild(XMLTag::Tag(p.second));
    return tag;
  }


  static XMLTag* Tag(int n, std::string name = "");
  static XMLTag* Tag(double d, std::string name = "");
  static XMLTag* Tag(std::string value, std::string name = "");
  static XMLTag* Tag(const Matrix& alg, std::string name = "");


/****************************************************************************/
  template<typename T>
  static T Load(XMLTag* tag, T* t )
  {
    throw "Load Failed: Expected type has no loading function implemented";
  };

  template<typename T>
   static std::vector<T> Load(XMLTag* tag, std::vector<T>*  )
  {
    std::vector<T> vectorTemp;
    for(unsigned int i = 0; i < tag->CountChildren(); i++)
    {
      try
      {
        vectorTemp.push_back(Load(tag->GetChild(i), (T*)NULL));
      }
      catch(...)
      {
        printf("!!!!!!!!! Error reading a vector!!!!!!!!!! \n\n\n");
        /* Do nothing, try to get the others*/
      }
    }
    return vectorTemp;
  };

  template<typename T1, typename T2>
  static std::pair<T1, T2> Load(XMLTag* tag, std::pair<T1, T2>*  )
  {
    std::pair<T1, T2> p;
    int childCount = tag->CountChildren();
    if(childCount > 0)
      p.first = Load(tag->GetChild(0), (T1*)NULL);
    if(childCount > 1)
      p.second = Load(tag->GetChild(1), (T2*)NULL);

    return p;
  };

  static int Load(XMLTag* tag, int* )
  {
    if(tag == NULL)
      return 0;
    return  tag->GetCDataInt();
  };

  static double Load(XMLTag* tag,double *)
  {
    if(tag == NULL)
      return 0.0;
    return tag->GetCDataDouble();
  };

  static std::string Load(XMLTag* tag, std::string *)
  {
    if(tag == NULL)
      return "";
    return tag->GetCDataST();
  };

  static Matrix Load(XMLTag* tag, Matrix* );

/****************************************************************************/
  void AddProperty(const std::string& name, const std::string& value);
  void AddProperty(const std::string &name, const unsigned long &value);
  void AddProperty(const std::string& name, const int &value);
  void AddProperty(const std::string& name, const double &value);

/****************************************************************************/
  void SetCData(const int &value);
  void SetCData(const double &value);
  void SetCData(const std::string &value);

/****************************************************************************/
  int                   GetCDataInt() const;
  double                GetCDataDouble() const;
  std::string GetCDataST() const;


/****************************************************************************/
  std::string GetProperty(const std::string& name);
  double GetPropertyDouble(const std::string& name);
  int GetPropertyInt(const std::string& name);


/****************************************************************************/
  int AddChild(XMLTag* child, int position = -1);
  void ReplaceChild(XMLTag* child, int position);
/****************************************************************************/
  unsigned int CountChildren() const;
/****************************************************************************/
  void RemoveChild(const unsigned int &position);
  void RemoveChild(const std::string &name);

/****************************************************************************/
  XMLTag* GetChild(const unsigned int &position);
  XMLTag* GetChild(const std::string &name);
  XMLTag* GetChild(const std::string& name, int innerindex);

/****************************************************************************/
  std::string& GetName(){return m_name;}

  //static public Methods
  static unsigned int OldTag()
  {
    return 0;
  }

  unsigned long date();


  private:
    void Touch();

  // Private attributes
  //
  unsigned long                                                 m_lastChanged;
  std::vector<XMLTag*>                                  m_children;
  std::string                                                           m_cData;
  std::string                                                           m_name;
  std::map<std::string, std::string>            m_properties;
};

class ServiceLocatedObject :  public LocatedObject
{
public:
  //
  // Constructors/Destructors
  //


/********************************************************************/
  ServiceLocatedObject ( );

  ServiceLocatedObject(ServiceLocatedObject* locobj, const Matrix &matrix , const Matrix &covariance);


  ServiceLocatedObject(ServiceLocatedObject* locobj, double x = 0.0, double y = 0.0, double z = 0.0,
          double roll = 0.0, double pitch = 0.0, double yaw = 0.0,
          double sigmaX = 0.0, double sigmaY = 0.0, double sigmaZ = 0.0,
          double sigmaRoll = 0.0, double sigmaPitch = 0.0, double sigmaYaw = 0.0);

#ifdef _DEBUG
  void Print();
#endif
public:
  XMLTag* SaveComplete() ;
/*      void TransformPointLocally(const double& x_in, const double& y_in, const double& z_in, double& x_out, double& y_out, double& z_out, const double& scale);*/
  XMLTag* Save() ;

  void UpdateParent(ServiceLocatedObject* new_parent);
  void Update(Matrix m, Matrix cov,  ServiceLocatedObject*(* copy)(ServiceLocatedObject*, ServiceLocatedObject*), unsigned long(*del)(ServiceLocatedObject*), void (*updated)(unsigned long));
  void DecreaseReferenceCounter(){if(referenceCounter>0)referenceCounter--;}
  void IncreaseReferenceCounter(){referenceCounter++;}
  unsigned long GetReferenceCounter(){return referenceCounter;}
  /********************************************************************/
  virtual ~ServiceLocatedObject ( );

  /********************************************************************/
  ServiceLocatedObject operator- (ServiceLocatedObject &smallObject ) ;

  /********************************************************************/
  virtual void AddAttachedObject(ServiceLocatedObject* lo){IncreaseReferenceCounter();}

  /********************************************************************/
  virtual void RemoveAttachedObject(ServiceLocatedObject* lo){DecreaseReferenceCounter();}
  /********************************************************************/
  virtual void PropagateMovement(ServiceLocatedObject*(* copy)(ServiceLocatedObject*, ServiceLocatedObject*),
                                unsigned long (*del)(ServiceLocatedObject*), void (*updated)(unsigned long), ServiceLocatedObject*)
  {
    updated(m_uniqueID);
  }

  virtual bool NeedCopy (){return false;}
  virtual void TellParentNeedCopy(){return;}
  virtual void TellParentNeedNoCopy(){return;}

  /********************************************************************/
  virtual unsigned long GetLOType(){return LO_TYPE_PERCEIVED;}


  /********************************************************************/
  ServiceLocatedObject* m_relation;
  /********************************************************************/
  static void SetLastID(unsigned long lastID){if(lastID >= s_lastID) s_lastID = lastID + 1;}
  std::string m_mapstring;
protected:
  unsigned long referenceCounter;
  long m_needCopy;
private:
  friend class ServiceInterface;
  static unsigned long s_lastID;
};
  class LocatedObjectLoader : public LazyLocatedObjectLoader
  {
  public:
    LocatedObjectLoader(){}
    LocatedObject* GetParent(const LocatedObject& child){return ((ServiceLocatedObject*)&child)->m_relation;}
  };


}

#endif /*SERVICELOCATEDOBJECT*/