VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{ // This is called just once, at the start of V-REP
// Dynamically load and bind V-REP functions:
char curDirAndFile[1024];
GetModuleFileName(NULL,curDirAndFile,1023);
PathRemoveFileSpec(curDirAndFile);
std::string currentDirAndPath(curDirAndFile);
std::string temp(currentDirAndPath);
temp+="\\v_rep.dll";
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib==NULL)
{
std::cout << "Error, could not find or correctly load v_rep.dll. Cannot start 'Cam' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "Error, could not find all required functions in v_rep.dll. Cannot start 'Cam' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<20600) // if V-REP version is smaller than 2.06.00
{
std::cout << "Sorry, your V-REP copy is somewhat old, V-REP 2.6.0 or later is required. Cannot start 'Cam' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // initialization failed!!
}
// Marc modified following function to return a neg. value in case of initialization error:
deviceCount=setupESCAPI();
if (deviceCount<0)
{
std::cout << "ESCAPI initialization failed (error code: " << deviceCount << "). Is 'escapi.dll' available? Cannot start 'Cam' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // initialization failed!!
}
// Register one new Lua command:
int inArgs1[]={3,sim_lua_arg_int,sim_lua_arg_int,sim_lua_arg_int};
simRegisterCustomLuaFunction(LUA_START,strConCat("number result,number resX,number resY=",LUA_START,"(number deviceIndex,number resX,number resY)"),inArgs1,LUA_START_CALLBACK);
int inArgs2[]={1,sim_lua_arg_int};
simRegisterCustomLuaFunction(LUA_END,strConCat("number result=",LUA_END,"(number deviceIndex)"),inArgs2,LUA_END_CALLBACK);
simRegisterCustomLuaFunction(LUA_INFO,strConCat("string info=",LUA_INFO,"(number index)"),inArgs2,LUA_INFO_CALLBACK);
int inArgs5[]={2,sim_lua_arg_int,sim_lua_arg_int};
simRegisterCustomLuaFunction(LUA_GRAB,strConCat("number result=",LUA_GRAB,"(number index,number visionSensorHandle)"),inArgs5,LUA_GRAB_CALLBACK);
InitializeCriticalSection(&m_cs);
return(3); // initialization went fine, return the version number of this extension module (can be queried with simGetModuleName)
// version 1 was for V-REP versions before V-REP 2.5.12
// version 2 was for V-REP versions before V-REP 2.6.0
}
// This is the plugin start routine (called just once, just after the plugin was loaded):
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{
// Dynamically load and bind V-REP functions:
// ******************************************
// 1. Figure out this plugin's directory:
char curDirAndFile[1024];
#ifdef _WIN32
GetModuleFileName(NULL,curDirAndFile,1023);
PathRemoveFileSpec(curDirAndFile);
#elif defined (__linux) || defined (__APPLE__)
getcwd(curDirAndFile, sizeof(curDirAndFile));
#endif
std::string currentDirAndPath(curDirAndFile);
// 2. Append the V-REP library's name:
std::string temp(currentDirAndPath);
#ifdef _WIN32
temp+="\\v_rep.dll";
#elif defined (__linux)
temp+="/libv_rep.so";
#elif defined (__APPLE__)
temp+="/libv_rep.dylib";
#endif /* __linux || __APPLE__ */
// 3. Load the V-REP library:
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib==NULL)
{
std::cout << "Error, could not find or correctly load the V-REP library. Cannot start 'PluginSkeleton' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "Error, could not find all required functions in the V-REP library. Cannot start 'PluginSkeleton' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Check the version of V-REP:
// ******************************************
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<20604) // if V-REP version is smaller than 2.06.04
{
std::cout << "Sorry, your V-REP copy is somewhat old. Cannot start 'PluginSkeleton' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
simLockInterface(1);
// Here you could handle various initializations
// Here you could also register custom Lua functions or custom Lua constants
// etc.
simLockInterface(0);
return(PLUGIN_VERSION); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
}
// This is the plugin start routine:
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{ // This is called just once, at the start of V-REP
// Dynamically load and bind V-REP functions:
char curDirAndFile[1024];
#ifdef _WIN32
#ifdef QT_COMPIL
_getcwd(curDirAndFile, sizeof(curDirAndFile));
#else
GetModuleFileName(NULL,curDirAndFile,1023);
PathRemoveFileSpec(curDirAndFile);
#endif
#elif defined (__linux) || defined (__APPLE__)
getcwd(curDirAndFile, sizeof(curDirAndFile));
#endif
std::string currentDirAndPath(curDirAndFile);
std::string temp(currentDirAndPath);
#ifdef _WIN32
temp+="\\v_rep.dll";
#elif defined (__linux)
temp+="/libv_rep.so";
#elif defined (__APPLE__)
temp+="/libv_rep.dylib";
#endif /* __linux || __APPLE__ */
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib==NULL)
{
std::cout << "Error, could not find or correctly load the V-REP library. Cannot start 'OpenMesh' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "Error, could not find all required functions in the V-REP library. Cannot start 'OpenMesh' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<30200) // if V-REP version is smaller than 3.02.00
{
std::cout << "Sorry, your V-REP copy is somewhat old. Cannot start 'OpenMesh' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
std::vector<int> inArgs;
// Register the new Lua commands:
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_DECIMATE,inArgs);
simRegisterCustomLuaFunction(LUA_DECIMATE_COMMAND,strConCat("table newVertices,table newIndices=",LUA_DECIMATE_COMMAND,"(table vertices,table indices,number maxVertices,number maxTriangles)"),&inArgs[0],LUA_DECIMATE_CALLBACK);
return(2); // initialization went fine, we return the version number of this extension module (can be queried with simGetModuleName)
}
// This is the plugin start routine (called just once, just after the plugin was loaded):
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{
// Dynamically load and bind V-REP functions:
// ******************************************
// 1. Figure out this plugin's directory:
char curDirAndFile[1024];
getcwd(curDirAndFile, sizeof(curDirAndFile));
std::string currentDirAndPath(curDirAndFile);
// 2. Append the V-REP library's name:
std::string temp(currentDirAndPath);
#ifdef _WIN32
temp+="\\v_rep.dll";
#elif defined (__linux)
temp+="/libv_rep.so";
#elif defined (__APPLE__)
temp+="/libv_rep.dylib";
#endif
// 3. Load the V-REP library:
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib==NULL)
{
std::cout << "Error, could not find or correctly load the V-REP library. Cannot start 'rosSkeleton' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "Error, could not find all required functions in the V-REP library. Cannot start 'rosSkeleton' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Check the version of V-REP:
// ******************************************
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<30102) // if V-REP version is smaller than 3.01.02
{
std::cout << "Sorry, your V-REP copy is somewhat old. Cannot start 'rosSkeleton' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Initialize the ROS part:
if(!ROS_server::initialize())
{
std::cout << "ROS master is not running. Cannot start 'rosSkeleton' plugin.\n";
return (0); //If the master is not running then the plugin is not loaded.
}
return(PLUGIN_VERSION); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
}
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{ // This is called just once, at the start of V-REP.
// Dynamically load and bind V-REP functions:
// ******************************************
// 1. Figure out this plugin's directory:
char curDirAndFile[1024];
#ifdef _WIN32
_getcwd(curDirAndFile, sizeof(curDirAndFile));
#elif defined (__linux) || defined (__APPLE__)
getcwd(curDirAndFile, sizeof(curDirAndFile));
#endif
std::string currentDirAndPath(curDirAndFile);
// 2. Append the V-REP library's name:
std::string temp(currentDirAndPath);
#ifdef _WIN32
temp+="/v_rep.dll";
#elif defined (__linux)
temp+="/libv_rep.so";
#elif defined (__APPLE__)
temp+="/libv_rep.dylib";
#endif /* __linux || __APPLE__ */
// 3. Load the V-REP library:
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib==NULL)
{
std::cout << "Error, could not find or correctly load the V-REP library. Cannot start 'ExternalRenderer' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "Error, could not find all required functions in the V-REP library. Cannot start 'ExternalRenderer' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Check the version of V-REP:
// ******************************************
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<30201) // if V-REP version is smaller than 3.02.01
{
std::cout << "Sorry, your V-REP copy is somewhat old. Cannot start 'ExternalRenderer' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
return(2); // initialization went fine, return the version number of this plugin!
}
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{ // This is called just once, at the start of V-REP
// Dynamically load and bind V-REP functions:
char curDirAndFile[1024];
GetModuleFileName(NULL,curDirAndFile,1023);
PathRemoveFileSpec(curDirAndFile);
std::string currentDirAndPath(curDirAndFile);
std::string temp(currentDirAndPath);
temp+="\\v_rep.dll";
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib==NULL)
{
std::cout << "Error, could not find or correctly load v_rep.dll. Cannot start 'Wii' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "Error, could not find all required functions in v_rep.dll. Cannot start 'Wii' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<20600) // if V-REP version is smaller than 2.06.00
{
printf("Sorry, your V-REP copy is somewhat old, V-REP 2.6.0 or later is required. Cannot start 'Wii' plugin.\n");
unloadVrepLibrary(vrepLib);
return(0); // initialization failed!!
}
// Register one new Lua command:
int inArgs1[]={1,sim_lua_arg_int};
simRegisterCustomLuaFunction(LUA_START,strConCat("number deviceIndex=",LUA_START,"(number deviceIndex)"),inArgs1,LUA_START_CALLBACK);
int inArgs2[]={1,sim_lua_arg_int};
simRegisterCustomLuaFunction(LUA_END,strConCat("number result=",LUA_END,"(number deviceIndex)"),inArgs2,LUA_END_CALLBACK);
int inArgs3[]={3,sim_lua_arg_int,sim_lua_arg_int,sim_lua_arg_bool};
simRegisterCustomLuaFunction(LUA_SET,strConCat("number result=",LUA_SET,"(number deviceIndex,number ledStates,boolean rumble)"),inArgs3,LUA_SET_CALLBACK);
int inArgs4[]={1,sim_lua_arg_int};
simRegisterCustomLuaFunction(LUA_GET,strConCat("number result,number buttons,table_3 accelerations,table_2 rollAndPitch,number battery=",LUA_GET,"(number deviceIndex)"),inArgs4,LUA_GET_CALLBACK);
InitializeCriticalSection(&m_cs);
return(3); // initialization went fine, return the version number of this plugin!
// version 1 was for V-REP 2.5.11 or earlier
// version 2 was for V-REP 2.6.0 or earlier
}
// This is the plugin end routine (called just once, when V-REP is ending, i.e. releasing this plugin):
VREP_DLLEXPORT void v_repEnd()
{
// Here you could handle various clean-up tasks
//pool_table.shutdown();
ros::shutdown();
unloadVrepLibrary(vrepLib); // release the library
}
// This is the plugin end routine (called just once, when V-REP is ending, i.e. releasing this plugin):
VREP_DLLEXPORT void v_repEnd()
{
// Here you could handle various clean-up tasks
exitRTM();
unloadVrepLibrary(vrepLib); // release the library
std::cout << "exiting v_repEnd" << std::endl;
}
VREP_DLLEXPORT void v_repEnd()
{ // This is called just once, at the end of V-REP
// Release resources here..
// The thread should have been exited alreads (all simulations stopped!)
unloadVrepLibrary(vrepLib); // release the library
}
VREP_DLLEXPORT void v_repEnd()
{ // This is called just once, at the end of V-REP
WIN_AFX_MANAGE_STATE;
delete objectContainer;
objectContainer=NULL;
unloadVrepLibrary(vrepLib); // release the library
}
// This is the plugin start routine (called just once, just after the plugin was loaded):
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{
// Dynamically load and bind V-REP functions:
// ******************************************
// 1. Figure out this plugin's directory:
char curDirAndFile[1024];
char * filenptr = getcwd(curDirAndFile, sizeof(curDirAndFile));
if (filenptr == NULL)
{
std::cout << "[V-REP_KEPLER] Cannot get current directory." << std::endl;
return(0); // Means error, V-REP will unload this plugin
}
std::string currentDirAndPath(curDirAndFile);
// 2. Append the V-REP library's name:
std::string temp(currentDirAndPath);
temp+="/libv_rep.so";
// 3. Load the V-REP library:
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib == NULL)
{
std::cout << "[V-REP_KEPLER] Cannot find or correctly load the V-REP library." << std::endl;
return (0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "[V-REP_KEPLER] Could not find all required functions in the V-REP library." << std::endl;
unloadVrepLibrary(vrepLib);
return (0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Here you could handle various initializations
std::cout << "[V-REP_DXL_ARM] Initializing v-rep plugin..." << std::endl;
int argc = 0;
char** argv = NULL;
ros::init(argc, argv, "v_rep_dxl_arm_plugin");
if (!ros::master::check())
{
ROS_ERROR("[V-REP_DXL_ARM] Cannot detect ROS master!");
return 0;
}
node = new ros::NodeHandle();
pub = node->advertise<std_msgs::Float32MultiArray>("/dxl_arm_control/joints", 1000);
msg.data.resize(DOFs);
ROS_INFO("[V-REP_DXL_ARM] Created node...");
return (PLUGIN_VERSION); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
}
// This is the plugin end routine (called just once, when V-REP is ending, i.e. releasing this plugin):
VREP_DLLEXPORT void v_repEnd()
{
// Here you could handle various clean-up tasks
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_repEnd_callback();
}
unloadVrepLibrary(vrepLib); // release the library
}
VREP_DLLEXPORT void v_repEnd()
{ // This is called just once, at the end of V-REP
for (size_t i=0;i<oglOffscreens.size();i++)
delete oglOffscreens[i];
oglOffscreens.clear();
delete meshContainer;
delete textureContainer;
unloadVrepLibrary(vrepLib); // release the library
}
// This is the plugin end routine (called just once, when V-REP is ending, i.e. releasing this plugin):
VREP_DLLEXPORT void v_repEnd()
{
// Here you could handle various clean-up tasks
/////////////////////////
// PUT OBJECT RESET CODE HERE
// close out as necessary
////////////////////
kukaPluginPG.reset();
unloadVrepLibrary(vrepLib); // release the library
}
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{ // This is called just once, at the start of V-REP.
WIN_AFX_MANAGE_STATE;
// Dynamically load and bind V-REP functions:
char curDirAndFile[1024];
#ifdef _WIN32
GetModuleFileName(NULL,curDirAndFile,1023);
PathRemoveFileSpec(curDirAndFile);
#elif defined (__linux) || defined (__APPLE__)
if (getcwd(curDirAndFile, sizeof(curDirAndFile))==NULL){
// output some error if desired
}
#endif
std::string currentDirAndPath(curDirAndFile);
std::string temp(currentDirAndPath);
#ifdef _WIN32
temp+="\\v_rep.dll";
#elif defined (__linux)
temp+="/libv_rep.so";
#elif defined (__APPLE__)
temp+="/libv_rep.dylib";
#endif /* __linux || __APPLE__ */
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib==NULL)
{
std::cout << "Error, could not find or correctly load v_rep.dll. Cannot start '" << pluginName << "' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "Error, could not find all required functions in v_rep.dll. Cannot start '" << pluginName << "' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// Check the V-REP version:
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<VREP_VERISON_MAJOR*1e4+VREP_VERSION_MINOR*1e2+VREP_VERSION_PATCH)
{
std::cout << "Sorry, your V-REP copy is somewhat old, V-REP" <<
VREP_VERISON_MAJOR << "." << VREP_VERSION_MINOR << "." << VREP_VERSION_PATCH <<
"or higher is required. Cannot start '" << pluginName << "' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
//Initialize ROS
if(!ros::isInitialized()){
int argc = 0;
ros::init(argc,NULL,"vrep");
}
if(!ros::master::check())
{
std::cout << "ROS master is not running. Cannot start '" << pluginName << "' plugin.\n";
return (0);
}
// Do the normal plugin initialization:
simLockInterface(1);
objectContainer = new GenericObjectContainer();
// Register lua collbacks
const int inArgsSimExtGetAllCustomData[]={1,sim_lua_arg_int};
simRegisterCustomLuaFunction("simExtGetAllCustomData",
"string customData=simExtGetAllCustomData(number objectHandle)",
inArgsSimExtGetAllCustomData, GenericObjectContainer::simExtGetAllCustomData);
const int inArgsSimExtGetCustomDataFromHeader[]={2,sim_lua_arg_int,sim_lua_arg_int};
simRegisterCustomLuaFunction("simExtGetCustomDataFromHeader",
"number data=simExtGetCustomDataFromHeader(number objectHandle, number dataHeader)",
inArgsSimExtGetCustomDataFromHeader, GenericObjectContainer::simExtGetCustomDataFromHeader);
const int inArgsSimExtSetFloatCustomDataFromHeader[]={3,sim_lua_arg_int,sim_lua_arg_int,sim_lua_arg_float};
simRegisterCustomLuaFunction("simExtSetFloatCustomDataFromHeader",
"number data=simExtSetFloatCustomDataFromHeader(number objectHandle, number dataHeader, number value)",
inArgsSimExtSetFloatCustomDataFromHeader, GenericObjectContainer::simExtSetFloatCustomDataFromHeader);
const int inArgsSimExtSetIntCustomDataFromHeader[]={3,sim_lua_arg_int,sim_lua_arg_int,sim_lua_arg_int};
simRegisterCustomLuaFunction("simExtSetIntCustomDataFromHeader",
"number data=simExtSetIntCustomDataFromHeader(number objectHandle, number dataHeader, number value)",
inArgsSimExtSetIntCustomDataFromHeader, GenericObjectContainer::simExtSetIntCustomDataFromHeader);
CustomDataHeaders::registerCustomDataHeaders();
simLockInterface(0);
return(PLUGIN_VERSION); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
}
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{ // This is called just once, at the start of V-REP.
// Dynamically load and bind V-REP functions:
char curDirAndFile[1024];
#ifdef _WIN32
#ifdef QT_COMPIL
_getcwd(curDirAndFile, sizeof(curDirAndFile));
#else
GetModuleFileName(NULL,curDirAndFile,1023);
PathRemoveFileSpec(curDirAndFile);
#endif
#elif defined (__linux) || defined (__APPLE__)
getcwd(curDirAndFile, sizeof(curDirAndFile));
#endif
std::string currentDirAndPath(curDirAndFile);
std::string temp(currentDirAndPath);
#ifdef _WIN32
temp+="\\v_rep.dll";
#elif defined (__linux)
temp+="/libv_rep.so";
#elif defined (__APPLE__)
temp+="/libv_rep.dylib";
#endif /* __linux || __APPLE__ */
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib==NULL)
{
std::cout << "Error, could not find or correctly load v_rep.dll. Cannot start 'K3' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "Error, could not find all required functions in v_rep.dll. Cannot start 'K3' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// Check the V-REP version:
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<30200) // if V-REP version is smaller than 3.02.00
{
std::cout << "Sorry, your V-REP copy is somewhat old, V-REP 3.2.0 or higher is required. Cannot start 'K3' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// Register 4 new Lua commands:
std::vector<int> inArgs;
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_CREATE,inArgs);
simRegisterCustomLuaFunction(LUA_CREATE_COMMAND,strConCat("number k3Handle=",LUA_CREATE_COMMAND,"(table_2 wheelMotorHandles,table_2 colorSensorHandles,table_9 IrSensorHandles,table_5 usSensorHandles,table_6 armMotorHandles,table_3 fingerMotorHandles,table_2 gripperDistSensHandles,table_2 gripperColSensHandles,number uiHandle)"),&inArgs[0],LUA_CREATE_CALLBACK);
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_DESTROY,inArgs);
simRegisterCustomLuaFunction(LUA_DESTROY_COMMAND,strConCat("boolean result=",LUA_DESTROY_COMMAND,"(number k3Handle)"),&inArgs[0],LUA_DESTROY_CALLBACK);
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_GETINFRARED,inArgs);
simRegisterCustomLuaFunction(LUA_GETINFRARED_COMMAND,strConCat("number distance=",LUA_GETINFRARED_COMMAND,"(number k3Handle,number index)"),&inArgs[0],LUA_GETINFRARED_CALLBACK);
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_GETULTRASONIC,inArgs);
simRegisterCustomLuaFunction(LUA_GETULTRASONIC_COMMAND,strConCat("number distance=",LUA_GETULTRASONIC_COMMAND,"(number k3Handle,number index)"),&inArgs[0],LUA_GETULTRASONIC_CALLBACK);
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_GETLINESENSOR,inArgs);
simRegisterCustomLuaFunction(LUA_GETLINESENSOR_COMMAND,strConCat("number intensity=",LUA_GETLINESENSOR_COMMAND,"(number k3Handle,number index)"),&inArgs[0],LUA_GETLINESENSOR_CALLBACK);
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_GETENCODER,inArgs);
simRegisterCustomLuaFunction(LUA_GETENCODER_COMMAND,strConCat("number encoderValue=",LUA_GETENCODER_COMMAND,"(number k3Handle,number index)"),&inArgs[0],LUA_GETENCODER_CALLBACK);
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_GETGRIPPERPROXSENSOR,inArgs);
simRegisterCustomLuaFunction(LUA_GETGRIPPERPROXSENSOR_COMMAND,strConCat("number distance=",LUA_GETGRIPPERPROXSENSOR_COMMAND,"(number k3Handle,number index)"),&inArgs[0],LUA_GETGRIPPERPROXSENSOR_CALLBACK);
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_SETARMPOSITION,inArgs);
simRegisterCustomLuaFunction(LUA_SETARMPOSITION_COMMAND,strConCat("boolean result=",LUA_SETARMPOSITION_COMMAND,"(number k3Handle,number position)"),&inArgs[0],LUA_SETARMPOSITION_CALLBACK);
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_SETGRIPPERGAP,inArgs);
simRegisterCustomLuaFunction(LUA_SETGRIPPERGAP_COMMAND,strConCat("boolean result=",LUA_SETGRIPPERGAP_COMMAND,"(number k3Handle,number gap)"),&inArgs[0],LUA_SETGRIPPERGAP_CALLBACK);
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_SETVELOCITY,inArgs);
simRegisterCustomLuaFunction(LUA_SETVELOCITY_COMMAND,strConCat("boolean result=",LUA_SETVELOCITY_COMMAND,"(number k3Handle,number leftVelocity,number rightVelocity)"),&inArgs[0],LUA_SETVELOCITY_CALLBACK);
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_SETENCODERS,inArgs);
simRegisterCustomLuaFunction(LUA_SETENCODERS_COMMAND,strConCat("boolean result=",LUA_SETENCODERS_COMMAND,"(number k3Handle,number leftEncoderValue,number rightEncoderValue)"),&inArgs[0],LUA_SETENCODERS_CALLBACK);
return(6); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
// version 1 was for V-REP 2.5.11 and below
// version 2 was for V-REP 2.5.12 and below
// version 3 was for V-REP 3.0.5 and below
// version 4 was for V-REP 3.1.3 and below
// version 6 is for V-REP versions after V-REP 3.2.0 (completely rewritten)
}
// This is the plugin end routine:
VREP_DLLEXPORT void v_repEnd()
{ // This is called just once, at the end of V-REP
allConnections.removeAllConnections();
unloadVrepLibrary(vrepLib); // release the library
}
// This is the plugin start routine:
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{ // This is called just once, at the start of V-REP
// Dynamically load and bind V-REP functions:
char curDirAndFile[1024];
#ifdef _WIN32
#ifdef QT_COMPIL
_getcwd(curDirAndFile, sizeof(curDirAndFile));
#else
GetModuleFileName(NULL,curDirAndFile,1023);
PathRemoveFileSpec(curDirAndFile);
#endif
#elif defined (__linux) || defined (__APPLE__)
getcwd(curDirAndFile, sizeof(curDirAndFile));
#endif
std::string currentDirAndPath(curDirAndFile);
std::string temp(currentDirAndPath);
#ifdef _WIN32
temp+="\\v_rep.dll";
#elif defined (__linux)
temp+="/libv_rep.so";
#elif defined (__APPLE__)
temp+="/libv_rep.dylib";
#endif /* __linux || __APPLE__ */
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib==NULL)
{
std::cout << "Error, could not find or correctly load the V-REP library. Cannot start 'RemoteApi' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "Error, could not find all required functions in the V-REP library. Cannot start 'RemoteApi' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<20604) // if V-REP version is smaller than 2.06.04
{
std::cout << "Sorry, your V-REP copy is somewhat old. Cannot start 'RemoteApi' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// Register 3 new Lua commands:
int inArgs1[]={1,sim_lua_arg_int};
simRegisterCustomLuaFunction(LUA_STATUS_COMMAND,strConCat("number status,table_5 info=",LUA_STATUS_COMMAND,"(number socketPort)"),inArgs1,LUA_STATUS_COMMAND_CALLBACK);
int inArgs2[]={4,sim_lua_arg_int,sim_lua_arg_int,sim_lua_arg_bool,sim_lua_arg_bool}; // 3/3/2014
simRegisterCustomLuaFunction(LUA_START_COMMAND,strConCat("number result=",LUA_START_COMMAND,"(number socketPort,number maxPacketSize=1300,boolean debug=false,boolean preEnableTrigger=false)"),inArgs2,LUA_START_COMMAND_CALLBACK); // 3/3/2014
simRegisterCustomLuaFunction(LUA_STOP_COMMAND,strConCat("number result=",LUA_STOP_COMMAND,"(number socketPort)"),inArgs1,LUA_STOP_COMMAND_CALLBACK);
simRegisterCustomLuaFunction(LUA_RESET_COMMAND,strConCat("number result=",LUA_RESET_COMMAND,"(number socketPort)"),inArgs1,LUA_RESET_COMMAND_CALLBACK);
// Read the configuration file to prepare socket connections:
CConfReader conf;
temp=currentDirAndPath;
#ifdef _WIN32
temp+="\\remoteApiConnections.txt";
#endif /* _WIN32 */
#if defined (__linux) || defined (__APPLE__)
temp+="/remoteApiConnections.txt";
#endif /* __linux || __APPLE__ */
conf.readConfiguration(temp.c_str());
conf.getBoolean("useAlternateSocketRoutines",CSimxSocket::useAlternateSocketRoutines);
int index=1;
while (true)
{
std::stringstream strStream;
strStream << index;
std::string variableNameBase("portIndex");
variableNameBase+=strStream.str();
std::string variableName=variableNameBase+"_port";
int portNb;
if (conf.getInteger(variableName.c_str(),portNb))
{
bool debug=false;
int maxPacketSize=1300;
if (portNb<0)
maxPacketSize=3200000;
bool synchronousTrigger=false;
variableName=variableNameBase+"_maxPacketSize";
conf.getInteger(variableName.c_str(),maxPacketSize);
variableName=variableNameBase+"_debug";
conf.getBoolean(variableName.c_str(),debug);
variableName=variableNameBase+"_syncSimTrigger";
conf.getBoolean(variableName.c_str(),synchronousTrigger);
if (portNb<0)
{ // when using shared memory
if (maxPacketSize<1000)
maxPacketSize=1000;
if (maxPacketSize>32000000)
maxPacketSize=32000000;
}
else
{ // when using sockets
if (maxPacketSize<200)
maxPacketSize=200;
if (maxPacketSize>30000)
maxPacketSize=30000;
}
if (allConnections.getConnectionFromPort(portNb)==NULL)
{
// 3/3/2014 synchronousTrigger&=(allConnections.getSynchronousSimulationTriggerConnection()==NULL);
CSimxSocket* oneSocketConnection=new CSimxSocket(portNb,false,debug,maxPacketSize,synchronousTrigger);
oneSocketConnection->start();
allConnections.addSocketConnection(oneSocketConnection);
// 3/3/2014 if (synchronousTrigger)
// 3/3/2014 allConnections.setSynchronousSimulationTriggerConnection(oneSocketConnection); // only one connection can act as trigger!
std::cout << "Starting a remote API server on port " << portNb << std::endl;
}
else
std::cout << "Failed starting a remote API server on port " << portNb << std::endl;
index++;
}
else
break;
}
return(SIMX_VERSION); // initialization went fine, we return the version number of this extension module (can be queried with simGetModuleName)
}
// This is the plugin start routine (called just once, just after the plugin was loaded):
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{
// Dynamically load and bind V-REP functions:
// ******************************************
// 1. Figure out this plugin's directory:
char curDirAndFile[1024];
#ifdef _WIN32
_getcwd(curDirAndFile, sizeof(curDirAndFile));
#elif defined (__linux) || defined (__APPLE__)
getcwd(curDirAndFile, sizeof(curDirAndFile));
#endif
std::string currentDirAndPath(curDirAndFile);
// 2. Append the V-REP library's name:
std::string temp(currentDirAndPath);
#ifdef _WIN32
temp+="/v_rep.dll";
#elif defined (__linux)
temp+="/libv_rep.so";
#elif defined (__APPLE__)
temp+="/libv_rep.dylib";
#endif /* __linux || __APPLE__ */
// 3. Load the V-REP library:
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib==NULL)
{
std::cout << "Error, could not find or correctly load the V-REP library. Cannot start 'Collada' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "Error, could not find all required functions in the V-REP library. Cannot start 'Collada' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Check the version of V-REP:
// ******************************************
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<20604) // if V-REP version is smaller than 2.06.04
{
std::cout << "Sorry, your V-REP copy is somewhat old. Cannot start 'Collada' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Check if V-REP runs in headless mode:
// ******************************************
if (simGetBooleanParameter(sim_boolparam_headless)>0)
{
std::cout << "V-REP runs in headless mode. Cannot start 'Collada' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
QWidget* pMainWindow = (QWidget*)simGetMainWindow(1);
colladaDialog=new CColladaDialog(pMainWindow); // The plugin dialog
simAddModuleMenuEntry("",1,&colladaDialog->dialogMenuItemHandle);
simSetModuleMenuItemState(colladaDialog->dialogMenuItemHandle,1,"COLLADA import/export...");
return(PLUGIN_VERSION); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
}
// This is the plugin end routine (called just once, when V-REP is ending, i.e. releasing this plugin):
VREP_DLLEXPORT void v_repEnd()
{
// Here you could handle various clean-up tasks
delete colladaDialog;
unloadVrepLibrary(vrepLib); // release the library
}
VREP_DLLEXPORT void v_repEnd()
{ // This is called just once, at the end of V-REP
unloadVrepLibrary(vrepLib); // release the library
}
// This is the plugin start routine (called just once, just after the plugin was loaded):
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{
// Dynamically load and bind V-REP functions:
// ******************************************
// 1. Figure out this plugin's directory:
char curDirAndFile[1024];
#ifdef _WIN32
GetModuleFileName(NULL,curDirAndFile,1023);
PathRemoveFileSpec(curDirAndFile);
#elif defined (__linux) || defined (__APPLE__)
getcwd(curDirAndFile, sizeof(curDirAndFile));
#endif
std::string currentDirAndPath(curDirAndFile);
// 2. Append the V-REP library's name:
std::string temp(currentDirAndPath);
#ifdef _WIN32
temp+="\\v_rep.dll";
#elif defined (__linux)
temp+="/libv_rep.so";
#elif defined (__APPLE__)
temp+="/libv_rep.dylib";
#endif /* __linux || __APPLE__ */
// 3. Load the V-REP library:
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib==NULL)
{
std::cout << "Error, could not find or correctly load the V-REP library. Cannot start 'youBot ROS' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "Error, could not find all required functions in the V-REP library. Cannot start 'youBot ROS' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Check the version of V-REP:
// ******************************************
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<20604) // if V-REP version is smaller than 2.06.04
{
std::cout << "Sorry, your V-REP copy is somewhat old. Cannot start 'PluginSkeleton' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
simLockInterface(1);
// Here you could handle various initializations
// Here you could also register custom Lua functions or custom Lua constants
// etc.
int argc = 0;
char** argv = {};
ros::init(argc, argv,"V_RepyouBot");
if(!ros::master::check()) {
std::cout << "No ROS master available - youBot Plugin not initialized" << std::endl;
return(false);
}
for (unsigned int i=0; i < registry.getCallbacks().size(); i++) {
GenericLuaCallback* cb = registry.getCallbacks()[i];
GenericLuaCallback::LuaDescription desc = cb->getDescription();
GenericLuaCallback::callBackPtr pointer = cb->getPointer();
if (pointer != 0) {
simRegisterCustomLuaFunction(desc.name.c_str(),
desc.tooltip.c_str(),
desc.argTypes,
pointer);
std::cout << "LUA function registered: " << desc.name.c_str() << " " << std::endl;
} else {
std::cout << "Callback function 0 for Callback " << desc.name << std::endl;
}
}
std::cout << "youBot Plugin registered"<< std::endl;
simLockInterface(0);
return(PLUGIN_VERSION); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
}
// This is the plugin start routine (called just once, just after the plugin was loaded):
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{
// Dynamically load and bind V-REP functions:
// ******************************************
// 1. Figure out this plugin's directory:
char curDirAndFile[1024];
#ifdef _WIN32
_getcwd(curDirAndFile, sizeof(curDirAndFile));
#elif defined (__linux) || defined (__APPLE__)
getcwd(curDirAndFile, sizeof(curDirAndFile));
#endif
std::string currentDirAndPath(curDirAndFile);
// 2. Append the V-REP library's name:
std::string temp(currentDirAndPath);
#ifdef _WIN32
temp+="/v_rep.dll";
#elif defined (__linux)
temp+="/libv_rep.so";
#elif defined (__APPLE__)
temp+="/libv_rep.dylib";
#endif /* __linux || __APPLE__ */
// 3. Load the V-REP library:
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib==NULL)
{
std::cout << "Error, could not find or correctly load the V-REP library. Cannot start 'Urdf' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "Error, could not find all required functions in the V-REP library. Cannot start 'Urdf' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Check the version of V-REP:
// ******************************************
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<30001) // if V-REP version is smaller than 3.00.01
{
std::cout << "Sorry, your V-REP copy is somewhat old. Cannot start 'Urdf' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Check if V-REP runs in headless mode:
// ******************************************
if (simGetBooleanParameter(sim_boolparam_headless)>0)
{
std::cout << "V-REP runs in headless mode. Cannot start 'Urdf' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
QWidget* pMainWindow = (QWidget*)simGetMainWindow(1);
urdfDialog=new CUrdfDialog(pMainWindow); // The plugin dialog
simAddModuleMenuEntry("",1,&urdfDialog->dialogMenuItemHandle);
simSetModuleMenuItemState(urdfDialog->dialogMenuItemHandle,1,"URDF import...");
int inputArgsTypes[] = {
10,
sim_lua_arg_string, // URDF contents
sim_lua_arg_bool, // assign collision links to layer 9
sim_lua_arg_bool, // assign joints to layer 10
sim_lua_arg_bool, // convex decompose
sim_lua_arg_bool, // create visual links if none
sim_lua_arg_bool, // show convex decomposition dialog
sim_lua_arg_bool, // center model above ground
sim_lua_arg_bool, // prepare model definition if feasible
sim_lua_arg_bool, // alternate local respondable masks
sim_lua_arg_bool // enable position control for revolute and prismatic joints
};
simRegisterCustomLuaFunction("simExtImportUrdf",
"string robot_name=simExtImportUrdf("
"string urdf,"
"bool hideCollisionLinks,"
"bool hideJoints,"
"bool convexDecomposeNonConvexCollidables,"
"bool createVisualIfNone,"
"bool showConvexDecompositionDlg,"
"bool centerAboveGround,"
"bool makeModel,"
"bool noSelfCollision,"
"bool positionCtrl"
")",
inputArgsTypes, v_repImportUrdfCallback);
int inputArgsTypesFile[] = {
10,
sim_lua_arg_string, // filenameAndPath
sim_lua_arg_bool, // assign collision links to layer 9
sim_lua_arg_bool, // assign joints to layer 10
sim_lua_arg_bool, // convex decompose
sim_lua_arg_bool, // create visual links if none
sim_lua_arg_bool, // show convex decomposition dialog
sim_lua_arg_bool, // center model above ground
sim_lua_arg_bool, // prepare model definition if feasible
sim_lua_arg_bool, // alternate local respondable masks
sim_lua_arg_bool // enable position control for revolute and prismatic joints
};
simRegisterCustomLuaFunction("simExtImportUrdfFile",
"string robot_name=simExtImportUrdf("
"string fileAndPath,"
"bool hideCollisionLinks,"
"bool hideJoints,"
"bool convexDecomposeNonConvexCollidables,"
"bool createVisualIfNone,"
"bool showConvexDecompositionDlg,"
"bool centerAboveGround,"
"bool makeModel,"
"bool noSelfCollision,"
"bool positionCtrl"
")",
inputArgsTypesFile, v_repImportUrdfCallback);
return(PLUGIN_VERSION); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
}
// This is the plugin start routine (called just once, just after the plugin was loaded):
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{
// Dynamically load and bind V-REP functions:
// ******************************************
// 1. Figure out this plugin's directory:
char curDirAndFile[1024];
getcwd(curDirAndFile, sizeof(curDirAndFile));
std::string currentDirAndPath(curDirAndFile);
// 2. Append the V-REP library's name:
std::string temp(currentDirAndPath);
#ifdef _WIN32
temp+="\\v_rep.dll";
#elif defined (__linux)
temp+="/libv_rep.so";
#elif defined (__APPLE__)
temp+="/libv_rep.dylib";
#endif
// 3. Load the V-REP library:
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib==NULL)
{
std::cout << "Error, could not find or correctly load the V-REP library. Cannot start 'ROS' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "Error, could not find all required functions in the V-REP library. Cannot start 'ROS' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Check the version of V-REP:
// ******************************************
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<20605) // if V-REP version is smaller than 2.06.04
{
std::cout << "Sorry, your V-REP copy is somewhat old. Cannot start 'ROS' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Initialize the ROS part:
if(!ROS_server::initialize())
{
std::cout << "ROS master is not running. Cannot start 'ROS' plugin.\n";
return (0); //If the master is not running then the plugin is not loaded.
}
// Register the new Lua commands:
simRegisterScriptCallbackFunction(strConCat(LUA_ENABLEPUBLISHER_COMMAND,"@","Ros"),strConCat("string topicName=",LUA_ENABLEPUBLISHER_COMMAND,"(string topicName,number queueSize,number rosStreamCmd,number auxInt1,number auxInt2,string auxString,number publishCnt=0)"),LUA_ENABLEPUBLISHER_CALLBACK);
simRegisterScriptCallbackFunction(strConCat(LUA_DISABLEPUBLISHER_COMMAND,"@","Ros"),strConCat("number referenceCounter=",LUA_DISABLEPUBLISHER_COMMAND,"(string topicName)"),LUA_DISABLEPUBLISHER_CALLBACK);
simRegisterScriptCallbackFunction(strConCat(LUA_WAKEPUBLISHER_COMMAND,"@","Ros"),strConCat("number result=",LUA_WAKEPUBLISHER_COMMAND,"(string topicName,number publishCnt)"),LUA_WAKEPUBLISHER_CALLBACK);
simRegisterScriptCallbackFunction(strConCat(LUA_ENABLESUBSCRIBER_COMMAND,"@","Ros"),strConCat("number subscriberId=",LUA_ENABLESUBSCRIBER_COMMAND,"(string topicName,number queueSize,number rosStreamCmd,number auxInt1,number auxInt2,string auxString,number callbackTag_before=-1,number callbackTag_after=-1)"),LUA_ENABLESUBSCRIBER_CALLBACK);
simRegisterScriptCallbackFunction(strConCat(LUA_DISABLESUBSCRIBER_COMMAND,"@","Ros"),strConCat("boolean result=",LUA_DISABLESUBSCRIBER_COMMAND,"(number subscriberID)"),LUA_DISABLESUBSCRIBER_CALLBACK);
// Publisher constants:
simRegisterScriptVariable("simros_strmcmd_get_laser_scanner_data",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_laser_scanner_data))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_odom_data",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_odom_data))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_object_selection",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_object_selection))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_array_parameter",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_array_parameter))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_boolean_parameter",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_boolean_parameter))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_dialog_result",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_dialog_result))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_floating_parameter",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_floating_parameter))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_integer_parameter",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_integer_parameter))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_joint_state",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_joint_state))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_object_pose",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_object_pose))).c_str(),0);
// simRegisterScriptVariable("simros_strmcmd_get_object_quaternion",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_object_quaternion))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_object_parent",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_object_parent))).c_str(),0);
// simRegisterScriptVariable("simros_strmcmd_get_object_position",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_object_position))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_objects",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_objects))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_string_parameter",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_string_parameter))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_ui_event_button",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_ui_event_button))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_vision_sensor_depth_buffer",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_vision_sensor_depth_buffer))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_vision_sensor_image",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_vision_sensor_image))).c_str(),0);
// simRegisterScriptVariable("simros_strmcmd_get_joint_force",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_joint_force))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_read_collision",(boost::lexical_cast<std::string>(int(simros_strmcmd_read_collision))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_read_distance",(boost::lexical_cast<std::string>(int(simros_strmcmd_read_distance))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_read_force_sensor",(boost::lexical_cast<std::string>(int(simros_strmcmd_read_force_sensor))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_read_proximity_sensor",(boost::lexical_cast<std::string>(int(simros_strmcmd_read_proximity_sensor))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_read_vision_sensor",(boost::lexical_cast<std::string>(int(simros_strmcmd_read_vision_sensor))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_object_float_parameter",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_object_float_parameter))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_object_int_parameter",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_object_int_parameter))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_ui_button_property",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_ui_button_property))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_ui_slider",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_ui_slider))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_float_signal",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_float_signal))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_integer_signal",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_integer_signal))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_string_signal",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_string_signal))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_and_clear_string_signal",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_and_clear_string_signal))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_transform",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_transform))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_range_finder_data",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_range_finder_data))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_depth_sensor_data",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_depth_sensor_data))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_vision_sensor_info",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_vision_sensor_info))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_twist_status",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_twist_status))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_object_group_data",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_object_group_data))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_receive_data_from_script_function",(boost::lexical_cast<std::string>(int(simros_strmcmd_receive_data_from_script_function))).c_str(),0);
// Subscriber constants:
simRegisterScriptVariable("simros_strmcmd_add_status_bar_message",(boost::lexical_cast<std::string>(int(simros_strmcmd_add_status_bar_message))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_object_selection",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_object_selection))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_auxiliary_console_print",(boost::lexical_cast<std::string>(int(simros_strmcmd_auxiliary_console_print))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_array_parameter",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_array_parameter))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_boolean_parameter",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_boolean_parameter))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_floating_parameter",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_floating_parameter))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_integer_parameter",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_integer_parameter))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_joint_force",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_joint_force))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_joint_position",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_joint_position))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_joint_target_position",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_joint_target_position))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_joint_target_velocity",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_joint_target_velocity))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_vision_sensor_image",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_vision_sensor_image))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_object_float_parameter",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_object_float_parameter))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_object_int_parameter",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_object_int_parameter))).c_str(),0);
// simRegisterScriptVariable("simros_strmcmd_set_object_orientation",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_object_orientation))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_object_position",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_object_position))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_object_pose",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_object_pose))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_joint_state",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_joint_state))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_object_quaternion",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_object_quaternion))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_ui_button_label",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_ui_button_label))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_ui_button_property",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_ui_button_property))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_ui_slider",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_ui_slider))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_clear_float_signal",(boost::lexical_cast<std::string>(int(simros_strmcmd_clear_float_signal))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_clear_integer_signal",(boost::lexical_cast<std::string>(int(simros_strmcmd_clear_integer_signal))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_clear_string_signal",(boost::lexical_cast<std::string>(int(simros_strmcmd_clear_string_signal))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_float_signal",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_float_signal))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_integer_signal",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_integer_signal))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_string_signal",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_string_signal))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_append_string_signal",(boost::lexical_cast<std::string>(int(simros_strmcmd_append_string_signal))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_twist_command",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_twist_command))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_set_joy_sensor",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_joy_sensor))).c_str(),0);
// simRegisterScriptVariable("simros_strmcmd_set_joint_trajectory",(boost::lexical_cast<std::string>(int(simros_strmcmd_set_joint_trajectory))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_send_data_to_script_function",(boost::lexical_cast<std::string>(int(simros_strmcmd_send_data_to_script_function))).c_str(),0);
return(PLUGIN_VERSION); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
}
// This is the plugin start routine (called just once, just after the plugin was loaded):
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{
// Dynamically load and bind V-REP functions:
// ******************************************
// 1. Figure out this plugin's directory:
char curDirAndFile[1024];
#ifdef _WIN32
#ifdef QT_COMPIL
_getcwd(curDirAndFile, sizeof(curDirAndFile));
#else
GetModuleFileName(NULL,curDirAndFile,1023);
PathRemoveFileSpec(curDirAndFile);
#endif
#elif defined (__linux) || defined (__APPLE__)
getcwd(curDirAndFile, sizeof(curDirAndFile));
#endif
std::string currentDirAndPath(curDirAndFile);
// 2. Append the V-REP library's name:
std::string temp(currentDirAndPath);
#ifdef _WIN32
temp+="\\v_rep.dll";
#elif defined (__linux)
temp+="/libv_rep.so";
#elif defined (__APPLE__)
temp+="/libv_rep.dylib";
#endif /* __linux || __APPLE__ */
// 3. Load the V-REP library:
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib==NULL)
{
std::cout << "Error, could not find or correctly load the V-REP library. Cannot start 'PluginSkeleton' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "Error, could not find all required functions in the V-REP library. Cannot start 'PluginSkeleton' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Check the version of V-REP:
// ******************************************
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<30200) // if V-REP version is smaller than 3.02.00
{
std::cout << "Sorry, your V-REP copy is somewhat old. Cannot start 'PluginSkeleton' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
std::vector<int> inArgs;
// Register the new Lua command "simExtSkeleton_getSensorData":
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_GETSENSORDATA,inArgs);
simRegisterCustomLuaFunction(LUA_GETSENSORDATA_COMMAND,strConCat("number result,table data,number distance=",LUA_GETSENSORDATA_COMMAND,"(number sensorIndex,table_3 floatParameters,table_2 intParameters)"),&inArgs[0],LUA_GETSENSORDATA_CALLBACK);
return(PLUGIN_VERSION); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
}
int main(int argc, char* argv[])
{
if (TIMER_RESOLUTION_IN_MS>0)
{
TIMECAPS tc;
UINT wTimerRes;
if (timeGetDevCaps(&tc,sizeof(TIMECAPS))==TIMERR_NOERROR)
{
wTimerRes=MIN_VAL(MAX_VAL(tc.wPeriodMin,TIMER_RESOLUTION_IN_MS),tc.wPeriodMax);
timeBeginPeriod(wTimerRes);
}
}
char curDirAndFile[1024];
GetModuleFileName(NULL,curDirAndFile,1023);
PathRemoveFileSpec(curDirAndFile);
std::string currentDirAndPath(curDirAndFile);
std::string temp(currentDirAndPath);
temp+="\\v_rep.dll";
LIBRARY lib=loadVrepLibrary(temp.c_str());
bool wasRunning=false;
if (lib!=NULL)
{
if (getVrepProcAddresses(lib)!=0)
{
int guiItems=sim_gui_all;
for (int i=1;i<argc;i++)
{
std::string arg(argv[i]);
if (arg[0]=='-')
{
if (arg.length()>=2)
{
if (arg[1]=='h')
guiItems=sim_gui_headless;
if (arg[1]=='s')
{
autoStart=true;
simStopDelay=0;
unsigned int p=2;
while (arg.length()>p)
{
simStopDelay*=10;
if ((arg[p]>='0')&&(arg[p]<='9'))
simStopDelay+=arg[p]-'0';
else
{
simStopDelay=0;
break;
}
p++;
}
}
if (arg[1]=='q')
autoQuit=true;
if ((arg[1]=='a')&&(arg.length()>2))
{
std::string tmp;
tmp.assign(arg.begin()+2,arg.end());
simSetStringParameter(sim_stringparam_additional_addonscript_firstscene,tmp.c_str()); // normally, never call API functions before simRunSimulator!!
}
if ((arg[1]=='b')&&(arg.length()>2))
{
std::string tmp;
tmp.assign(arg.begin()+2,arg.end());
simSetStringParameter(sim_stringparam_additional_addonscript,tmp.c_str()); // normally, never call API functions before simRunSimulator!!
}
if ((arg[1]=='g')&&(arg.length()>2))
{
static int cnt=0;
std::string tmp;
tmp.assign(arg.begin()+2,arg.end());
if (cnt<9)
simSetStringParameter(sim_stringparam_app_arg1+cnt,tmp.c_str()); // normally, never call API functions before simRunSimulator!!
cnt++;
}
}
}
else
{
if (sceneOrModelOrUiToLoad.length()==0)
sceneOrModelOrUiToLoad=arg;
}
}
if (simRunSimulator("V-REP",guiItems,simulatorInit,simulatorLoop,simulatorDeinit)!=1)
printf("Failed initializing and running V-REP\n");
else
wasRunning=true;
}
else
printf("Error: could not find all required functions in v_rep.dll\n");
unloadVrepLibrary(lib);
}
else
printf("Error: could not find or correctly load v_rep.dll\n");
if (!wasRunning)
getchar();
return(0);
}
// This is the plugin start routine (called just once, just after the plugin was loaded):
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{
// Dynamically load and bind V-REP functions:
// ******************************************
// 1. Figure out this plugin's directory:
char curDirAndFile[1024];
#ifdef _WIN32
GetModuleFileName(NULL,curDirAndFile,1023);
PathRemoveFileSpec(curDirAndFile);
#elif defined (__linux) || defined (__APPLE__)
getcwd(curDirAndFile, sizeof(curDirAndFile));
#endif
std::string currentDirAndPath(curDirAndFile);
// 2. Append the V-REP library's name:
std::string temp(currentDirAndPath);
#ifdef _WIN32
temp+="\\v_rep.dll";
#elif defined (__linux)
temp+="/libv_rep.so";
#elif defined (__APPLE__)
temp+="/libv_rep.dylib";
#endif /* __linux || __APPLE__ */
// 3. Load the V-REP library:
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib==NULL)
{
std::cout << "Error, could not find or correctly load the V-REP library. Cannot start 'RTCSkeleton' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "Error, could not find all required functions in the V-REP library. Cannot start 'RTCSkeleton' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Check the version of V-REP:
// ******************************************
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<20604) // if V-REP version is smaller than 2.06.04
{
std::cout << "Sorry, your V-REP copy is somewhat old. Cannot start 'RTCSkeleton' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Here you could handle various initializations
// Here you could also register custom Lua functions or custom Lua constants
// etc.
const simChar* entryLabel = "";
simInt itemCount = 1;
// simAddModuleMenuEntry(entryLabel, itemCount, &mainItemHandle);
// simSetModuleMenuItemState(mainItemHandle, 1, "RTCPlugin");
entryLabel = "RTCPlugin";
itemCount = 1;
simAddModuleMenuEntry(entryLabel, itemCount, &robotItemHandle);
simSetModuleMenuItemState(robotItemHandle, 1, "Add Robot RTC");
simAddModuleMenuEntry(entryLabel, itemCount, &rangeItemHandle);
simSetModuleMenuItemState(rangeItemHandle, 1, "Add Range RTC");
simAddModuleMenuEntry(entryLabel, itemCount, &cameraItemHandle);
simSetModuleMenuItemState(cameraItemHandle, 1, "Add Camera RTC");
simAddModuleMenuEntry(entryLabel, itemCount, &accelItemHandle);
simSetModuleMenuItemState(accelItemHandle, 1, "Add Acceleration RTC");
simAddModuleMenuEntry(entryLabel, itemCount, &gyroItemHandle);
simSetModuleMenuItemState(gyroItemHandle, 1, "Add Gyro RTC");
simAddModuleMenuEntry(entryLabel, itemCount, &depthItemHandle);
simSetModuleMenuItemState(depthItemHandle, 1, "Add Depth RTC");
simAddModuleMenuEntry(entryLabel, itemCount, &objectItemHandle);
simSetModuleMenuItemState(objectItemHandle, 1, "Add Object RTC");
initRTM();
simulatorClock.setSimulationTimeStep(simGetSimulationTimeStep());
std::cout << "v_repExtRTC plugin safely loaded.\n";
return(PLUGIN_VERSION); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
}
// This is the plugin start routine (called just once, just after the plugin was loaded):
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{
// Dynamically load and bind V-REP functions:
// ******************************************
// 1. Figure out this plugin's directory:
char curDirAndFile[1024];
char * filenptr = getcwd(curDirAndFile, sizeof(curDirAndFile));
if (filenptr == NULL)
{
std::cout << "[V-REP_ROS_POOL_SIM] Cannot get current directory. Failed to start 'ROS' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
std::string currentDirAndPath(curDirAndFile);
// 2. Append the V-REP library's name:
std::string temp(currentDirAndPath);
temp+="/libv_rep.so";
// 3. Load the V-REP library:
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib == NULL)
{
std::cout << "[V-REP_ROS_POOL_SIM]ind or correctly load the V-REP library. Cannot start 'ROS' plugin.\n";
return (0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "[V-REP_ROS_POOL_SIM] Error, could not find all required functions in the V-REP library. Cannot start 'ROS' plugin.\n";
unloadVrepLibrary(vrepLib);
return (0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Check the version of V-REP:
// ******************************************
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<20604) // if V-REP version is smaller than 2.06.04
{
std::cout << "[V-REP_ROS_POOL_SIM] Sorry, your V-REP copy is somewhat old. Cannot start 'ROS' plugin.\n";
unloadVrepLibrary(vrepLib);
return (0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Here you could handle various initializations
// Here you could also register custom Lua functions or custom Lua constants
//
// e.g. you could register following functions here:
// nodeHandle=simExtRosStart(...)
// simExtRosFinish(nodeHandle)
// simExtRosEnableService(nodeHandle,...)
// etc.
std::cout << "[V-REP_ROS_POOL_SIM] Initializing v-rep plugin...\n";
int argc = 0;
char** argv = NULL;
ros::init(argc,argv,"v_rep_pool_sim");
if (!ros::master::check())
{
ROS_ERROR("[V-REP_ROS_POOL_SIM] Cannot detect ROS master!");
return 0;
}
ROS_INFO("[V-REP_ROS_POOL_SIM] Creating node...");
node = new ros::NodeHandle();
img_transport = new image_transport::ImageTransport(*node);
img_pub = img_transport->advertise("camera_image", 2);
table_state_pub = node->advertise<vrep_plugin::Pool_table_state>("pool_table_state", 1);
score_pub = node->advertise<std_msgs::Int32>("turn_score", 1);
ROS_INFO("[V-REP_ROS_POOL_SIM] Subscribing...");
turn_sub = node->subscribe("player_turn", 1, new_player_turn_callback);
//pool_table = node->advertiseService("player_turn", evaluate_player_turn_callback);
//sim_change_sub = node->subscribe("sim_change_status", 2, sim_change_callback);
ROS_INFO("[V-REP_ROS_POOL_SIM] Initialized!");
return (PLUGIN_VERSION); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
}
// This is the plugin end routine (called just once, when V-REP is ending, i.e. releasing this plugin):
VREP_DLLEXPORT void v_repEnd()
{
ROS_server::shutDown(); // shutdown the ROS_server
unloadVrepLibrary(vrepLib); // release the library
}
// This is the plugin start routine (called just once, just after the plugin was loaded):
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{
// Dynamically load and bind V-REP functions:
// ******************************************
// 1. Figure out this plugin's directory:
char curDirAndFile[1024];
#ifdef _WIN32
GetModuleFileName(NULL,curDirAndFile,1023);
PathRemoveFileSpec(curDirAndFile);
#elif defined (__linux) || defined (__APPLE__)
getcwd(curDirAndFile, sizeof(curDirAndFile));
#endif
std::string currentDirAndPath(curDirAndFile);
// 2. Append the V-REP library's name:
std::string temp(currentDirAndPath);
#ifdef _WIN32
temp+="\\v_rep.dll";
#elif defined (__linux)
temp+="/libv_rep.so";
#elif defined (__APPLE__)
temp+="/libv_rep.dylib";
#endif /* __linux || __APPLE__ */
// 3. Load the V-REP library:
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib==NULL)
{
BOOST_LOG_TRIVIAL(error) << "Error, could not find or correctly load the V-REP library. Cannot start 'v_repExtKukaLBRiiwa' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
BOOST_LOG_TRIVIAL(error) << "Error, could not find all required functions in the V-REP library. Cannot start 'v_repExtKukaLBRiiwa' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Check the version of V-REP:
// ******************************************
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<20604) // if V-REP version is smaller than 2.06.04
{
BOOST_LOG_TRIVIAL(error) << "Sorry, your V-REP copy is somewhat old. Cannot start 'v_repExtKukaLBRiiwa' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
// ******************************************
// Register the new Lua command "simExtSkeleton_getSensorData":
// ******************************************
std::vector<int> inArgs;
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_KUKA_LBR_IIWA_START,inArgs);
simRegisterCustomLuaFunction
(
LUA_KUKA_LBR_IIWA_START_COMMAND,
LUA_KUKA_LBR_IIWA_START_CALL_TIP.c_str(),
&inArgs[0],
LUA_SIM_EXT_KUKA_LBR_IIWA_START
);
// Expected input arguments are: int sensorIndex, float floatParameters[3], int intParameters[2]
//int inArgs_getSensorData[]={3,sim_lua_arg_int,sim_lua_arg_float|sim_lua_arg_table,sim_lua_arg_int|sim_lua_arg_table}; // this says we expect 3 arguments (1 integer, a table of floats, and a table of ints)
// Return value can change on the fly, so no need to specify them here, except for the calltip.
// Now register the callback:
//simRegisterCustomLuaFunction(LUA_GET_SENSOR_DATA_COMMAND,strConCat("number result,table data,number distance=",LUA_GET_SENSOR_DATA_COMMAND,"(number sensorIndex,table_3 floatParams,table_2 intParams)"),inArgs_getSensorData,LUA_GET_SENSOR_DATA_CALLBACK);
// ******************************************
BOOST_LOG_TRIVIAL(info) << "KUKA LBR iiwa plugin initialized. Build date/time: " << __DATE__ << " " << __TIME__ <<"\n";
return(PLUGIN_VERSION); // initialization went fine, we return the version number of this plugin (can be queried with simGetModuleName)
}