void simulatorLoop() { // The main application loop (excluding the GUI part) static bool wasRunning=false; int auxValues[4]; int messageID=0; int dataSize; if (autoStart) { simStartSimulation(); autoStart=false; } while (messageID!=-1) { simChar* data=simGetSimulatorMessage(&messageID,auxValues,&dataSize); if (messageID!=-1) { if (messageID==sim_message_simulation_start_resume_request) simStartSimulation(); if (messageID==sim_message_simulation_pause_request) simPauseSimulation(); if (messageID==sim_message_simulation_stop_request) simStopSimulation(); if (data!=NULL) simReleaseBuffer(data); } } // Handle a running simulation: if ( (simGetSimulationState()&sim_simulation_advancing)!=0 ) { wasRunning=true; if ( (simGetRealTimeSimulation()!=1)||(simIsRealTimeSimulationStepNeeded()==1) ) { if ((simHandleMainScript()&sim_script_main_script_not_called)==0) simAdvanceSimulationByOneStep(); if ((simStopDelay>0)&&(simGetSimulationTime()>=float(simStopDelay)/1000.0f)) { simStopDelay=0; simStopSimulation(); } } } else { if (wasRunning&&autoQuit) { wasRunning=false; simQuitSimulator(true); // will post the quit command } } }
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; } }