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