Exemplo n.º 1
0
bool CSimxSocket::getWaitForTrigger()
{
	// make sure we override the external trigger if the user presses stop:
	int ver;
	simGetIntegerParameter(sim_intparam_program_version,&ver);
	int req;
	simGetIntegerParameter(sim_intparam_stop_request_counter,&req);
	if ((req!=_stopSimulationRequestCounter)&&(ver>=30006)) // stop in stepped mode only supported from V-REP 3.0.6 on!
	{
		_waitForTrigger=false;
		_waitForTriggerFunctionEnabled=false;
	}
	return(_waitForTriggerFunctionEnabled&&_waitForTrigger);
}
Exemplo n.º 2
0
// 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())
			replyData[0]=0; // this tells V-REP that we don't wanna execute the main script
		else
			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
		allConnections.simulationEnded();
	}

	simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
	return(retVal);
}
Exemplo n.º 3
0
void LUA_DISABLEPUBLISHER_CALLBACK(SLuaCallBack* p)
{
	p->outputArgCount=0;
	CLuaFunctionData D;
	int result=-1;
	if (D.readDataFromLua(p,inArgs_DISABLEPUBLISHER,inArgs_DISABLEPUBLISHER[0],LUA_DISABLEPUBLISHER_COMMAND))
	{
		std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
		std::string topicName(inData->at(0).stringData[0]);
		if (topicName.length()>0)
		{
			int errorModeSaved;
			simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
			simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);

			result=ROS_server::removePublisher(topicName.c_str(),false);

			simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings

			if (result==-1)
				simSetLastError(LUA_DISABLEPUBLISHER_COMMAND, "Topic could not be unpublished."); // output an error
		}
		else
			simSetLastError(LUA_DISABLEPUBLISHER_COMMAND, "Invalid topic name."); // output an error
	}
	D.pushOutData(CLuaFunctionDataItem(result));
	D.writeDataToLua(p);
}
Exemplo n.º 4
0
void LUA_WAKEPUBLISHER_CALLBACK(SScriptCallBack* p)
{

	CScriptFunctionData D;
	int result=-1;
	if (D.readDataFromStack(p->stackID,inArgs_WAKEPUBLISHER,inArgs_WAKEPUBLISHER[0],LUA_WAKEPUBLISHER_COMMAND))
	{
		std::vector<CScriptFunctionDataItem>* inData=D.getInDataPtr();
		std::string topicName(inData->at(0).stringData[0]);
		if (topicName.length()>0)
		{
			int publishCnt=inData->at(1).int32Data[0];

			int errorModeSaved;
			simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
			simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);

			result=ROS_server::wakePublisher(topicName.c_str(),publishCnt);

			simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings

			if (result==-2)
				simSetLastError(LUA_WAKEPUBLISHER_COMMAND, "Topic could not be found."); // output an error
		}
		else
			simSetLastError(LUA_WAKEPUBLISHER_COMMAND, "Invalid topic name."); // output an error
	}
	D.pushOutData(CScriptFunctionDataItem(result));
	D.writeDataToStack(p->stackID);
}
Exemplo n.º 5
0
void CSimxContainer::executeCommands(CSimxContainer* outputContainer,CSimxSocket* sock)
{
	if (outputContainer->getCommandCount()==0)
	{ // Make sure the output buffer was already sent and is empty
		outputContainer->setMessageID(_messageID);
		outputContainer->setDataTimeStamp(_dataTimeStamp);
		int simState=simGetSimulationState();
		BYTE serverState=0;
		outputContainer->setDataServerTimeStamp(int(simGetSystemTime()*1000.1f));
		if (simState!=sim_simulation_stopped)
		{
			serverState|=1; // simulation is not stopped
			if (simState==sim_simulation_paused)
				serverState|=2; // simulation is paused
		}
		if (simGetRealTimeSimulation()>0)
			serverState|=4;
		int editModeType;
		simGetIntegerParameter(sim_intparam_edit_mode_type,&editModeType);
		serverState|=(editModeType<<3);
		int sceneUniqueID;
		simGetIntegerParameter(sim_intparam_scene_unique_id,&sceneUniqueID);
		outputContainer->setSceneID(WORD(sceneUniqueID));
		outputContainer->setServerState(serverState);

		// Prepare for correct error reporting:
		int errorModeSaved;
		simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
		simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_report);
		char* err=simGetLastError(); // just clear the last error
		if (err!=NULL)
			simReleaseBuffer(err);

		// Execute pending commands:
		for (unsigned int i=0;i<_allCommands.size();i++)
		{
			CSimxCmd* outputCmd=_allCommands[i]->execute(sock,_otherSideIsBigEndian);
			if (outputCmd!=NULL) // gradual or split commands are not added here!
				outputContainer->addCommand(outputCmd,false);
		}

		// Restore previous error report mode:
		simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); 

		_removeNonContinuousCommands(); // simx_opmode_oneshot_split commands are also kept!
	}
}
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
}
Exemplo n.º 7
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)
}
Exemplo n.º 9
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];
	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)
}
Exemplo n.º 10
0
void CSimxSocket::setWaitForTriggerEnabled(bool e)
{
	if (_waitForTriggerFunctionAuthorized)
	{
		_waitForTriggerFunctionEnabled=e;
		_waitForTrigger=e;
		// initialize the stop request counter:
		simGetIntegerParameter(sim_intparam_stop_request_counter,&_stopSimulationRequestCounter);
	}
}
Exemplo n.º 11
0
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
	simLockInterface(1);
	// 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;


	// Acknowledgment message display:
	// *****************************************************************
	static int auxConsoleHandleForAcknowledgmentDisplay=-1;
	static int acknowledgmentDisplayStartTime=0;
	if (displayAcknowledgment)
	{
		auxConsoleHandleForAcknowledgmentDisplay=simAuxiliaryConsoleOpen("Acknowledgments",10,2+4+16,NULL,NULL,NULL,NULL);
		simAuxiliaryConsolePrint(auxConsoleHandleForAcknowledgmentDisplay,"The wiimote plugin is courtesy of Eric Rohmer.\n\nThe wiimote plugin contains WiiYourself! wiimote code by gl.tter\nhttp://gl.tter.org");
		acknowledgmentDisplayStartTime=timeGetTime();
		displayAcknowledgment=false;
	}
	if ( (auxConsoleHandleForAcknowledgmentDisplay!=-1)&&(timeGetTime()-acknowledgmentDisplayStartTime>5000) )
	{
		simAuxiliaryConsoleClose(auxConsoleHandleForAcknowledgmentDisplay);
		auxConsoleHandleForAcknowledgmentDisplay=-1;
	}
	// *****************************************************************

	// Clean-up at simulation end:
	// *****************************************************************
	if (message==sim_message_eventcallback_simulationended)
	{
		if (auxConsoleHandleForAcknowledgmentDisplay!=-1)
		{
			simAuxiliaryConsoleClose(auxConsoleHandleForAcknowledgmentDisplay);
			auxConsoleHandleForAcknowledgmentDisplay=-1;
		}
		for (int i=0;i<4;i++) // for the 4 devices
		{
			while (startCountPerDevice[i]>0)
			{
				startCountOverall--;
				startCountPerDevice[i]--;
				if (startCountOverall==0)
					killThread();
			}
		}
	}
	// *****************************************************************

	simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
	simLockInterface(0);
	return(retVal);
}
Exemplo n.º 12
0
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!
}
Exemplo n.º 13
0
// This is the plugin messaging routine (i.e. V-REP calls this function very often, with various messages):
VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customData,int* replyData)
{ // This is called quite often. Just watch out for messages/events you want to handle
	// Keep following 6 lines at the beginning and unchanged:
	static bool refreshDlgFlag=true;
	int errorModeSaved;
	simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
	simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
	void* retVal=NULL;

	// Here we can intercept many messages from V-REP (actually callbacks).
	// For a complete list of messages that you can intercept/react with, search for "sim_message_eventcallback"-type constants
	// in the V-REP user manual.

	if (message==sim_message_eventcallback_refreshdialogs)
		refreshDlgFlag=true; // V-REP dialogs were refreshed. Maybe a good idea to refresh this plugin's dialog too

	if (message==sim_message_eventcallback_menuitemselected)
	{ // A custom menu bar entry was selected..
		if (auxiliaryData[0]==urdfDialog->dialogMenuItemHandle)
			urdfDialog->makeVisible(!urdfDialog->getVisible()); // Toggle visibility of the dialog
	}

	if (message==sim_message_eventcallback_instancepass)
	{ // It is important to always correctly react to events in V-REP. This message is the most convenient way to do so:
		urdfDialog->handleCommands();
		urdfDialog->setSimulationStopped(simGetSimulationState()==sim_simulation_stopped);

		int flags=auxiliaryData[0];
		bool sceneContentChanged=((flags&(1+2+4+8+16+32+64+256))!=0); // object erased, created, model or scene loaded, und/redo called, instance switched, or object scaled since last sim_message_eventcallback_instancepass message
		bool instanceSwitched=((flags&64)!=0);

		if (instanceSwitched)
		{
			// React to an instance switch here!!
		}

		if (sceneContentChanged)
		{
			refreshDlgFlag=true;
		}
	}
	if ((message==sim_message_eventcallback_guipass)&&refreshDlgFlag)
	{ // handle refresh of the plugin's dialog:
		urdfDialog->refresh(); // Refresh the dialog
		refreshDlgFlag=false;
	}

	// Keep following unchanged:
	simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
	return(retVal);
}
Exemplo n.º 14
0
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
}
Exemplo n.º 15
0
// 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 ROS_server::spinOnce()
{
	// Disable error reporting (it is enabled in the service processing part, but we don't want error reporting for publishers/subscribers)
	int errorModeSaved;
	simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
	simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);

	//Handle all streaming (publishers)
	streamAllData();

	//Process all requested services and topic subscriptions
	ros::spinOnce();

	// Restore previous error report mode:
	simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); 
}
Exemplo n.º 17
0
// 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:
	simLockInterface(1);
	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_simulationabouttostart)
	{
	    // Simulation is about to start
		ROS_INFO("[V-REP_DXL_ARM] Simulation is about to start.");
		getJointHandles();
	}

	if (message==sim_message_eventcallback_mainscriptabouttobecalled)
	{
		std::cout << " ---- ---- ---- " << std::endl;
	    // A simulation iteration is about to take place
		for (int i = 0; i < JOINT_COUNT; ++i)
		{
			simGetJointPosition(joint_handles[i], &msg.data[i]);
			msg.data[i] *= RAD_TO_DEG;
			msg.data[i] += 150;
			std::cout << msg.data[i] << std::endl;
		}
		msg.data[JOINT_COUNT] = 150; // Grasp -> center position

		pub.publish(msg);
	}

	if (message==sim_message_eventcallback_simulationended)
	{
	    // Simulation just ended
	    ROS_INFO("[V-REP_DXL_ARM] Simulation just ended.");
	}

	// Keep following unchanged:
	simSetIntegerParameter(sim_intparam_error_report_mode, errorModeSaved); // restore previous settings
	simLockInterface(0);
	return (retVal);
}
Exemplo n.º 18
0
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_simulationabouttostart)
		simulationAboutToStart();
	if (message==sim_message_eventcallback_simulationended)
		simulationEnded();

	simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
	return(retVal);
}
Exemplo n.º 19
0
void LUA_ENABLESUBSCRIBER_CALLBACK(SScriptCallBack* p)
{

	CScriptFunctionData D;
	int result=-1;
	if (D.readDataFromStack(p->stackID,inArgs_ENABLESUBSCRIBER,inArgs_ENABLESUBSCRIBER[0]-2,LUA_ENABLESUBSCRIBER_COMMAND)) // -2 because the last two args are optional
	{
		std::vector<CScriptFunctionDataItem>* inData=D.getInDataPtr();
		std::string topicName(inData->at(0).stringData[0]);
		if (topicName.length()>0)
		{
			int queueSize=inData->at(1).int32Data[0];
			int streamCmd=inData->at(2).int32Data[0];
			int auxInt1=inData->at(3).int32Data[0];
			int auxInt2=inData->at(4).int32Data[0];
			std::string auxString(inData->at(5).stringData[0]);
			int callbackTag_before=-1; // no callback (default)
			int callbackTag_after=-1; // no callback (default)
			if (inData->size()>6)
				callbackTag_before=inData->at(6).int32Data[0];
			if (inData->size()>7)
				callbackTag_after=inData->at(7).int32Data[0];
			
			int errorModeSaved;
			simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
			simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);

			result=ROS_server::addSubscriber(topicName.c_str(),queueSize,streamCmd,auxInt1,auxInt2,auxString.c_str(),callbackTag_before,callbackTag_after);

			simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings

			if (result==-1)
				simSetLastError(LUA_ENABLESUBSCRIBER_COMMAND, "Topic could not be subscribed."); // output an error
		}
		else
			simSetLastError(LUA_ENABLESUBSCRIBER_COMMAND, "Invalid topic name."); // output an error
	}
	D.pushOutData(CScriptFunctionDataItem(result));
	D.writeDataToStack(p->stackID);
}
Exemplo n.º 20
0
void LUA_DISABLESUBSCRIBER_CALLBACK(SScriptCallBack* p)
{

	CScriptFunctionData D;
	bool result=false;
	if (D.readDataFromStack(p->stackID,inArgs_DISABLESUBSCRIBER,inArgs_DISABLESUBSCRIBER[0],LUA_DISABLESUBSCRIBER_COMMAND))
	{
		std::vector<CScriptFunctionDataItem>* inData=D.getInDataPtr();
		int errorModeSaved;
		simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
		simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);

		result=ROS_server::removeSubscriber(inData->at(0).int32Data[0]);

		simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings

		if (!result)
			simSetLastError(LUA_DISABLESUBSCRIBER_COMMAND, "Topic could not be unsubscribed."); // output an error
	}
	D.pushOutData(CScriptFunctionDataItem(result));
	D.writeDataToStack(p->stackID);
}
Exemplo n.º 21
0
void LUA_ENABLEPUBLISHER_CALLBACK(SScriptCallBack* p)
{

	CScriptFunctionData D;
	std::string effectiveTopicName;
	if (D.readDataFromStack(p->stackID,inArgs_ENABLEPUBLISHER,inArgs_ENABLEPUBLISHER[0]-1,LUA_ENABLEPUBLISHER_COMMAND)) // -1 because the last argument is optional
	{
		std::vector<CScriptFunctionDataItem>* inData=D.getInDataPtr();
		std::string topicName(inData->at(0).stringData[0]);
		if (topicName.length()>0)
		{
			int queueSize=inData->at(1).int32Data[0];
			int streamCmd=inData->at(2).int32Data[0];
			int auxInt1=inData->at(3).int32Data[0];
			int auxInt2=inData->at(4).int32Data[0];
			std::string auxString(inData->at(5).stringData[0]);
			int publishCnt=0; // 0 is the default value for this optional argument
			if (inData->size()>6)
				publishCnt=inData->at(6).int32Data[0];
			
			int errorModeSaved;
			simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
			simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);

			effectiveTopicName=ROS_server::addPublisher(topicName.c_str(),queueSize,streamCmd,auxInt1,auxInt2,auxString.c_str(),publishCnt);

			simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings

			if (effectiveTopicName.length()==0)
				simSetLastError(LUA_ENABLEPUBLISHER_COMMAND, "Topic could not be published."); // output an error
		}
		else
			simSetLastError(LUA_ENABLEPUBLISHER_COMMAND, "Invalid topic name."); // output an error
	}
	if (effectiveTopicName.size()!=0)
		D.pushOutData(CScriptFunctionDataItem(effectiveTopicName));
	D.writeDataToStack(p->stackID);
}
Exemplo n.º 22
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
	 _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)
}
Exemplo n.º 23
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];
	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)
}
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)

}
Exemplo n.º 25
0
// 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);
}
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
	WIN_AFX_MANAGE_STATE;

	simLockInterface(1);
	static bool refreshDlgFlag=true;

	// 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_refreshdialogs){
	    // V-REP dialogs were refreshed. Maybe a good idea to refresh this plugin's dialog too
	    refreshDlgFlag=true;
	}


	if (message==sim_message_eventcallback_menuitemselected){
	    // A custom menu bar entry was selected..
	}

	if (message==sim_message_eventcallback_instancepass){
	    // It is important to always correctly react to events in V-REP. This message is the most convenient way to do so:

		const int flags=auxiliaryData[0];
		const 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
		const bool instanceSwitched=((flags&64)!=0);

		if (instanceSwitched)
		{
			// React to an instance switch here!!
		}

		if (sceneContentChanged)
		{ // we actualize plugin objects for changes in the scene
			objectContainer->actualizeForSceneContent();
			refreshDlgFlag=true;
		}
	}

	if (message==sim_message_eventcallback_moduleopen)
	{ // A script called simOpenModule (by default the main script). Is only called during simulation.
		if ( (customData==NULL)||(_stricmp(pluginName,(char*)customData)==0) ) // is the command also meant for this plugin?
			objectContainer->startOfSimulation();				// yes!
	}

	if (message==sim_message_eventcallback_modulehandle){
	    // A script called simHandleModule (by default the main script). Is only called during simulation.


		if ( (customData==NULL)||(_stricmp(pluginName,(char*)customData)==0) ) // is the command also meant for this plugin?
		{
			//clock_t init, final;

			//init=clock();
			objectContainer->handleSimulation();
			//final=clock()-init;
			//std::cout << (double)final / ((double)CLOCKS_PER_SEC) << std::endl;
		}// yes!
	}



	if (message==sim_message_eventcallback_moduleclose){
	    // A script called simCloseModule (by default the main script). Is only called during simulation.
		if ( (customData==NULL)||(_stricmp(pluginName,(char*)customData)==0) ) // is the command also meant for this plugin?
			objectContainer->endOfSimulation();					// yes!
	}

	if (message==sim_message_eventcallback_instanceswitch){
        // Here the user switched the instance. 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)
	}

	if (message==sim_message_eventcallback_simulationabouttostart){
	    // Simulation is about to start
	}

	if (message==sim_message_eventcallback_simulationended){
	    // Simulation just ended
	}

	// You can add more messages to handle here

	if ((message==sim_message_eventcallback_guipass)&&refreshDlgFlag){
	    // handle refresh of the plugin's dialog:
		refreshDlgFlag=false;
	}

	simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
	simLockInterface(0);
	return(retVal);
}
// 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)
}
Exemplo n.º 28
0
// 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 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:
    simLockInterface(1);
    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
    }

    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 commands (e.g. commands needed to generate ROS messages)
        //
        // e.g. to read all joint positions:
        simLockInterface(1);

        ros::spinOnce();

        sensor_msgs::Image image_msg;

        //
        // Execute player speceific functions
        //
        if ((player_turn_state == PLAYER_NONE) && new_player_turn)
        {
            //ROS_INFO("New player turn received: %f %f", player_turn.power, player_turn.angle);
            ROS_INFO("[PLAYER_NONE] Player turn ended.\n");
            new_player_turn = false;
        }
        else if (player_turn_state == PLAYER_ROTATE_MOVE)
        {
            tip_position[0] = table_state.white_ball.x - TIP_OFFSET_DISTANCE * sin(player_turn.angle);
            tip_position[1] = table_state.white_ball.y + TIP_OFFSET_DISTANCE * cos(player_turn.angle);

            tip_orientation[2] = player_turn.angle;

            std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_ROTATE_MOVE] Orientation success: " <<
                      simSetObjectOrientation(tip_handle, WORLD_FRAME, tip_orientation) << std::endl;
            std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_ROTATE_MOVE] Position success: " <<
                      simSetObjectPosition(tip_handle, WORLD_FRAME, tip_position) << std::endl;

            player_turn_state = PLAYER_CUE_DOWN;
        }
        else if (player_turn_state == PLAYER_CUE_DOWN)
        {
            tip_position[2] = CUE_ON_TABLE_HEIGHT;
            std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_CUE_DOWN] Position success: " <<
                      simSetObjectPosition(tip_handle, WORLD_FRAME, tip_position) << std::endl;

            player_turn_state = PLAYER_EXECUTE_SHOT;
        }
        else if (player_turn_state == PLAYER_EXECUTE_SHOT)
        {
            double dist = player_turn.power * POWER_DISTANCE_RATIO + MIN_POWER_DISTANCE;
            tip_position[0] += sin(player_turn.angle) * dist;
            tip_position[1] -= cos(player_turn.angle) * dist;

            std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_EXECUTE_SHOT] Position success: " <<
                      simSetObjectPosition(tip_handle, WORLD_FRAME, tip_position) << std::endl;

            player_turn_state = PLAYER_RAISE_CUE;
        }
        else if (player_turn_state == PLAYER_RAISE_CUE)
        {
            tip_position[2] = CUE_RAISED_HEIGHT;
            std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_RAISE_CUE] Position success: " <<
                      simSetObjectPosition(tip_handle, WORLD_FRAME, tip_position) << std::endl;

            player_turn_state = PLAYER_NONE;
        }

        generate_image_message(image_msg);

        //
        // Transfer to old table state and get new state
        //
        old_table_state = table_state;
        get_ball_positions();
        get_cue_position_orientation();

        //
        //Check if table is in steady state
        //
        steady_state = (table_state.white_ball.z < POOL_TABLE_HEIGHT)?
                       (true):(closeTo(old_table_state.white_ball, table_state.white_ball, STEADY_STATE_DRIFT));
        for (unsigned int i = 0; (i < BALL_COUNT) && steady_state; i++)
        {
            if (!closeTo(old_table_state.balls[i], table_state.balls[i], STEADY_STATE_DRIFT))
            {
                steady_state = false;
            }
        }

        //
        // Publish information
        //
        if (steady_state && (player_turn_state == PLAYER_NONE && new_player_turn == false))
        {
            ROS_INFO("Table is in steady state!\n");
            //calculate turn score and publish it.
            int score = calculate_turn_score();
            ROS_INFO("The score for the last turn is %d", score);
            std_msgs::Int32 score_msg;
            score_msg.data = score;
            score_pub.publish(score_msg);
            usleep(50 * 1000); //50ms
            table_state_pub.publish(table_state); // publish a message only if nothing is moving
        }

        //
        // Move white ball if potted
        //
        if (steady_state && (table_state.white_ball.z < POOL_TABLE_HEIGHT)&&
                (player_turn_state == PLAYER_NONE && new_player_turn == false))
        {
            std::cout << "[V-REP_ROS_POOL_SIM] Reset: " << simResetDynamicObject(white_ball_handle) << " ";
            std::cout << "[V-REP_ROS_POOL_SIM] Moving white to initial position success: " <<
                      simSetObjectPosition(white_ball_handle, WORLD_FRAME, white_ball_initial_position) << std::endl;
            table_state.white_ball.x = white_ball_initial_position[0];
            table_state.white_ball.y = white_ball_initial_position[1];
            table_state.white_ball.z = white_ball_initial_position[2];
        }

        //
        // Move all red balls is all potted
        //
        if (steady_state)
        {
            int balls_in = 0;
            for (unsigned int i = 0; i < BALL_COUNT; i++)
            {
                if (table_state.balls[i].z < POOL_TABLE_HEIGHT)
                {
                    balls_in++;
                }
            }
            if (balls_in == BALL_COUNT)
            {
                //Move all balls to the top!
                std::cout << "[V-REP_ROS_POOL_SIM] Reset: " << simResetDynamicObject(white_ball_handle) << " ";
                std::cout << "[V-REP_ROS_POOL_SIM] Moving white to initial position success: " <<
                          simSetObjectPosition(white_ball_handle, WORLD_FRAME, white_ball_initial_position) << std::endl;

                for (unsigned int i = 0; i < BALL_COUNT; i++)
                {
                    std::cout << "[V-REP_ROS_POOL_SIM] Ball " << i << std::endl;
                    float p[3];
                    p[0] = initial_table_state.balls[i].x;
                    p[1] = initial_table_state.balls[i].y;
                    p[2] = initial_table_state.balls[i].z;

                    std::cout << "[V-REP_ROS_POOL_SIM] Reset: " << simResetDynamicObject(balls_handle[i]) << std::endl;
                    std::cout << "[V-REP_ROS_POOL_SIM] Moving ball position success: " <<
                              simSetObjectPosition(balls_handle[i], WORLD_FRAME, p) << std::endl;
                }
                table_state = initial_table_state;
                games_played++;
                ROS_INFO("The are %d games played so far.", games_played);
            }
        }

        simLockInterface(0);

        //Publish messages
        img_pub.publish(image_msg);

        // The best would be to start ROS activity when a simulation is running, and stop it when a simulation ended
        // If you allow ROS activity while a simulation is not running, then it becomes a little bit tricky when a scene
        // was switched for example (e.g. the clients would need a way to detect that)
    }

    if (message==sim_message_eventcallback_simulationabouttostart)
    {   // Simulation is about to start
        // Here you could launch the ROS node (if using just one node)
        // If more than one node should be supported (e.g. with different services per node),
        // then it is better to start a node via a custom Lua function
        white_ball_handle = simGetObjectHandle("white_ball");
        std::cout << "[V-REP POOL PLUGIN] White ball handle: " << white_ball_handle << std::endl;
        tip_handle = simGetObjectHandle("tip");

        for (int i=0; i < BALL_COUNT; i++)
        {
            std::stringstream ball_name;
            ball_name << "ball";
            ball_name << i;
            balls_handle[i] = simGetObjectHandle(ball_name.str().c_str());
            //std::cout << "Ball " << i << " handle: " << balls_handle[i] << std::endl;
        }
        camera_handle = simGetObjectHandle("camera_sensor");
        std::cout << "[V-REP POOL PLUGIN] Camera handle: " << camera_handle << std::endl;

        //Read all positions and make them as old positions
        get_ball_positions();
        old_table_state = table_state;
        pre_hit_table_state = table_state;
        initial_table_state = table_state;
    }

    if (message==sim_message_eventcallback_simulationended)
    {   // Simulation just ended
        // Here you could kill the ROS node(s) that are still active. There could also be a custom Lua function to kill a specific ROS node.
    }

    // Keep following unchanged:
    simSetIntegerParameter(sim_intparam_error_report_mode, errorModeSaved); // restore previous settings
    simLockInterface(0);
    return (retVal);
}
Exemplo n.º 30
0
// This is the plugin messaging routine (i.e. V-REP calls this function very often, with various messages):
VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customData,int* replyData)
{ // This is called quite often. Just watch out for messages/events you want to handle
	// Keep following 6 lines at the beginning and unchanged:
	static bool refreshDlgFlag=true;
	int errorModeSaved;
	simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
	simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
	void* retVal=NULL;

	// Here we can intercept many messages from V-REP (actually callbacks).
	// For a complete list of messages that you can intercept/react with, search for "sim_message_eventcallback"-type constants
	// in the V-REP user manual.

	if (message==sim_message_eventcallback_refreshdialogs)
		refreshDlgFlag=true; // V-REP dialogs were refreshed. Maybe a good idea to refresh this plugin's dialog too

	if (message==sim_message_eventcallback_menuitemselected)
	{ // A custom menu bar entry was selected..
		if (auxiliaryData[0]==colladaDialog->dialogMenuItemHandle)
		{
			colladaDialog->makeVisible(!colladaDialog->getVisible()); // Toggle visibility of the dialog
		}
	}

	if (message==sim_message_eventcallback_colladaplugin)
	{
		if (auxiliaryData[0]==0)
			replyData[0]=PLUGIN_VERSION; // this is the version number of this plugin
		if (auxiliaryData[0]==1)
			replyData[0]=colladaDialog->importSingleGroupedShape((const char*)customData,(auxiliaryData[1]&1)!=0,float(auxiliaryData[2])/1000.0f);
	}

	if (message==sim_message_eventcallback_instancepass)
	{ // It is important to always correctly react to events in V-REP. This message is the most convenient way to do so:
		colladaDialog->handleCommands();
		colladaDialog->setSimulationStopped(simGetSimulationState()==sim_simulation_stopped);

		int flags=auxiliaryData[0];
		bool sceneContentChanged=((flags&(1+2+4+8+16+32+64+256))!=0); // object erased, created, model or scene loaded, und/redo called, instance switched, or object scaled since last sim_message_eventcallback_instancepass message
		bool instanceSwitched=((flags&64)!=0);

		if (instanceSwitched)
		{
			// React to an instance switch here!!
		}

		if (sceneContentChanged)
		{
			refreshDlgFlag=true;
		}
	}
	if ((message==sim_message_eventcallback_guipass)&&refreshDlgFlag)
	{ // handle refresh of the plugin's dialog:
		colladaDialog->refresh(); // Refresh the dialog
		refreshDlgFlag=false;
	}

	if (message==sim_message_eventcallback_simulationabouttostart)
	{
		colladaDialog->simulationAboutToStart();
	}
	if (message==sim_message_eventcallback_mainscriptabouttobecalled)
	{
		colladaDialog->mainScriptAboutToBeCalled();
	}
	if (message==sim_message_eventcallback_beforerendering)
	{ // we are still in the main SIM thread!
		colladaDialog->renderingPassAboutToBeCalled();
	}
	if (message==sim_message_eventcallback_simulationended)
	{
		colladaDialog->simulationEnded();
	}

	// You can add many more messages to handle here

	// Keep following unchanged:
	simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
	return(retVal);
}