void CSubscriberData::addStatusbarMessageCallback(const std_msgs::String::ConstPtr& msg) { if (_handleGeneralCallback_before()) { if (simAddStatusbarMessage(msg->data.c_str())==-1) shutDownGeneralSubscriber(); _handleGeneralCallback_after(); } }
void xmlutils::reportUnknownAttributes(const std::string &widget, tinyxml2::XMLElement *e) { BOOST_FOREACH(const std::string &a, getUnknownAttributes(e)) { simAddStatusbarMessage((boost::format("WARNING: unknown UI XML attribute '%s' in widget '%s'") % a % widget).str().c_str()); }
void LUA_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack* p) { // the callback function of the new Lua command ("simExtSkeleton_getSensorData") // return Lua Table or arrays containing position, torque, torque minus motor force, timestamp, FRI state try { if (!kukaPluginPG) { BOOST_LOG_TRIVIAL(info) << "Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n"; CLuaFunctionData data; if (data.readDataFromLua(p,inArgs_KUKA_LBR_IIWA_START,inArgs_KUKA_LBR_IIWA_START[0],LUA_KUKA_LBR_IIWA_START_COMMAND)) { std::vector<CLuaFunctionDataItem>* inData=data.getInDataPtr(); std::vector<std::string> JointHandles; for (size_t i=0;i<inData->at(0).stringData.size();i++) { JointHandles.push_back(std::string(inData->at(0).stringData[i].c_str())); } std::string RobotTipHandle (inData->at(1 ).stringData[0]); std::string RobotTargetHandle (inData->at(2 ).stringData[0]); std::string RobotTargetBaseHandle (inData->at(3 ).stringData[0]); std::string RobotModel (inData->at(4 ).stringData[0]); std::string LocalZMQAddress (inData->at(5 ).stringData[0]); std::string RemoteZMQAddress (inData->at(6 ).stringData[0]); std::string LocalHostKukaKoniUDPAddress (inData->at(7 ).stringData[0]); std::string LocalHostKukaKoniUDPPort (inData->at(8 ).stringData[0]); std::string RemoteHostKukaKoniUDPAddress (inData->at(9 ).stringData[0]); std::string RemoteHostKukaKoniUDPPort (inData->at(10).stringData[0]); std::string KukaCommandMode (inData->at(11).stringData[0]); std::string KukaMonitorMode (inData->at(12).stringData[0]); std::string IKGroupName (inData->at(13).stringData[0]); kukaPluginPG=std::make_shared<grl::vrep::KukaVrepPlugin>( std::make_tuple( JointHandles , RobotTipHandle , RobotTargetHandle , RobotTargetBaseHandle , RobotModel , LocalZMQAddress , RemoteZMQAddress , LocalHostKukaKoniUDPAddress , LocalHostKukaKoniUDPPort , RemoteHostKukaKoniUDPAddress , RemoteHostKukaKoniUDPPort , KukaCommandMode , KukaMonitorMode , IKGroupName ) ); kukaPluginPG->construct(); } else { /// @todo report an error? // use default params kukaPluginPG=std::make_shared<grl::vrep::KukaVrepPlugin>(); kukaPluginPG->construct(); } } } 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(); } }
// 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); }
// Subscribers: void ROS_server::addStatusbarMessage_callback(const std_msgs::String::ConstPtr& msg) { simAddStatusbarMessage(msg->data.c_str()); }
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); } } }