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