00001 00002 00003 #include "vrep_ros_plugin/GenericObjectHandler.h" 00004 #include "vrep_ros_plugin/access.h" 00005 #include "v_repLib.h" 00006 00007 GenericObjectHandler::GenericObjectHandler(): 00008 _id(-1), 00009 _associatedObjectUniqueID(-1), 00010 _associatedObjectID(-1), 00011 _initialized(false), 00012 _nh(ros::this_node::getName()){ 00013 } 00014 00015 GenericObjectHandler::~GenericObjectHandler(){ 00016 } 00017 00018 void GenericObjectHandler::setID(int newID){ 00019 _id=newID; 00020 } 00021 00022 int GenericObjectHandler::getID(){ 00023 return(_id); 00024 } 00025 00026 void GenericObjectHandler::setAssociatedObject(int objID,int objUniqueID){ 00027 _associatedObjectID=objID; 00028 _associatedObjectUniqueID=objUniqueID; 00029 } 00030 00031 int GenericObjectHandler::getAssociatedObject(){ 00032 return(_associatedObjectID); 00033 } 00034 00035 int GenericObjectHandler::getAssociatedObjectUniqueId(){ 00036 return(_associatedObjectUniqueID); 00037 } 00038 00039 void GenericObjectHandler::synchronizeSceneObject(){ 00040 // We update GenericObjectHandler's associated scene object custom data: 00041 // putTagToSceneObject(_associatedObjectID,0.0); 00042 } 00043 00044 void GenericObjectHandler::synchronize(){ 00045 } 00046 00047 void GenericObjectHandler::getDeveloperCustomData(std::vector<unsigned char> &developerCustomData){ 00048 const uint buffSize=simGetObjectCustomDataLength(_associatedObjectID,CustomDataHeaders::DEVELOPER_DATA_HEADER); 00049 developerCustomData.resize(buffSize); 00050 simGetObjectCustomData(_associatedObjectID,CustomDataHeaders::DEVELOPER_DATA_HEADER,(simChar*)developerCustomData.data()); 00051 } 00052 00053 00054 void GenericObjectHandler::startOfSimulation(){ 00055 _initialize(); 00056 } 00057 00058 bool GenericObjectHandler::endOfSimulation(){ 00059 _initialized=false; 00060 return(false); // We don't want this object automatically destroyed at the end of simulation 00061 }