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