void CMtbRobotDialog::refresh()
{
inRefreshRoutine=true;
// Get handle of last selected object:
int lastSel=simGetObjectLastSelection();
updateObjectsInList();
// Now initialized other dialog elements:
CMtbRobot* taggedObj=NULL;
ui->qqRobotProgram->setPlainText("");
ui->qqAutomatic->setChecked(false);
if (lastSel!=-1)
{
taggedObj=CAccess::mtbRobotContainer->getFromAssociatedObject(lastSel);
if (taggedObj==NULL)
selectObjectInList(-1);
else
{ // The last selected object is the base object of a MTB model!
selectObjectInList(taggedObj->getID());
std::string prg(taggedObj->getProgram());
ui->qqRobotProgram->setPlainText(prg.c_str());
ui->qqAutomatic->setChecked((taggedObj->getOptions()&1)!=0);
}
}
else
selectObjectInList(-1);
ui->qqRobotProgram->setEnabled((taggedObj!=NULL)&&(simGetSimulationState()==sim_simulation_stopped));
ui->qqAutomatic->setEnabled((taggedObj!=NULL)&&(simGetSimulationState()==sim_simulation_stopped));
inRefreshRoutine=false;
}
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
}
}
}
// This is the plugin messaging routine (i.e. V-REP calls this function very often, with various messages):
VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customData,int* replyData)
{ // This is called quite often. Just watch out for messages/events you want to handle
// Keep following 6 lines at the beginning and unchanged:
static bool refreshDlgFlag=true;
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
void* retVal=NULL;
// Here we can intercept many messages from V-REP (actually callbacks).
// For a complete list of messages that you can intercept/react with, search for "sim_message_eventcallback"-type constants
// in the V-REP user manual.
if (message==sim_message_eventcallback_refreshdialogs)
refreshDlgFlag=true; // V-REP dialogs were refreshed. Maybe a good idea to refresh this plugin's dialog too
if (message==sim_message_eventcallback_menuitemselected)
{ // A custom menu bar entry was selected..
if (auxiliaryData[0]==urdfDialog->dialogMenuItemHandle)
urdfDialog->makeVisible(!urdfDialog->getVisible()); // Toggle visibility of the dialog
}
if (message==sim_message_eventcallback_instancepass)
{ // It is important to always correctly react to events in V-REP. This message is the most convenient way to do so:
urdfDialog->handleCommands();
urdfDialog->setSimulationStopped(simGetSimulationState()==sim_simulation_stopped);
int flags=auxiliaryData[0];
bool sceneContentChanged=((flags&(1+2+4+8+16+32+64+256))!=0); // object erased, created, model or scene loaded, und/redo called, instance switched, or object scaled since last sim_message_eventcallback_instancepass message
bool instanceSwitched=((flags&64)!=0);
if (instanceSwitched)
{
// React to an instance switch here!!
}
if (sceneContentChanged)
{
refreshDlgFlag=true;
}
}
if ((message==sim_message_eventcallback_guipass)&&refreshDlgFlag)
{ // handle refresh of the plugin's dialog:
urdfDialog->refresh(); // Refresh the dialog
refreshDlgFlag=false;
}
// Keep following unchanged:
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
return(retVal);
}
void CSimxContainer::executeCommands(CSimxContainer* outputContainer,CSimxSocket* sock)
{
if (outputContainer->getCommandCount()==0)
{ // Make sure the output buffer was already sent and is empty
outputContainer->setMessageID(_messageID);
outputContainer->setDataTimeStamp(_dataTimeStamp);
int simState=simGetSimulationState();
BYTE serverState=0;
outputContainer->setDataServerTimeStamp(int(simGetSystemTime()*1000.1f));
if (simState!=sim_simulation_stopped)
{
serverState|=1; // simulation is not stopped
if (simState==sim_simulation_paused)
serverState|=2; // simulation is paused
}
if (simGetRealTimeSimulation()>0)
serverState|=4;
int editModeType;
simGetIntegerParameter(sim_intparam_edit_mode_type,&editModeType);
serverState|=(editModeType<<3);
int sceneUniqueID;
simGetIntegerParameter(sim_intparam_scene_unique_id,&sceneUniqueID);
outputContainer->setSceneID(WORD(sceneUniqueID));
outputContainer->setServerState(serverState);
// Prepare for correct error reporting:
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_report);
char* err=simGetLastError(); // just clear the last error
if (err!=NULL)
simReleaseBuffer(err);
// Execute pending commands:
for (unsigned int i=0;i<_allCommands.size();i++)
{
CSimxCmd* outputCmd=_allCommands[i]->execute(sock,_otherSideIsBigEndian);
if (outputCmd!=NULL) // gradual or split commands are not added here!
outputContainer->addCommand(outputCmd,false);
}
// Restore previous error report mode:
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved);
_removeNonContinuousCommands(); // simx_opmode_oneshot_split commands are also kept!
}
}
void CSimxSocket::instancePass()
{
// 3/3/2014
// if (!_simulationOnly)
{
int simState=simGetSimulationState();
if ((simState&sim_simulation_advancing)==0)
_executeCommands();
}
if (_debug)
{ // we should never access the V-REP API from a thread not created in V-REP!
_lock();
for (unsigned int i=0;i<_textToPrintToConsole.size();i++)
simAuxiliaryConsolePrint(_auxConsoleHandle,_textToPrintToConsole[i].c_str());
_textToPrintToConsole.clear();
_unlock();
}
}
// This is the plugin messaging routine (i.e. V-REP calls this function very often, with various messages):
VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customData,int* replyData)
{ // This is called quite often. Just watch out for messages/events you want to handle
// Keep following 6 lines at the beginning and unchanged:
static bool refreshDlgFlag=true;
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
void* retVal=NULL;
// Here we can intercept many messages from V-REP (actually callbacks).
// For a complete list of messages that you can intercept/react with, search for "sim_message_eventcallback"-type constants
// in the V-REP user manual.
if (message==sim_message_eventcallback_refreshdialogs)
refreshDlgFlag=true; // V-REP dialogs were refreshed. Maybe a good idea to refresh this plugin's dialog too
if (message==sim_message_eventcallback_menuitemselected)
{ // A custom menu bar entry was selected..
if (auxiliaryData[0]==colladaDialog->dialogMenuItemHandle)
{
colladaDialog->makeVisible(!colladaDialog->getVisible()); // Toggle visibility of the dialog
}
}
if (message==sim_message_eventcallback_colladaplugin)
{
if (auxiliaryData[0]==0)
replyData[0]=PLUGIN_VERSION; // this is the version number of this plugin
if (auxiliaryData[0]==1)
replyData[0]=colladaDialog->importSingleGroupedShape((const char*)customData,(auxiliaryData[1]&1)!=0,float(auxiliaryData[2])/1000.0f);
}
if (message==sim_message_eventcallback_instancepass)
{ // It is important to always correctly react to events in V-REP. This message is the most convenient way to do so:
colladaDialog->handleCommands();
colladaDialog->setSimulationStopped(simGetSimulationState()==sim_simulation_stopped);
int flags=auxiliaryData[0];
bool sceneContentChanged=((flags&(1+2+4+8+16+32+64+256))!=0); // object erased, created, model or scene loaded, und/redo called, instance switched, or object scaled since last sim_message_eventcallback_instancepass message
bool instanceSwitched=((flags&64)!=0);
if (instanceSwitched)
{
// React to an instance switch here!!
}
if (sceneContentChanged)
{
refreshDlgFlag=true;
}
}
if ((message==sim_message_eventcallback_guipass)&&refreshDlgFlag)
{ // handle refresh of the plugin's dialog:
colladaDialog->refresh(); // Refresh the dialog
refreshDlgFlag=false;
}
if (message==sim_message_eventcallback_simulationabouttostart)
{
colladaDialog->simulationAboutToStart();
}
if (message==sim_message_eventcallback_mainscriptabouttobecalled)
{
colladaDialog->mainScriptAboutToBeCalled();
}
if (message==sim_message_eventcallback_beforerendering)
{ // we are still in the main SIM thread!
colladaDialog->renderingPassAboutToBeCalled();
}
if (message==sim_message_eventcallback_simulationended)
{
colladaDialog->simulationEnded();
}
// You can add many more messages to handle here
// Keep following unchanged:
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
return(retVal);
}
// This is the plugin messaging routine (i.e. V-REP calls this function very often, with various messages):
VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customData,int* replyData)
{ // This is called quite often. Just watch out for messages/events you want to handle
// Keep following 5 lines at the beginning and unchanged:
static bool refreshDlgFlag=true;
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
void* retVal=NULL;
// Here we can intercept many messages from V-REP (actually callbacks). Only the most important messages are listed here.
// For a complete list of messages that you can intercept/react with, search for "sim_message_eventcallback"-type constants
// in the V-REP user manual.
if (message==sim_message_eventcallback_refreshdialogs)
refreshDlgFlag=true; // V-REP dialogs were refreshed. Maybe a good idea to refresh this plugin's dialog too
if (message==sim_message_eventcallback_menuitemselected)
{ // A custom menu bar entry was selected..
// here you could make a plugin's main dialog visible/invisible
}
if (message==sim_message_eventcallback_instancepass)
{ // This message is sent each time the scene was rendered (well, shortly after) (very often)
// It is important to always correctly react to events in V-REP. This message is the most convenient way to do so:
int flags=auxiliaryData[0];
bool sceneContentChanged=((flags&(1+2+4+8+16+32+64+256))!=0); // object erased, created, model or scene loaded, und/redo called, instance switched, or object scaled since last sim_message_eventcallback_instancepass message
bool instanceSwitched=((flags&64)!=0);
if (instanceSwitched)
{
// React to an instance switch here!!
}
if (sceneContentChanged)
{ // we actualize plugin objects for changes in the scene
refreshDlgFlag=true; // always a good idea to trigger a refresh of this plugin's dialog here
}
//...
//////////////
// PUT MAIN CODE HERE
/////////////
if (simGetSimulationState() != sim_simulation_advancing_abouttostop) //checks if the simulation is still running
{
//if(kukaPluginPG) BOOST_LOG_TRIVIAL(info) << "current simulation time:" << simGetSimulationTime() << std::endl; // gets simulation time point
}
// make sure it is "right" (what does that mean?)
// find the v-rep C functions to do the following:
////////////////////////////////////////////////////
// Use handles that were found at the "start" of this simulation running
// next few Lines get the joint angles, torque, etc from the simulation
if (kukaPluginPG)// && kukaPluginPG->allHandlesSet == true // allHandlesSet now handled internally
{
try
{
// run one loop synchronizing the arm and plugin
kukaPluginPG->run_one();
} catch (const boost::exception& e){
// log the error and print it to the screen, don't release the exception
std::string initerr("v_repExtKukaLBRiiwa plugin encountered the following error and will disable itself:\n" + boost::diagnostic_information(e));
simAddStatusbarMessage( initerr.c_str());
BOOST_LOG_TRIVIAL(error) << initerr;
kukaPluginPG.reset();
} catch (const std::exception& e){
// log the error and print it to the screen, don't release the exception
std::string initerr("v_repExtKukaLBRiiwa plugin encountered the following error and will disable itself:\n" + boost::diagnostic_information(e));
simAddStatusbarMessage( initerr.c_str());
BOOST_LOG_TRIVIAL(error) << initerr;
kukaPluginPG.reset();
} catch (...){
// log the error and print it to the screen, don't release the exception
std::string initerr("v_repExtKukaLBRiiwa plugin encountered an unknown error and will disable itself. Please debug this issue! file and line:" + std::string(__FILE__) + " " + boost::lexical_cast<std::string>(__LINE__) + "\n");
simAddStatusbarMessage( initerr.c_str());
BOOST_LOG_TRIVIAL(error) << initerr;
kukaPluginPG.reset();
}
}
}
if (message==sim_message_eventcallback_mainscriptabouttobecalled)
{ // The main script is about to be run (only called while a simulation is running (and not paused!))
}
if (message==sim_message_eventcallback_simulationabouttostart)
{ // Simulation is about to start
/////////////////////////
// PUT OBJECT STARTUP CODE HERE
////////////////////
// get the handles to all the objects, joints, etc that we need
/////////////////////
// simGetObjectHandle
// try {
// BOOST_LOG_TRIVIAL(info) << "Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n";
// kukaPluginPG = std::make_shared<grl::KukaVrepPlugin>();
// kukaPluginPG->construct();
// //kukaPluginPG->run_one(); // for debugging purposes only
// //kukaPluginPG.reset(); // for debugging purposes only
// } catch (boost::exception& e){
// // log the error and print it to the screen, don't release the exception
// std::string initerr("v_repExtKukaLBRiiwa plugin initialization error:\n" + boost::diagnostic_information(e));
// simAddStatusbarMessage( initerr.c_str());
// BOOST_LOG_TRIVIAL(error) << initerr;
// }
}
if (message==sim_message_eventcallback_simulationended)
{ // Simulation just ended
/////////////////////////
// PUT OBJECT RESET CODE HERE
// close out as necessary
////////////////////
BOOST_LOG_TRIVIAL(info) << "Ending KUKA LBR iiwa plugin connection to Kuka iiwa\n";
kukaPluginPG.reset();
}
if (message==sim_message_eventcallback_moduleopen)
{ // A script called simOpenModule (by default the main script). Is only called during simulation.
if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
{
// we arrive here only at the beginning of a simulation
}
}
if (message==sim_message_eventcallback_modulehandle)
{ // A script called simHandleModule (by default the main script). Is only called during simulation.
if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
{
// we arrive here only while a simulation is running
}
}
if (message==sim_message_eventcallback_moduleclose)
{ // A script called simCloseModule (by default the main script). Is only called during simulation.
if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
{
// we arrive here only at the end of a simulation
}
}
if (message==sim_message_eventcallback_instanceswitch)
{ // Here the user switched the scene. React to this message in a similar way as you would react to a full
// scene content change. In this plugin example, we react to an instance switch by reacting to the
// sim_message_eventcallback_instancepass message and checking the bit 6 (64) of the auxiliaryData[0]
// (see here above)
}
if (message==sim_message_eventcallback_broadcast)
{ // Here we have a plugin that is broadcasting data (the broadcaster will also receive this data!)
}
if (message==sim_message_eventcallback_scenesave)
{ // The scene is about to be saved. If required do some processing here (e.g. add custom scene data to be serialized with the scene)
}
// You can add many more messages to handle here
if ((message==sim_message_eventcallback_guipass)&&refreshDlgFlag)
{ // handle refresh of the plugin's dialogs
// ...
refreshDlgFlag=false;
}
// Keep following unchanged:
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
return(retVal);
}
void ROS_server::instancePass()
{ // When simulation is not running, we "spinOnce" here:
int simState=simGetSimulationState();
if ((simState&sim_simulation_advancing)==0)
spinOnce();
}
void JointControlPluglet::v_repMessage_callback() {
if (simGetSimulationState() != sim_simulation_advancing_running) {
return;
}
}
void LaserScanPluglet::v_repMessage_callback() {
if (simGetSimulationState() != sim_simulation_advancing_running) {
return;
}
int packNumRead;
int packNumWrite;
simTubeStatus(tubeHandle, &packNumRead, &packNumWrite);
if (packNumRead > 0) {
int dlength = 0;
float* packet = (float*)simTubeRead(tubeHandle, &dlength);
float pos[3];
int size = dlength / sizeof(float);
int scan_size = size / 3;
sensor_msgs::LaserScan scan;
scan.header.frame_id = frame_id;
scan.header.stamp = ros::Time::now();
scan.angle_min = -240 * M_PI / 180 / 2;
scan.angle_max = -scan.angle_min;
scan.angle_increment = 2 * M_PI / 1024;
scan.range_min = 0.05;
scan.range_max = 4.5;
scan.scan_time = 0.05;
scan.intensities.resize(scan_size, 0.0);
scan.ranges.resize(scan_size, 0.0);
int scancounter = 0;
//simFloat matrix[12];
//simGetObjectMatrix(publishers[pubI].auxInt1, -1, matrix);
//simInvertMatrix(matrix);
for (int i=0; i<size-3; i+=3) {
pos[0] = packet[i+0];
pos[1] = packet[i+1];
pos[2] = packet[i+2];
//simTransformVector(matrix, pos);
//std::cout << "Tubedata received: " << pos[0] << ", " << pos[1] << ", " << pos[2] << ", " << std::endl;
float d = sqrt(pos[0]*pos[0] + pos[1]*pos[1]);
scan.ranges[scancounter++] = d;
}
publisher.publish(scan);
simReleaseBuffer((char*)packet);
} else {
//std::cout << "No Tubedata received" << std::endl;
}
}
// This is the plugin messaging routine (i.e. V-REP calls this function very often, with various messages):
VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customData,int* replyData)
{ // This is called quite often. Just watch out for messages/events you want to handle
// Keep following 6 lines at the beginning and unchanged:
simLockInterface(1);
static bool refreshDlgFlag=true;
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
void* retVal=NULL;
// Here we can intercept many messages from V-REP (actually callbacks). Only the most important messages are listed here.
// For a complete list of messages that you can intercept/react with, search for "sim_message_eventcallback"-type constants
// in the V-REP user manual.
if (message==sim_message_eventcallback_refreshdialogs)
refreshDlgFlag=true; // V-REP dialogs were refreshed. Maybe a good idea to refresh this plugin's dialog too
if (message==sim_message_eventcallback_menuitemselected)
{ // A custom menu bar entry was selected..
// here you could make a plugin's main dialog visible/invisible
}
if (message==sim_message_eventcallback_instancepass)
{ // This message is sent each time the scene was rendered (well, shortly after) (very often)
// It is important to always correctly react to events in V-REP. This message is the most convenient way to do so:
int flags=auxiliaryData[0];
bool sceneContentChanged=((flags&(1+2+4+8+16+32+64+256))!=0); // object erased, created, model or scene loaded, und/redo called, instance switched, or object scaled since last sim_message_eventcallback_instancepass message
bool instanceSwitched=((flags&64)!=0);
if (instanceSwitched)
{
// React to an instance switch here!!
}
if (simGetSimulationState() == sim_simulation_advancing_running) {
for (unsigned int i=0; i < vrep::VRepPlugletRegistry::getInstance()->getPluglets().size(); i++) {
if (vrep::VRepPlugletRegistry::getInstance()->getPluglets()[i]->is_active())
vrep::VRepPlugletRegistry::getInstance()->getPluglets()[i]->v_repMessage_callback();
}
}
if (sceneContentChanged)
{ // we actualize plugin objects for changes in the scene
//...
refreshDlgFlag=true; // always a good idea to trigger a refresh of this plugin's dialog here
}
}
if (message==sim_message_eventcallback_mainscriptabouttobecalled)
{ // The main script is about to be run (only called while a simulation is running (and not paused!))
}
if (message==sim_message_eventcallback_simulationabouttostart)
{ // Simulation is about to start
for (unsigned int i=0; i < vrep::VRepPlugletRegistry::getInstance()->getPluglets().size(); i++) {
if (vrep::VRepPlugletRegistry::getInstance()->getPluglets()[i]->is_active())
vrep::VRepPlugletRegistry::getInstance()->getPluglets()[i]->v_repSimStarts_callback();
}
}
if (message==sim_message_eventcallback_simulationended)
{ // Simulation just ended
for (unsigned int i=0; i < vrep::VRepPlugletRegistry::getInstance()->getPluglets().size(); i++) {
if (vrep::VRepPlugletRegistry::getInstance()->getPluglets()[i]->is_active())
vrep::VRepPlugletRegistry::getInstance()->getPluglets()[i]->v_repSimEnds_callback();
}
}
if (message==sim_message_eventcallback_moduleopen)
{ // A script called simOpenModule (by default the main script). Is only called during simulation.
if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
{
// we arrive here only at the beginning of a simulation
}
}
if (message==sim_message_eventcallback_modulehandle)
{ // A script called simHandleModule (by default the main script). Is only called during simulation.
if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
{
// we arrive here only while a simulation is running
}
}
if (message==sim_message_eventcallback_moduleclose)
{ // A script called simCloseModule (by default the main script). Is only called during simulation.
if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
{
// we arrive here only at the end of a simulation
}
}
if (message==sim_message_eventcallback_instanceswitch)
{ // Here the user switched the scene. React to this message in a similar way as you would react to a full
// scene content change. In this plugin example, we react to an instance switch by reacting to the
// sim_message_eventcallback_instancepass message and checking the bit 6 (64) of the auxiliaryData[0]
// (see here above)
}
if (message==sim_message_eventcallback_broadcast)
{ // Here we have a plugin that is broadcasting data (the broadcaster will also receive this data!)
}
if (message==sim_message_eventcallback_scenesave)
{ // The scene is about to be saved. If required do some processing here (e.g. add custom scene data to be serialized with the scene)
}
// You can add many more messages to handle here
if ((message==sim_message_eventcallback_guipass)&&refreshDlgFlag)
{ // handle refresh of the plugin's dialogs
// ...
refreshDlgFlag=false;
}
// Keep following unchanged:
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
simLockInterface(0);
return(retVal);
}
