void message_pump() { // Task Queue Check for unsynchronized task thrown from Service Port of RT-Component. Task t = taskQueue.popTask(); simInt ret; switch(t.value){ case Task::START: std::cout << " - Task::Starting Simulation" << std::endl; ret = simStartSimulation(); if (ret == 0) { returnQueue.returnReturn(Return(Return::RET_FAILED)); } else if(ret < 0) { returnQueue.returnReturn(Return(Return::RET_ERROR)); } else { returnQueue.returnReturn(Return(Return::RET_OK)); } break; case Task::STOP: std::cout << " - Task::Stopping Simulation" << std::endl; ret = simStopSimulation(); if (ret == 0) { returnQueue.returnReturn(Return(Return::RET_FAILED)); } else if(ret < 0) { returnQueue.returnReturn(Return(Return::RET_ERROR)); } else { returnQueue.returnReturn(Return(Return::RET_OK)); } break; case Task::PAUSE: std::cout << " - Task::Pausing Simulation" << std::endl; ret = simPauseSimulation(); if (ret == 0) { returnQueue.returnReturn(Return(Return::RET_FAILED)); } else if(ret < 0) { returnQueue.returnReturn(Return(Return::RET_ERROR)); } else { returnQueue.returnReturn(Return(Return::RET_OK)); } break; case Task::LOADPROJECT: std::cout << " - Task::Loading Project: " << t.key.c_str() << std::endl; ret = simLoadScene(t.key.c_str()); std::cout << " -- ret = " << ret; if (ret < 0) { returnQueue.returnReturn(Return(Return::RET_ERROR)); } else { returnQueue.returnReturn(Return(Return::RET_OK)); } break; case Task::SPAWNROBOT: std::cout << " - Task::SpawnRobot" << std::endl; if (spawnRobotRTC(t.key, t.arg) < 0) { returnQueue.returnReturn(Return(Return::RET_ERROR)); } else { returnQueue.returnReturn(Return(Return::RET_OK)); } break; case Task::SPAWNRANGE: std::cout << " - Task::Spawnrange" << std::endl; if (spawnRangeRTC(t.key, t.arg) < 0) { returnQueue.returnReturn(Return(Return::RET_ERROR)); } else { returnQueue.returnReturn(Return(Return::RET_OK)); } break; case Task::SPAWNDEPTH: std::cout << " - Task::Spawndepth" << std::endl; if (spawnDepthRTC(t.key, t.arg) < 0) { returnQueue.returnReturn(Return(Return::RET_ERROR)); } else { returnQueue.returnReturn(Return(Return::RET_OK)); } break; case Task::SPAWNACCEL: std::cout << " - Task::Spawnaccel" << std::endl; if (spawnAccelerometerRTC(t.key, t.arg) < 0) { returnQueue.returnReturn(Return(Return::RET_ERROR)); } else { returnQueue.returnReturn(Return(Return::RET_OK)); } break; case Task::SPAWNGYRO: std::cout << " - Task::Spawngyro" << std::endl; if (spawnGyroRTC(t.key, t.arg) < 0) { returnQueue.returnReturn(Return(Return::RET_ERROR)); } else { returnQueue.returnReturn(Return(Return::RET_OK)); } break; case Task::SPAWNCAMERA: std::cout << " - Task::SpawnCamera" << std::endl; if (spawnCameraRTC(t.key, t.arg) < 0) { returnQueue.returnReturn(Return(Return::RET_ERROR)); } else { returnQueue.returnReturn(Return(Return::RET_OK)); } break; case Task::SPAWNOBJECT: std::cout << " - Task::SpawnObject" << std::endl; if (spawnObjectRTC(t.key, t.arg) < 0) { returnQueue.returnReturn(Return(Return::RET_ERROR)); } else { returnQueue.returnReturn(Return(Return::RET_OK)); } break; case Task::KILLRTC: std::cout << " - Task::KillRTC" << std::endl; if (killRTC(t.key) < 0) { returnQueue.returnReturn(Return(Return::RET_ERROR)); } else { returnQueue.returnReturn(Return(Return::RET_OK)); } break; case Task::KILLALLRTC: std::cout << " - Task::KillAllRTC" << std::endl; if (killAllRTC() < 0) { returnQueue.returnReturn(Return(Return::RET_ERROR)); } else { returnQueue.returnReturn(Return(Return::RET_OK)); } break; case Task::SYNCRTC: std::cout << " - Task::SYNCRTC" << std::endl; if (syncRTC(t.key) < 0) { returnQueue.returnReturn(Return(Return::RET_ERROR)); } else { returnQueue.returnReturn(Return(Return::RET_OK)); } break; case Task::GETSYNCRTC: std::cout << " - Task::GETSYNCRTC" << std::endl; { Return r(Return::RET_OK); if (getSyncRTCs(r.stringList) < 0) { r.value = Return::RET_ERROR; returnQueue.returnReturn(r); } else { for(int i = 0;i < r.stringList.size();i++) { std::cout << " - Task: " << r.stringList[i] << std::endl; } returnQueue.returnReturn(r); } } break; case Task::GETSIMTIME: std::cout << " - Task::GETSIMTIME" << std::endl; { Return r(Return::RET_OK); r.floatValue = simGetSimulationTime(); returnQueue.returnReturn(r); } break; case Task::GETSIMSTEP: std::cout << " - Task::GETSIMSTEP" << std::endl; { Return r(Return::RET_OK); r.floatValue = simGetSimulationTimeStep(); returnQueue.returnReturn(r); } break; case Task::GETOBJPOSE: returnQueue.returnReturn(Return(Return::RET_OK)); break; case Task::SETOBJPOSE: returnQueue.returnReturn(Return(Return::RET_OK)); break; default: //returnQueue.returnReturn(Return(Return::RET_ERROR)); break; } }
void simulatorInit() { // called by the GUI thread! std::cout << "Simulator launched.\n"; // Get all the possible plugin files: char curDirAndFile[1024]; GetModuleFileName(NULL,curDirAndFile,1023); PathRemoveFileSpec(curDirAndFile); std::string currentDirAndPath(curDirAndFile); std::string temp(currentDirAndPath); temp+="\\v_repExt*.dll"; WIN32_FIND_DATA fData; HANDLE h=FindFirstFile(temp.c_str(),&fData); std::vector<std::string> theNames; std::vector<std::string> theDirAndNames; while (h!=INVALID_HANDLE_VALUE) { std::string tmp; int i=8; while (fData.cFileName[i]!='.') tmp+=fData.cFileName[i++]; std::string tmp2(currentDirAndPath); tmp2+="\\"; tmp2+=fData.cFileName; if (tmp.length()==0) tmp="VrepExt"; // This is the extension module of v_rep (exception in naming)! bool underscoreFound=false; for (int i=0;i<int(tmp.length());i++) { if (tmp[i]=='_') underscoreFound=true; } if (!underscoreFound) { theNames.push_back(tmp); theDirAndNames.push_back(tmp2); } if (FindNextFile(h,&fData)==0) break; } FindClose(h); // Load the system plugins first: for (int i=0;i<int(theNames.size());i++) { if ((theNames[i].compare("MeshCalc")==0)||(theNames[i].compare("Dynamics")==0)||(theNames[i].compare("PathPlanning")==0)) { int pluginHandle=loadPlugin(theNames[i].c_str(),theDirAndNames[i].c_str()); if (pluginHandle>=0) pluginHandles.push_back(pluginHandle); theDirAndNames[i]=""; // mark as 'already loaded' } } simLoadModule("",""); // indicate that we are done with the system plugins // Now load the other plugins too: for (int i=0;i<int(theNames.size());i++) { if (theDirAndNames[i].compare("")!=0) { // not yet loaded int pluginHandle=loadPlugin(theNames[i].c_str(),theDirAndNames[i].c_str()); if (pluginHandle>=0) pluginHandles.push_back(pluginHandle); } } if (sceneOrModelOrUiToLoad.length()!=0) { // Here we double-clicked a V-REP file or dragged-and-dropped it onto this application icon int l=int(sceneOrModelOrUiToLoad.length()); if ((l>4)&&(sceneOrModelOrUiToLoad[l-4]=='.')&&(sceneOrModelOrUiToLoad[l-3]=='t')&&(sceneOrModelOrUiToLoad[l-2]=='t')) { simSetBooleanParameter(sim_boolparam_scene_and_model_load_messages,1); if (sceneOrModelOrUiToLoad[l-1]=='t') // trying to load a scene? { if (simLoadScene(sceneOrModelOrUiToLoad.c_str())==-1) simAddStatusbarMessage("Scene could not be opened."); } if (sceneOrModelOrUiToLoad[l-1]=='m') // trying to load a model? { if (simLoadModel(sceneOrModelOrUiToLoad.c_str())==-1) simAddStatusbarMessage("Model could not be loaded."); } if (sceneOrModelOrUiToLoad[l-1]=='b') // trying to load a UI? { if (simLoadUI(sceneOrModelOrUiToLoad.c_str(),0,NULL)==-1) simAddStatusbarMessage("UI could not be loaded."); } simSetBooleanParameter(sim_boolparam_scene_and_model_load_messages,0); } } }