00001
00002 #include "vrep_ros_plugin/access.h"
00003 #include "v_repLib.h"
00004
00005 #include <vrep_ros_plugin/GenericObjectContainer.h>
00006
00007 #include "vrep_ros_plugin/ConsoleHandler.h"
00008
00009
00010 GenericObjectContainer::GenericObjectContainer():
00011 _object_handler_loader("vrep_ros_plugin", "GenericObjectHandler"),
00012 _nh(ros::this_node::getName()){
00013
00014 std::vector< std::string > plugins = _object_handler_loader.getDeclaredClasses();
00015
00016 std::stringstream ss;
00017 ss << "Available ObjectHandlers: " << std::endl;
00018 for (uint i=0; i<plugins.size();++i){
00019 ss << plugins[i] << std::endl;
00020 try{
00021 _allExistingPlugins.insert ( std::pair<std::string, boost::shared_ptr<GenericObjectHandler> >(plugins[i],_object_handler_loader.createInstance(plugins[i])) );
00022 } catch(pluginlib::PluginlibException& ex){
00023
00024 ROS_ERROR("The plugin %s failed to load for some reason. Error: %s", plugins[i].c_str(), ex.what());
00025 }
00026 }
00027
00028 _pubClock = _nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
00029
00030 ConsoleHandler::printInConsole(ss);
00031 }
00032
00033 GenericObjectContainer::~GenericObjectContainer(){
00034 removeAll();
00035 }
00036
00037 void GenericObjectContainer::removeAll(){
00038 _allObjects.clear();
00039 _allExistingPlugins.clear();
00040 }
00041
00042 void GenericObjectContainer::removeFromID(int theID){
00043 for (int i=0;i<getCount();i++)
00044 {
00045 if (_allObjects[i]->getID()==theID)
00046 {
00047 _allObjects.erase(_allObjects.begin()+i);
00048 break;
00049 }
00050 }
00051 }
00052
00053 int GenericObjectContainer::insert(boost::shared_ptr<GenericObjectHandler> objectHandler){
00054 int newID=0;
00055 while (getFromID(newID)!=NULL)
00056 newID++;
00057 _allObjects.push_back(objectHandler);
00058 objectHandler->setID(newID);
00059 return(newID);
00060 }
00061
00062 boost::shared_ptr<GenericObjectHandler> GenericObjectContainer::getFromID(int theID){
00063 for (int i=0;i<getCount();i++)
00064 {
00065 if (_allObjects[i]->getID()==theID)
00066 return(_allObjects[i]);
00067 }
00068 return boost::shared_ptr<GenericObjectHandler>();
00069 }
00070
00071 boost::shared_ptr<GenericObjectHandler> GenericObjectContainer::getFromIndex(int ind){
00072 if ( (ind<0)||(ind>=getCount()) )
00073 return boost::shared_ptr<GenericObjectHandler>();
00074 return(_allObjects[ind]);
00075 }
00076
00077 boost::shared_ptr<GenericObjectHandler> GenericObjectContainer::getFromAssociatedObject(int theAssociatedObjectID){
00078 for (int i=0;i<getCount();i++)
00079 {
00080 if (_allObjects[i]->getAssociatedObject()==theAssociatedObjectID)
00081 return(_allObjects[i]);
00082 }
00083 return boost::shared_ptr<GenericObjectHandler>();
00084 }
00085
00086 int GenericObjectContainer::getCount(){
00087 return(int(_allObjects.size()));
00088 }
00089
00090 void GenericObjectContainer::startOfSimulation(){
00091 for (int i=0;i<getCount();i++)
00092 _allObjects[i]->startOfSimulation();
00093 }
00094
00095 void GenericObjectContainer::endOfSimulation(){
00096 for (int i=0;i<getCount();i++)
00097 {
00098 if (_allObjects[i]->endOfSimulation())
00099 {
00100 _allObjects.erase(_allObjects.begin()+i);
00101 i--;
00102 }
00103 }
00104 }
00105
00106 void GenericObjectContainer::handleSimulation(){
00107
00108 for (int i=0;i<getCount();i++)
00109 _allObjects[i]->handleSimulation();
00110
00111 _clock_msg.clock = ros::Time(simGetSimulationTime());
00112 _pubClock.publish(_clock_msg);
00113 }
00114
00115 void GenericObjectContainer::actualizeForSceneContent(){
00116
00117 int i=0;
00118 while (i<int(_allObjects.size()))
00119 {
00120 boost::shared_ptr<GenericObjectHandler> objectHandler=_allObjects[i];
00121 int associatedObject=objectHandler->getAssociatedObject();
00122 int uniqueID;
00123 if (simGetObjectUniqueIdentifier(associatedObject,&uniqueID)==-1)
00124 {
00125 removeFromID(objectHandler->getID());
00126 i=-1;
00127 }
00128 else
00129 {
00130 if (uniqueID!=objectHandler->getAssociatedObjectUniqueId())
00131 {
00132 removeFromID(objectHandler->getID());
00133 i=-1;
00134 } else {
00135
00136
00137 int buffSize=simGetObjectCustomDataLength(associatedObject,CustomDataHeaders::DEVELOPER_DATA_HEADER);
00138 char* datBuff=new char[buffSize];
00139 simGetObjectCustomData(associatedObject,CustomDataHeaders::DEVELOPER_DATA_HEADER,datBuff);
00140 std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
00141 delete[] datBuff;
00142
00143 std::vector<unsigned char> tempMainData;
00144 if (!CAccess::extractSerializationData(developerCustomData,objectHandler->getObjectType(),tempMainData))
00145 {
00146 removeFromID(objectHandler->getID());
00147 i=-1;
00148 }
00149 }
00150 }
00151 i++;
00152 }
00153
00154
00155
00156
00157
00158 int objHandle=0;
00159 int ind=0;
00160 while (objHandle!=-1)
00161 {
00162 objHandle=simGetObjects(ind++,sim_handle_all);
00163 if (objHandle!=-1)
00164 {
00165
00166 if (getFromAssociatedObject(objHandle)==NULL)
00167 {
00168
00169
00170 int buffSize=simGetObjectCustomDataLength(objHandle,CustomDataHeaders::DEVELOPER_DATA_HEADER);
00171 char* datBuff=new char[buffSize];
00172 simGetObjectCustomData(objHandle, CustomDataHeaders::DEVELOPER_DATA_HEADER,datBuff);
00173 std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
00174 delete[] datBuff;
00175
00176 std::vector<unsigned char> tempMainData;
00177 bool objectFound = false;
00178
00179 boost::shared_ptr<GenericObjectHandler> objectHandler;
00180
00181 std::stringstream ss;
00182
00183 for (std::map< std::string, boost::shared_ptr<GenericObjectHandler> >::iterator it = _allExistingPlugins.begin(); it!=_allExistingPlugins.end(); ++it){
00184 if ((objectFound = CAccess::extractSerializationData(developerCustomData, it->second->getObjectType(), tempMainData)) == true) {
00185
00186 try {
00187 objectHandler = _object_handler_loader.createInstance(it->first);
00188 insert(objectHandler);
00189 int uniqueID;
00190 simGetObjectUniqueIdentifier(objHandle,&uniqueID);
00191 objectHandler->setAssociatedObject(objHandle,uniqueID);
00192 objectHandler->synchronize();
00193 ss << "Instantiating new " << it->first << " for " << simGetObjectName(objHandle) << "." << std::endl;
00194 ConsoleHandler::printInConsole(ss);
00195 } catch(pluginlib::PluginlibException& ex){
00196
00197 ROS_ERROR("The plugin %s failed to load for some reason. Error: %s", it->first.c_str(), ex.what());
00198 }
00199 }
00200 }
00201 }
00202 }
00203 }
00204 }
00205
00206
00207
00208
00209 void GenericObjectContainer::simExtGetAllCustomData(SLuaCallBack* p){
00210 if (p->inputArgCount!=1){
00211 simSetLastError(pluginName,"This function requires 1 input argument.");
00212 return;
00213 }
00214
00215 if (p->outputArgCount>1){
00216 simSetLastError(pluginName,"This function accepts 1 output argument.");
00217 return;
00218 }
00219
00220 if(p->inputArgTypeAndSize[0]!=sim_lua_arg_int || p->inputInt[0]<0){
00221 simSetLastError(pluginName,"Wrong input argument type.");
00222 return;
00223 }
00224
00225 if (p->outputArgCount==1 &&
00226 p->outputArgTypeAndSize[0]!=sim_lua_arg_string){
00227 simSetLastError(pluginName,"Wrong output argument type.");
00228 return;
00229 }
00230
00231
00232 const int objectHandle = p->inputInt[0];
00233
00234
00235 const uint buffSize=simGetObjectCustomDataLength(objectHandle, CustomDataHeaders::DEVELOPER_DATA_HEADER);
00236
00237 if (buffSize<0){
00238 simSetLastError(pluginName,"The object does not contain any custom data for this plugin.");
00239 return;
00240 }
00241
00242 std::vector<unsigned char> developerCustomData(buffSize);
00243 simGetObjectCustomData(objectHandle,CustomDataHeaders::DEVELOPER_DATA_HEADER,(simChar*)developerCustomData.data());
00244
00245 std::stringstream ss;
00246 ss << "[";
00247 while (developerCustomData.size()>0){
00248 const int* dataHeader = ((int *)developerCustomData.data());
00249 const int* dataLength = ((int *)developerCustomData.data()+1);
00250 const unsigned char * data = developerCustomData.data()+2*sizeof(int);
00251
00252 ss << std::dec << *dataHeader << ", " << *dataLength << ", ";
00253 if (*dataHeader == CustomDataHeaders::QUADROTOR_DATA_CTRL_MODE){
00254 ss << *((int *)data);
00255 } else if (*dataHeader == CustomDataHeaders::QUADROTOR_DATA_TF_RATIO){
00256 ss << *((float*)data);
00257 } else {
00258 ss << "0x";
00259 for (uint i=0; i<*dataLength; ++i){
00260 ss << std::hex << std::setw(2) << std::setfill('0') << (int)data[i];
00261 }
00262 }
00263
00264 developerCustomData.erase(developerCustomData.begin(), developerCustomData.begin()+2*sizeof(int)+*dataLength);
00265 if (developerCustomData.size()>0){
00266 ss << "; ";
00267 } else {
00268 ss << "]";
00269 }
00270 }
00271
00272 p->outputArgCount=1;
00273 p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt));
00274 p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_string;
00275 p->outputArgTypeAndSize[2*0+1]=sim_lua_arg_nil;
00276 p->outputChar=(simChar*)simCreateBuffer(ss.str().size()+1);
00277 strcpy(p->outputChar,ss.str().c_str());
00278 }
00279
00280
00281 void GenericObjectContainer::simExtGetCustomDataFromHeader(SLuaCallBack* p){
00282 if (p->inputArgCount!=2){
00283 simSetLastError(pluginName,"This function requires 2 input arguments.");
00284 return;
00285 }
00286
00287 if (p->outputArgCount>1){
00288 simSetLastError(pluginName,"This function accepts 1 output argument.");
00289 return;
00290 }
00291
00292 if(p->inputArgTypeAndSize[0]!=sim_lua_arg_int || p->inputInt[0]<0 ||
00293 p->inputArgTypeAndSize[2]!=sim_lua_arg_int){
00294 simSetLastError(pluginName,"Wrong input argument type.");
00295 return;
00296 }
00297
00298 if (p->outputArgCount==1 &&
00299 p->outputArgTypeAndSize[0]!=sim_lua_arg_float){
00300 simSetLastError(pluginName,"Wrong output argument type.");
00301 return;
00302 }
00303
00304
00305 const int objectHandle = p->inputInt[0];
00306 const int dataHeader = p->inputInt[1];
00307
00308 const uint buffSize=simGetObjectCustomDataLength(objectHandle, CustomDataHeaders::DEVELOPER_DATA_HEADER);
00309
00310 if (buffSize<0){
00311 simSetLastError(pluginName,"The object does not contain any custom data for this plugin.");
00312 return;
00313 }
00314
00315 std::vector<unsigned char> developerCustomData(buffSize);
00316 simGetObjectCustomData(objectHandle,CustomDataHeaders::DEVELOPER_DATA_HEADER,(simChar*)developerCustomData.data());
00317
00318 std::vector<unsigned char> tempData;
00319
00320 if(CAccess::extractSerializationData(developerCustomData, dataHeader, tempData)){
00321 p->outputArgCount=1;
00322 p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt));
00323 p->outputArgTypeAndSize[1]=sim_lua_arg_nil;
00324
00325 if (dataHeader == CustomDataHeaders::QUADROTOR_DATA_CTRL_MODE){
00326 p->outputArgTypeAndSize[0]=sim_lua_arg_int;
00327 p->outputInt=(simInt*)simCreateBuffer(1*sizeof(simInt));
00328 p->outputInt[0] = (simInt)CAccess::pop_int(tempData);
00329
00330 } else {
00331 p->outputArgTypeAndSize[0]=sim_lua_arg_float;
00332 p->outputFloat=(simFloat*)simCreateBuffer(1*sizeof(simFloat));
00333 p->outputFloat[0] = (simFloat)CAccess::pop_float(tempData);
00334 }
00335 } else {
00336 p->outputArgCount=0;
00337 simSetLastError(pluginName,"The data was not found.");
00338 }
00339 }
00340
00341
00342 simVoid GenericObjectContainer::simExtSetFloatCustomDataFromHeader(SLuaCallBack* p){
00343
00344 if (p->inputArgCount!=3){
00345 simSetLastError(pluginName,"This function requires 2 input arguments.");
00346 return;
00347 }
00348
00349 if (p->outputArgCount>1){
00350 simSetLastError(pluginName,"This function accepts 1 output argument.");
00351 return;
00352 }
00353
00354 if(p->inputArgTypeAndSize[0]!=sim_lua_arg_int || p->inputInt[0]<0 ||
00355 p->inputArgTypeAndSize[2]!=sim_lua_arg_int ||
00356 p->inputArgTypeAndSize[4]!=sim_lua_arg_float){
00357 simSetLastError(pluginName,"Wrong input argument type.");
00358 return;
00359 }
00360
00361 if (p->outputArgCount==1 &&
00362 p->outputArgTypeAndSize[0]!=sim_lua_arg_float){
00363 simSetLastError(pluginName,"Wrong output argument type.");
00364 return;
00365 }
00366
00367
00368 const int objectHandle = p->inputInt[0];
00369 const int dataHeader = p->inputInt[1];
00370
00371 const uint buffSize=simGetObjectCustomDataLength(objectHandle, CustomDataHeaders::DEVELOPER_DATA_HEADER);
00372
00373 if (buffSize<0){
00374 simSetLastError(pluginName,"Error getting custom data from the object.");
00375 return;
00376 }
00377
00378 std::vector<unsigned char> developerCustomData(buffSize);
00379 simGetObjectCustomData(objectHandle,CustomDataHeaders::DEVELOPER_DATA_HEADER,(simChar*)developerCustomData.data());
00380
00381
00382 std::vector<unsigned char> tempData;
00383 CAccess::extractSerializationData(developerCustomData, dataHeader, tempData);
00384
00385 tempData.clear();
00386
00387 const float dataValue = p->inputFloat[0];
00388 CAccess::push_float(tempData, dataValue);
00389
00390 CAccess::insertSerializationData(developerCustomData,dataHeader,tempData);
00391
00392
00393 simAddObjectCustomData(objectHandle,CustomDataHeaders::DEVELOPER_DATA_HEADER,
00394 (simChar*)developerCustomData.data(),int(developerCustomData.size()));
00395
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412 }
00413
00414 simVoid GenericObjectContainer::simExtSetFloatArrayCustomDataFromHeader(SLuaCallBack* p){
00415
00416 if (p->inputArgCount!=3){
00417 simSetLastError(pluginName,"This function requires 2 input arguments.");
00418 return;
00419 }
00420
00421 if (p->outputArgCount>1){
00422 simSetLastError(pluginName,"This function accepts 1 output argument.");
00423 return;
00424 }
00425
00426 if(p->inputArgTypeAndSize[0]!=sim_lua_arg_int || p->inputInt[0]<0 ||
00427 p->inputArgTypeAndSize[2]!=sim_lua_arg_int ||
00428 p->inputArgTypeAndSize[4]!=(sim_lua_arg_float | sim_lua_arg_table)){
00429 simSetLastError(pluginName,"Wrong input argument type.");
00430 return;
00431 }
00432
00433 if (p->outputArgCount==1 &&
00434 p->outputArgTypeAndSize[0]!=sim_lua_arg_float){
00435 simSetLastError(pluginName,"Wrong output argument type.");
00436 return;
00437 }
00438
00439
00440 const int objectHandle = p->inputInt[0];
00441 const int dataHeader = p->inputInt[1];
00442
00443 const uint buffSize=simGetObjectCustomDataLength(objectHandle, CustomDataHeaders::DEVELOPER_DATA_HEADER);
00444
00445 if (buffSize<0){
00446 simSetLastError(pluginName,"Error getting custom data from the object.");
00447 return;
00448 }
00449
00450 std::vector<unsigned char> developerCustomData(buffSize);
00451 simGetObjectCustomData(objectHandle,CustomDataHeaders::DEVELOPER_DATA_HEADER,(simChar*)developerCustomData.data());
00452
00453
00454 std::vector<unsigned char> tempData;
00455 CAccess::extractSerializationData(developerCustomData, dataHeader, tempData);
00456
00457 tempData.clear();
00458
00459
00460 std::vector<simFloat> dataValue(p->inputArgTypeAndSize[5]);
00461 memcpy(dataValue.data(), p->inputFloat, p->inputArgTypeAndSize[5]*sizeof(simFloat));
00462 CAccess::push_float(tempData, dataValue);
00463
00464 CAccess::insertSerializationData(developerCustomData,dataHeader,tempData);
00465
00466
00467 simAddObjectCustomData(objectHandle,CustomDataHeaders::DEVELOPER_DATA_HEADER,
00468 (simChar*)developerCustomData.data(),int(developerCustomData.size()));
00469
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486 }
00487
00488 simVoid GenericObjectContainer::simExtSetIntCustomDataFromHeader(SLuaCallBack* p){
00489
00490 if (p->inputArgCount!=3){
00491 simSetLastError(pluginName,"This function requires 2 input arguments.");
00492 return;
00493 }
00494
00495 if (p->outputArgCount>1){
00496 simSetLastError(pluginName,"This function accepts 1 output argument.");
00497 return;
00498 }
00499
00500 if(p->inputArgTypeAndSize[0]!=sim_lua_arg_int || p->inputInt[0]<0 ||
00501 p->inputArgTypeAndSize[2]!=sim_lua_arg_int ||
00502 p->inputArgTypeAndSize[4]!=sim_lua_arg_int){
00503 simSetLastError(pluginName,"Wrong input argument type.");
00504 return;
00505 }
00506
00507 if (p->outputArgCount==1 &&
00508 p->outputArgTypeAndSize[0]!=sim_lua_arg_float){
00509 simSetLastError(pluginName,"Wrong output argument type.");
00510 return;
00511 }
00512
00513
00514 const int objectHandle = p->inputInt[0];
00515 const int dataHeader = p->inputInt[1];
00516
00517 const uint buffSize=simGetObjectCustomDataLength(objectHandle, CustomDataHeaders::DEVELOPER_DATA_HEADER);
00518
00519 if (buffSize<0){
00520 simSetLastError(pluginName,"Error getting custom data from the object.");
00521 return;
00522 }
00523
00524 std::vector<unsigned char> developerCustomData(buffSize);
00525 simGetObjectCustomData(objectHandle,CustomDataHeaders::DEVELOPER_DATA_HEADER,(simChar*)developerCustomData.data());
00526
00527
00528 std::vector<unsigned char> tempData;
00529 CAccess::extractSerializationData(developerCustomData, dataHeader, tempData);
00530
00531 tempData.clear();
00532
00533 const int dataValue = p->inputInt[2];
00534 CAccess::push_int(tempData, dataValue);
00535
00536 CAccess::insertSerializationData(developerCustomData,dataHeader,tempData);
00537
00538
00539 simAddObjectCustomData(objectHandle,CustomDataHeaders::DEVELOPER_DATA_HEADER,
00540 (simChar*)developerCustomData.data(),int(developerCustomData.size()));
00541
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558 }
00559
00560
00561
00562
00563