PbdSceneGraphSource.cpp

Go to the documentation of this file.

#include "trainer/source/PbdSceneGraphSource.h"

namespace SceneModel {
 
  PbdSceneGraphSource::PbdSceneGraphSource()
  {
  }
  
  PbdSceneGraphSource::~PbdSceneGraphSource()
  {
  }

  void PbdSceneGraphSource::addSceneGraphMessage(std::vector<ISM::ObjectSetPtr> pMessage)
  {

   // ROS_INFO_STREAM("capacity " << pMessage.capacity());
    for  (ISM::ObjectSetPtr &objectSet : pMessage) // access by reference to avoid copying
          {
        for(ISM::ObjectPtr &ismObject : (new ISM::ObjectSet(*objectSet))->objects){
            boost::shared_ptr<ISM::ObjectSet> set = mObjectSetList.getSetIfExists((new ISM::Object(*ismObject))->type);

                if(!set){

                    ISM::ObjectSet *helper = new ISM::ObjectSet();

                    helper->mIdentifier = (new ISM::Object(*ismObject))->type;

                    set.reset(helper);
                    mObjectSetList.mObjectSets.push_back(set);

                  }


            // Add a new observed object to the object set.
            set->objects.push_back(ismObject);
        }

      }


  }
}