// 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 4 lines at the beginning and unchanged:
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:
if (message==sim_message_eventcallback_instancepass)
{
// This message is sent each time the scene was rendered (well, shortly after) (very often)
// When a simulation is not running, but you still need to execute some commands, then put some code here
ROS_server::instancePass();
}
if (message==sim_message_eventcallback_mainscriptabouttobecalled)
{
// Main script is about to be run (only called while a simulation is running (and not paused!))
//
// This is a good location to execute simulation commands
if (ROS_server::mainScriptAboutToBeCalled())
{
simSetBooleanParameter(sim_boolparam_waiting_for_trigger,1); // the remote API client can query that value
replyData[0]=0; // this tells V-REP that we don't wanna execute the main script
}
else
simSetBooleanParameter(sim_boolparam_waiting_for_trigger,0); // the remote API client can query that value
}
if (message==sim_message_eventcallback_simulationabouttostart)
{
// Simulation is about to start
ROS_server::simulationAboutToStart();
}
if (message==sim_message_eventcallback_simulationended)
{
// Simulation just ended
simSetBooleanParameter(sim_boolparam_waiting_for_trigger,0); // the remote API client can query that value
ROS_server::simulationEnded();
}
// Keep following unchanged:
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
return(retVal);
}
void CSubscriberData::setBooleanParameterCallback(const std_msgs::UInt8::ConstPtr& param)
{
if (_handleGeneralCallback_before())
{
if (simSetBooleanParameter(auxInt1,param->data)==-1)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
// 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
// This function should not generate any error messages:
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
void* retVal=NULL;
if (message==sim_message_eventcallback_instancepass)
{ // This message is sent each time the scene was rendered (well, shortly after) (very often)
allConnections.instancePass();
}
if (message==sim_message_eventcallback_mainscriptabouttobecalled)
{ // Main script is about to be run
if (allConnections.thereWasARequestToCallTheMainScript())
{
simSetBooleanParameter(sim_boolparam_waiting_for_trigger,1); // the remote API client can query that value
replyData[0]=0; // this tells V-REP that we don't wanna execute the main script
}
else
{
simSetBooleanParameter(sim_boolparam_waiting_for_trigger,0); // the remote API client can query that value
allConnections.mainScriptWillBeCalled(); // this simply tells all remote API server services to reactivate their triggers (if that function is enabled)
}
}
if (message==sim_message_eventcallback_simulationended)
{ // Simulation just ended
simSetBooleanParameter(sim_boolparam_waiting_for_trigger,0);
allConnections.simulationEnded();
}
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
return(retVal);
}
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);
}
}
}
