Esempio n. 1
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)
}
Esempio n. 2
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);
}
Esempio n. 3
0
void CBubbleRobDialog::OnLbnSelchangeBubblerobList()
{
	simLockInterface(1);
	int objectHandle=getSelectedObjectInList();
	simRemoveObjectFromSelection(sim_handle_all,-1);
	CBubbleRob* bubbleRob=CAccess::bubbleRobContainer->getFromID(objectHandle);
	if (bubbleRob!=NULL)
	{
		int sceneObjHandle=bubbleRob->getAssociatedObject();
		if (sceneObjHandle!=-1)
			simAddObjectToSelection(sim_handle_single,sceneObjHandle);
	}
	simLockInterface(0);
}
Esempio n. 4
0
void CMtbRobotDialog::on_qqAutomatic_clicked()
{
	simLockInterface(1);
	int objectHandle=getSelectedObjectInList();
	if (objectHandle!=-1)
	{
		CMtbRobot* mtbRobot=CAccess::mtbRobotContainer->getFromID(objectHandle);
		if (mtbRobot!=NULL)
		{
			mtbRobot->setOptions(mtbRobot->getOptions()^1);
			simAnnounceSceneContentChange(); // so that we have an undo point here
		}
	}
	simLockInterface(0);
}
Esempio n. 5
0
void CMtbRobotDialog::on_qqRobotProgram_textChanged()
{
	simLockInterface(1);
	int objectHandle=getSelectedObjectInList();
	if (objectHandle!=-1)
	{
		CMtbRobot* mtbRobot=CAccess::mtbRobotContainer->getFromID(objectHandle);
		if (mtbRobot!=NULL)
		{
			std::string prg=ui->qqRobotProgram->toPlainText().toStdString();
			mtbRobot->setProgram(prg); // Update the program
//			simAnnounceSceneContentChange(); // we don't allow for undo points here (otherwise every single char could be undone)
		}
	}
	simLockInterface(0);
}
Esempio n. 6
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);
}
Esempio n. 7
0
void CMtbRobotDialog::on_qqRobotList_itemSelectionChanged()
{
	if (!inRefreshRoutine)
	{
		simLockInterface(1);
		int objectHandle=getSelectedObjectInList();
		simRemoveObjectFromSelection(sim_handle_all,-1);
		CMtbRobot* mtbRobot=CAccess::mtbRobotContainer->getFromID(objectHandle);
		if (mtbRobot!=NULL)
		{
			int sceneObjHandle=mtbRobot->getAssociatedObject();
			if (sceneObjHandle!=-1)
				simAddObjectToSelection(sim_handle_single,sceneObjHandle); // select the associated scene object!
		}
		simLockInterface(0);
	}
}
Esempio n. 8
0
void LUA_END_CALLBACK(SLuaCallBack* p)
{ // the callback function of the new Lua command
	simLockInterface(1);

	int result=-1; // error

	if (p->inputArgCount>0)
	{ // Ok, we have at least 1 input argument
		if (p->inputArgTypeAndSize[0*2+0]==sim_lua_arg_int)
		{ // Ok, we have (at least) 1 int as argument
			if ( (p->inputInt[0]>=0)&&(p->inputInt[0]<4)&&(startCountPerDevice[p->inputInt[0]]>0) )
			{
				startCountOverall--;
				startCountPerDevice[p->inputInt[0]]--;
				if (startCountPerDevice[p->inputInt[0]]==0)
				{ // No one is using this device anymore... we remove it
					EnterCriticalSection(&m_cs);
					_allDevices[p->inputInt[0]]->Disconnect();
					delete _allDevices[p->inputInt[0]];
					_allDevices[p->inputInt[0]]=NULL;
					LeaveCriticalSection(&m_cs);
				}
				if (startCountOverall==0)
					killThread();
				result=1;
			}
			else
				simSetLastError(LUA_END,"Invalid device index."); // output an error
		}
		else
			simSetLastError(LUA_END,"Wrong argument type/size."); // output an error
	}
	else
		simSetLastError(LUA_END,"Not enough arguments."); // output an error


	// Now we prepare the return value:
	p->outputArgCount=1; // 1 return value
	p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer
	p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_int;	// The return value is an int
	p->outputArgTypeAndSize[2*0+1]=1;				// Not used (table size if the return value was a table)
	p->outputInt=(simInt*)simCreateBuffer(1*sizeof(result)); // 1 int return value
	p->outputInt[0]=result; // This is the int value we want to return

	simLockInterface(0);
}
Esempio n. 9
0
void CBubbleRobDialog::OnEnKillfocusMaxVelocity()
{
	simLockInterface(1);
	CBubbleRob* bubbleRob=CAccess::bubbleRobContainer->getFromAssociatedObject(simGetObjectLastSelection());
	if (bubbleRob!=NULL)
	{
		CString tmp;
		GetDlgItemText(IDC_MAX_VELOCITY,tmp);
		char* last;
		float v=float(strtod(tmp,&last));
		if (last[0]==0)
		{
			bubbleRob->setMaxVelocity(3.1415f*v/120.0f); // we had it displayed in RPM 
			simAnnounceSceneContentChange(); // this is so that V-REP registers a new undo-point here
		}
	}
	refresh();
	simLockInterface(0);
}
Esempio n. 10
0
void LUA_SET_CALLBACK(SLuaCallBack* p)
{ // the callback function of the new Lua command
	simLockInterface(1);

	int result=-1; // error

	if (p->inputArgCount>2)
	{ // Ok, we have at least 3 input argument
		if ( (p->inputArgTypeAndSize[0*2+0]==sim_lua_arg_int)&&(p->inputArgTypeAndSize[1*2+0]==sim_lua_arg_int)&&(p->inputArgTypeAndSize[2*2+0]==sim_lua_arg_bool) )
		{ // Ok, we have (at least) 2 ints and a bool as argument
			if ( (p->inputInt[0]>=0)&&(p->inputInt[0]<4)&&(startCountPerDevice[p->inputInt[0]]>0) )
			{ // Ok, we have an "open" device here
				_allDeviceData[p->inputInt[0]].leds=BYTE(p->inputInt[1]);
				_allDeviceData[p->inputInt[0]].rumble=(p->inputBool[1]!=0);
				result=1;
			}
			else
				simSetLastError(LUA_SET,"Invalid device index."); // output an error
		}
		else
			simSetLastError(LUA_SET,"Wrong argument type/size."); // output an error
	}
	else
		simSetLastError(LUA_SET,"Not enough arguments."); // output an error


	// Now we prepare the return value:
	p->outputArgCount=1; // 1 return value
	p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer
	p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_int;	// The return value is an int
	p->outputArgTypeAndSize[2*0+1]=1;				// Not used (table size if the return value was a table)
	p->outputInt=(simInt*)simCreateBuffer(1*sizeof(result)); // 1 int return value
	p->outputInt[0]=result; // This is the int value we want to return

	simLockInterface(0);
}
Esempio n. 11
0
void LUA_START_CALLBACK(SLuaCallBack* p)
{ // the callback function of the new Lua command
	simLockInterface(1);

	int deviceIndex=-1; // error

	if (p->inputArgCount>0)
	{ // Ok, we have at least 1 input argument
		if (p->inputArgTypeAndSize[0*2+0]==sim_lua_arg_int)
		{ // Ok, we have (at least) 1 int as argument
			int effectiveDeviceIndex=-1;
			int desiredIndex=p->inputInt[0];
			if (desiredIndex==-1)
			{ // We want any free device (not yet being used)
				// We search for a free slot:
				for (int i=0;i<4;i++)
				{
					if (_allDevices[i]==NULL)
					{
						desiredIndex=i;
						break;
					}
				}
			}
			if ( (desiredIndex>=0)&&(desiredIndex<4) )
			{
				if (_allDevices[desiredIndex]==NULL)
				{ // We connect to a new device!
					wiimote* it=new wiimote();
					it->ChangedCallback=on_state_change;
					it->CallbackTriggerFlags=(state_change_flags)(CONNECTED|EXTENSION_CHANGED|MOTIONPLUS_CHANGED);
					if (it->Connect(desiredIndex+1))
					{ // Success!
						_allDeviceData[desiredIndex].buttonStates=0;
						_allDeviceData[desiredIndex].batteryState=0;
						_allDeviceData[desiredIndex].accelerationX=0.0f;
						_allDeviceData[desiredIndex].accelerationY=0.0f;
						_allDeviceData[desiredIndex].accelerationZ=0.0f;
						_allDeviceData[desiredIndex].pitch=0.0f;
						_allDeviceData[desiredIndex].roll=0.0f;
						_allDeviceData[desiredIndex].rumble=0;
						_allDeviceData[desiredIndex].leds=0;
						_allDeviceData[desiredIndex].initialized=false;
						_allDevices[desiredIndex]=it;
					}
					else
						desiredIndex=-1; // No more devices available!
				}
			}
			if ( (desiredIndex>=0)&&(desiredIndex<4) )
			{
				if (startCountOverall==0)
				{ // Launch the thread!
					_wiiThreadLaunched=false;
					CreateThread(NULL,0,_wiiThread,NULL,THREAD_PRIORITY_NORMAL,NULL);
					while (!_wiiThreadLaunched)
						Sleep(2);
				}
				startCountOverall++;
				startCountPerDevice[desiredIndex]++;
				deviceIndex=desiredIndex;
			}
			else
				simSetLastError(LUA_START,"Invalid device index, or no available device at given index. Is device connected?"); // output an error
		}
		else
			simSetLastError(LUA_START,"Wrong argument type/size."); // output an error
	}
	else
		simSetLastError(LUA_START,"Not enough arguments."); // output an error

	// Now we prepare the return value:
	p->outputArgCount=1; // 1 return value
	p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer
	p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_int;	// The return value is an int
	p->outputArgTypeAndSize[2*0+1]=1;				// Not used (table size if the return value was a table)
	p->outputInt=(simInt*)simCreateBuffer(1*sizeof(deviceIndex)); // 1 int return value
	p->outputInt[0]=deviceIndex; // This is the int value we want to return

	simLockInterface(0);
}
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 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 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);
}
// 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 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:
	simLockInterface(1);
	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 (simGetSimulationState() == sim_simulation_advancing_running) {

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

		}

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

	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

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

	}

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

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

	}


	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
	simLockInterface(0);
	return(retVal);
}
Esempio n. 17
0
void CBubbleRobDialog::OnOK()
{
	simLockInterface(1);
	simLockInterface(0);
}
Esempio n. 18
0
void CBubbleRobDialog::OnCancel()
{ // We closed the dialog
	simLockInterface(1);
	setVisible(false);
	simLockInterface(0);
}
Esempio n. 19
0
void LUA_GET_CALLBACK(SLuaCallBack* p)
{ // the callback function of the new Lua command
	simLockInterface(1);

	int result=-1; // error

	struct SWiiData returnData;

	if (p->inputArgCount>0)
	{ // Ok, we have at least 1 input argument
		if (p->inputArgTypeAndSize[0*2+0]==sim_lua_arg_int)
		{ // Ok, we have (at least) 1 int as argument
			if ( (p->inputInt[0]>=0)&&(p->inputInt[0]<4)&&(startCountPerDevice[p->inputInt[0]]>0) )
			{ // Ok, we have an "open" device here
				returnData.buttonStates=_allDeviceData[p->inputInt[0]].buttonStates;
				returnData.accelerationX=_allDeviceData[p->inputInt[0]].accelerationX;
				returnData.accelerationY=_allDeviceData[p->inputInt[0]].accelerationY;
				returnData.accelerationZ=_allDeviceData[p->inputInt[0]].accelerationZ;
				returnData.roll=_allDeviceData[p->inputInt[0]].roll;
				returnData.pitch=_allDeviceData[p->inputInt[0]].pitch;
				returnData.batteryState=_allDeviceData[p->inputInt[0]].batteryState;
				result=1;
			}
			else
				simSetLastError(LUA_GET,"Invalid device index."); // output an error
		}
		else
			simSetLastError(LUA_GET,"Wrong argument type/size."); // output an error
	}
	else
		simSetLastError(LUA_GET,"Not enough arguments."); // output an error

	// Now we prepare the return value(s):
	if (result==-1)
	{ // We just return -1 in case of an error
		p->outputArgCount=1; // 1 return value
		p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer
		p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_int;	// The return value is an int
		p->outputArgTypeAndSize[2*0+1]=1;				// Not used (table size if the return value was a table)
		p->outputInt=(simInt*)simCreateBuffer(1*sizeof(result)); // 1 int return value
		p->outputInt[0]=result; // This is the int value we want to return
	}
	else
	{ // We return: result,buttons,acceleration(table_3),orientation(table_2),battery
		p->outputArgCount=5; // 5 return values
		p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*5*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer

		p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_int;	// The return value is an int
		p->outputArgTypeAndSize[2*0+1]=1;				// Not used (table size if the return value was a table)
		p->outputArgTypeAndSize[2*1+0]=sim_lua_arg_int;	// The return value is an int
		p->outputArgTypeAndSize[2*1+1]=1;				// Not used (table size if the return value was a table)
		p->outputArgTypeAndSize[2*2+0]=sim_lua_arg_float|sim_lua_arg_table;	// The return value is a float table
		p->outputArgTypeAndSize[2*2+1]=3;				// table size is 3
		p->outputArgTypeAndSize[2*3+0]=sim_lua_arg_float|sim_lua_arg_table;	// The return value is a float table
		p->outputArgTypeAndSize[2*3+1]=2;				// table size is 2
		p->outputArgTypeAndSize[2*4+0]=sim_lua_arg_int;	// The return value is an int
		p->outputArgTypeAndSize[2*4+1]=1;				// Not used (table size if the return value was a table)

		p->outputInt=(simInt*)simCreateBuffer(3*sizeof(int)); // 1+1+1 int return value
		p->outputInt[0]=result;
		p->outputInt[1]=int(returnData.buttonStates);
		p->outputInt[2]=int(returnData.batteryState);

		p->outputFloat=(simFloat*)simCreateBuffer(5*sizeof(float)); // 3+2 float return value
		p->outputFloat[0]=returnData.accelerationX;
		p->outputFloat[1]=returnData.accelerationY;
		p->outputFloat[2]=returnData.accelerationZ;

		p->outputFloat[3]=returnData.roll*DEGREE_TO_RAD;
		p->outputFloat[4]=returnData.pitch*DEGREE_TO_RAD;
	}

	simLockInterface(0);
}