// 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) }
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); }
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); }
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); }
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); }
// 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); }
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); } }
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); }
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); }
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); }
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); }
void CBubbleRobDialog::OnOK() { simLockInterface(1); simLockInterface(0); }
void CBubbleRobDialog::OnCancel() { // We closed the dialog simLockInterface(1); setVisible(false); simLockInterface(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); }