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);
}
}
}
