static simInt groupShapes(std::vector<urdfElement> &elements) { simInt n = -1; simInt *shapes = new simInt[elements.size()]; int validShapes = 0; for (unsigned int i=0; i<elements.size(); i++) { urdfElement &element = elements[i]; if (element.n!=-1) { simSetShapeColor(element.n,NULL,0,element.rgba); C7Vector frame; frame.X.set(element.xyz); frame.Q=getQuaternionFromRpy(element.rpy); C7Vector initFrame; simGetObjectPosition(element.n,-1,initFrame.X.data); C3Vector euler; simGetObjectOrientation(element.n,-1,euler.data); initFrame.Q.setEulerAngles(euler); C7Vector x(frame*initFrame); //C7Vector x(frame); simSetObjectPosition(element.n,-1,x.X.data); simSetObjectOrientation(element.n,-1,x.Q.getEulerAngles().data); shapes[validShapes++] = element.n; } } if (validShapes > 1) { n = simGroupShapes(shapes, validShapes); } else if (validShapes == 1) { n = shapes[0]; } delete shapes; return n; }
//Write void urdfLink::createLink(bool hideCollisionLinks,bool convexDecomposeNonConvexCollidables,bool createVisualIfNone,bool& showConvexDecompositionDlg) { std::string txt("Creating link '"+name+"'..."); printToConsole(txt.c_str()); //visuals.clear(); // Visuals for (int i=0; i<visuals.size(); i++) { urdfElement &visual = visuals[i]; if(!visual.meshFilename.empty()) { std::string fname(visual.meshFilename); bool exists=true; bool useAlt=false; if (!simDoesFileExist(fname.c_str())) { fname=visual.meshFilename_alt; exists=simDoesFileExist(fname.c_str()); useAlt=true; } if (!exists) printToConsole("ERROR: the mesh file could not be found."); else visual.n = simImportShape(visual.meshExtension,fname.c_str(),0,0.0001f,1.0); if (!visual.n) { if (!useAlt) txt="ERROR: failed to create the mesh '"+visual.meshFilename+"' with extension type "+boost::lexical_cast<std::string>(visual.meshExtension); else txt="ERROR: failed to create the mesh '"+visual.meshFilename+"' or '"+visual.meshFilename_alt+"' with extension type "+boost::lexical_cast<std::string>(visual.meshExtension); printToConsole(txt.c_str()); } else visual.n = scaleShapeIfRequired(visual.n,visual.mesh_scaling); } else if (!isArrayEmpty(visual.sphere_size)) visual.n = simCreatePureShape( 1,1+2+16, visual.sphere_size, mass, NULL); else if (!isArrayEmpty(visual.cylinder_size)) visual.n = simCreatePureShape( 2,1+2+16, visual.cylinder_size, mass, NULL); else if (!isArrayEmpty(visual.box_size)) visual.n = simCreatePureShape( 0,1+2+16, visual.box_size, mass, NULL); } //collisions.clear(); //mass=0.1; //collision for (int i=0; i<collisions.size(); i++) { urdfElement &collision = collisions[i]; if(!collision.meshFilename.empty()) { std::string fname(collision.meshFilename); bool exists=true; bool useAlt=false; if (!simDoesFileExist(fname.c_str())) { fname=collision.meshFilename_alt; exists=simDoesFileExist(fname.c_str()); useAlt=true; } if (!exists) printToConsole("ERROR: the mesh file could not be found"); else collision.n = simImportShape(collision.meshExtension,fname.c_str(),0,0.0001f,1.0); if (collision.n == -1) { if (!useAlt) txt="ERROR: failed to create the mesh '"+collision.meshFilename+"' with extension type "+boost::lexical_cast<std::string>(collision.meshExtension); else txt="ERROR: failed to create the mesh '"+collision.meshFilename+"' or '"+collision.meshFilename_alt+"' with extension type "+boost::lexical_cast<std::string>(collision.meshExtension); printToConsole(txt.c_str()); } else { collision.n=scaleShapeIfRequired(collision.n,collision.mesh_scaling); if (createVisualIfNone&&(visuals.size()==0)) { // We create a visual from the collision shape (before it gets morphed hereafter): simRemoveObjectFromSelection(sim_handle_all,-1); simAddObjectToSelection(sim_handle_single,collision.n); simCopyPasteSelectedObjects(); addVisual(); currentVisual().n = simGetObjectLastSelection(); } int p; int convInts[5]={1,500,200,0,0}; // 3rd value from 100 to 500 on 5/2/2014 float convFloats[5]={100.0f,30.0f,0.25f,0.0f,0.0f}; if ( convexDecomposeNonConvexCollidables&&(simGetObjectIntParameter(collision.n,3017,&p)>0)&&(p==0) ) { int aux=1+4+8+16+64; if (showConvexDecompositionDlg) aux=1+2+8+16+64; showConvexDecompositionDlg=false; simConvexDecompose(collision.n,aux,convInts,convFloats); // we generate convex shapes! } simSetObjectIntParameter(collision.n,3003,!inertiaPresent); // we make it non-static if there is an inertia simSetObjectIntParameter(collision.n,3004,1); // we make it respondable since it is a collision object } } else if (!isArrayEmpty(collision.sphere_size)) collision.n = simCreatePureShape( 1,1+2+4+8+16*(!inertiaPresent), collision.sphere_size, mass, NULL); else if (!isArrayEmpty(collision.cylinder_size)) collision.n = simCreatePureShape( 2,1+2+4+8+16*(!inertiaPresent), collision.cylinder_size, mass, NULL); else if (!isArrayEmpty(collision.box_size)) collision.n = simCreatePureShape( 0,1+2+4+8+16*(!inertiaPresent), collision.box_size, mass, NULL); } // Hack to draw COM in the collision layer /* addCollision(); currentCollision().xyz[0] = inertial_xyz[0]; currentCollision().xyz[1] = inertial_xyz[1]; currentCollision().xyz[0] = inertial_xyz[2]; currentCollision().rpy[0] = 1.5; float dummySize[3]={0.01f,0.01f,0.01f}; currentCollision().n = simCreatePureShape( 1,1+2+16, dummySize, mass, NULL); */ // Grouping collisions shapes nLinkCollision = groupShapes(collisions); // Inertia if (inertiaPresent) { C3Vector euler; if (nLinkCollision==-1) { // we do not have a collision object. Let's create a dummy collision object, since inertias can't exist on their own in V-REP: float dummySize[3]={0.01f,0.01f,0.01f}; //nLinkCollision = simCreatePureShape( 1,1+2+4, dummySize, mass, NULL); // we make it non-respondable! nLinkCollision = simCreatePureShape( 1,1+2+16, dummySize, mass, NULL); } C7Vector inertiaFrame; inertiaFrame.X.set(inertial_xyz); inertiaFrame.Q=getQuaternionFromRpy(inertial_rpy); //simSetObjectPosition(nLinkCollision,-1,inertiaFrame.X.data); //C7Vector collisionFrame; //collisionFrame.X.set(collision_xyz); //collisionFrame.Q=getQuaternionFromRpy(collision_rpy); C7Vector collisionFrame; simGetObjectPosition(nLinkCollision,-1,collisionFrame.X.data); simGetObjectOrientation(nLinkCollision,-1,euler.data); collisionFrame.Q.setEulerAngles(euler); //C4X4Matrix x((collisionFrame.getInverse()*inertiaFrame).getMatrix()); C4X4Matrix x(inertiaFrame.getMatrix()); float i[12]={x.M(0,0),x.M(0,1),x.M(0,2),x.X(0),x.M(1,0),x.M(1,1),x.M(1,2),x.X(1),x.M(2,0),x.M(2,1),x.M(2,2),x.X(2)}; simSetShapeMassAndInertia(nLinkCollision,mass,inertia,C3Vector::zeroVector.data,i); //std::cout << "Mass: " << mass << std::endl; } else { if (nLinkCollision!=-1) { std::string txt("ERROR: found a collision object without inertia data for link '"+ name+"'. Is that link meant to be static?"); printToConsole(txt.c_str()); } } if (createVisualIfNone&&(visuals.size()==0)&&(nLinkCollision!=-1)) { // We create a visual from the collision shape (meshes were handled earlier): addVisual(); urdfElement &visual = currentVisual(); simRemoveObjectFromSelection(sim_handle_all,-1); simAddObjectToSelection(sim_handle_single,nLinkCollision); simCopyPasteSelectedObjects(); visual.n=simGetObjectLastSelection(); simSetObjectIntParameter(visual.n,3003,1); // we make it static since only visual simSetObjectIntParameter(visual.n,3004,0); // we make it non-respondable since only visual } // Set the respondable mask: if (nLinkCollision!=-1) simSetObjectIntParameter(nLinkCollision,3019,0xff00); // colliding with everything except with other objects in that tree hierarchy // Grouping shapes nLinkVisual = groupShapes(visuals); // Set the names, visibility, etc.: if (nLinkVisual!=-1) { setVrepObjectName(nLinkVisual,std::string(name+"_visual").c_str()); const float specularDiffuse[3]={0.3f,0.3f,0.3f}; if (nLinkCollision!=-1) { // if we have a collision object, we attach the visual object to it, then forget the visual object C7Vector collisionFrame; C3Vector euler; simGetObjectPosition(nLinkCollision,-1,collisionFrame.X.data); simGetObjectOrientation(nLinkCollision,-1,euler.data); collisionFrame.Q.setEulerAngles(euler); C7Vector visualFrame; simGetObjectPosition(nLinkVisual,-1,visualFrame.X.data); simGetObjectOrientation(nLinkVisual,-1,euler.data); visualFrame.Q.setEulerAngles(euler); C7Vector x(collisionFrame.getInverse()*visualFrame); simSetObjectPosition(nLinkVisual,-1,x.X.data); simSetObjectOrientation(nLinkVisual,-1,x.Q.getEulerAngles().data); simSetObjectParent(nLinkVisual,nLinkCollision,0); } } if (nLinkCollision!=-1) { setVrepObjectName(nLinkCollision,std::string(name+"_respondable").c_str()); if (hideCollisionLinks) simSetObjectIntParameter(nLinkCollision,10,256); // we "hide" that object in layer 9 } }
void robot::createSensors() { std::string txt("There are "+boost::lexical_cast<std::string>(vSensors.size())+" sensors."); printToConsole(txt.c_str()); for(size_t i = 0; i < vSensors.size() ; i++) { sensor *Sensor = vSensors.at(i); if (Sensor->gazeboSpec) printToConsole("ERROR: sensor will not be created: the URDF specification is supported, but this is a Gazebo tag which is not documented as it seems."); else { if (Sensor->cameraSensorPresent) { int intParams[4]={Sensor->resolution[0],Sensor->resolution[1],0,0}; float floatParams[11]={Sensor->clippingPlanes[0],Sensor->clippingPlanes[1],60.0f*piValue/180.0f,0.2f,0.2f,0.4f,0.0f,0.0f,0.0f,0.0f,0.0f}; Sensor->nSensor=simCreateVisionSensor(1,intParams,floatParams,NULL); //Set the name: setVrepObjectName(Sensor->nSensor,std::string(name+"_camera").c_str()); } int proxSensHandle=-1; if (Sensor->proximitySensorPresent) { // Proximity sensors seem to be very very specific and not general / generic at all. How come?! I.e. a succession of ray description (with min/max distances) would do int intParams[8]={16,16,1,4,16,1,0,0}; float floatParams[15]={0.0f,0.48f,0.1f,0.1f,0.1f,0.1f,0.0f,0.02f,0.02f,30.0f*piValue/180.0f,piValue/2.0f,0.0f,0.02f,0.0f,0.0f}; proxSensHandle=simCreateProximitySensor(sim_proximitysensor_cone_subtype,sim_objectspecialproperty_detectable_all,0,intParams,floatParams,NULL); //Set the name: setVrepObjectName(proxSensHandle,std::string(Sensor->name+"_proximity").c_str()); } // the doc doesn't state if a vision and proximity sensor can be declared at the same time... if (proxSensHandle!=-1) { if (Sensor->nSensor!=-1) { Sensor->nSensorAux=proxSensHandle; simSetObjectParent(Sensor->nSensorAux,Sensor->nSensor,true); } else Sensor->nSensor=proxSensHandle; } // Find the local configuration: C7Vector sensorLocal; sensorLocal.X.set(Sensor->origin_xyz); sensorLocal.Q=getQuaternionFromRpy(Sensor->origin_rpy); C4Vector rot(0.0f,0.0f,piValue); // the V-REP sensors are rotated by 180deg around the Z-axis sensorLocal.Q=sensorLocal.Q*rot; // We attach the sensor to a link: C7Vector x; x.setIdentity(); int parentLinkIndex=getLinkPosition(Sensor->parentLink); if (parentLinkIndex!=-1) { int parentJointLinkIndex=getJointPosition(vLinks.at(parentLinkIndex)->parent); if (parentJointLinkIndex!=-1) x=vJoints.at(parentJointLinkIndex)->jointBaseFrame; } C7Vector sensorGlobal(x*sensorLocal); if (Sensor->nSensor!=-1) { simSetObjectPosition(Sensor->nSensor,-1,sensorGlobal.X.data); simSetObjectOrientation(Sensor->nSensor,-1,sensorGlobal.Q.getEulerAngles().data); } if ((parentLinkIndex!=-1)&&(Sensor->nSensor!=-1)) { if (vLinks.at(parentLinkIndex)->visuals.size()!=0) simSetObjectParent(Sensor->nSensor,vLinks.at(parentLinkIndex)->nLinkVisual,true); if (vLinks.at(parentLinkIndex)->nLinkCollision!=-1) simSetObjectParent(Sensor->nSensor,vLinks.at(parentLinkIndex)->nLinkCollision,true); } } } }
void robot::createLinks(bool hideCollisionLinks,bool convexDecomposeNonConvexCollidables,bool createVisualIfNone,bool showConvexDecompositionDlg) { std::string txt("There are "+boost::lexical_cast<std::string>(vLinks.size())+" links."); printToConsole(txt.c_str()); for(size_t i = 0; i < vLinks.size() ; i++) { urdfLink *Link = vLinks.at(i); Link->createLink(hideCollisionLinks,convexDecomposeNonConvexCollidables,createVisualIfNone,showConvexDecompositionDlg); // We attach the collision link to a joint. If the collision link isn't there, we use the visual link instead: C7Vector linkInitialConf; C7Vector linkDesiredConf; int effectiveLinkHandle=-1; if (Link->nLinkCollision!=-1) { effectiveLinkHandle=Link->nLinkCollision; if (effectiveLinkHandle != -1) { // Visual object position and orientation is already set in the Link float xyz[3] = {0,0,0}; float rpy[3] = {0,0,0}; linkDesiredConf.X.set(xyz); linkDesiredConf.Q=getQuaternionFromRpy(rpy); } } else { effectiveLinkHandle = Link->nLinkVisual; if (effectiveLinkHandle != -1) { // Visual object position and orientation is already set in the Link float xyz[3] = {0,0,0}; float rpy[3] = {0,0,0}; linkDesiredConf.X.set(xyz); linkDesiredConf.Q=getQuaternionFromRpy(rpy); } } simGetObjectPosition(effectiveLinkHandle,-1,linkInitialConf.X.data); C3Vector euler; simGetObjectOrientation(effectiveLinkHandle,-1,euler.data); linkInitialConf.Q.setEulerAngles(euler); // C7Vector trAbs(linkDesiredConf*linkInitialConf); // still local C7Vector trAbs(linkInitialConf); int parentJointIndex=getJointPosition(Link->parent); if( parentJointIndex!= -1) { joint* Joint = vJoints.at(parentJointIndex); trAbs=Joint->jointBaseFrame*trAbs; // now absolute } //set the transfrom matrix to the object simSetObjectPosition(effectiveLinkHandle,-1,trAbs.X.data); simSetObjectOrientation(effectiveLinkHandle,-1,trAbs.Q.getEulerAngles().data); } // Finally the real parenting: for(size_t i = 0; i < vJoints.size() ; i++) { int parentLinkIndex=getLinkPosition(vJoints.at(i)->parentLink); if (parentLinkIndex!=-1) { urdfLink* parentLink=vLinks[parentLinkIndex]; if (parentLink->nLinkCollision!=-1) simSetObjectParent(vJoints.at(i)->nJoint,parentLink->nLinkCollision,true); else simSetObjectParent(vJoints.at(i)->nJoint,parentLink->nLinkVisual,true); } int childLinkIndex=getLinkPosition(vJoints.at(i)->childLink); if (childLinkIndex!=-1) { urdfLink* childLink=vLinks[childLinkIndex]; if (childLink->nLinkCollision!=-1) simSetObjectParent(childLink->nLinkCollision,vJoints.at(i)->nJoint,true); else simSetObjectParent(childLink->nLinkVisual,vJoints.at(i)->nJoint,true); } } }
void robot::createJoints(bool hideJoints,bool positionCtrl) { std::string txt("There are "+boost::lexical_cast<std::string>(vJoints.size())+" joints."); printToConsole(txt.c_str()); //Set parents and childs for all the links for(size_t i = 0; i < vJoints.size() ; i++) { vLinks.at(getLinkPosition(vJoints.at(i)->parentLink))->child = vJoints.at(i)->name; vLinks.at(getLinkPosition(vJoints.at(i)->childLink))->parent = vJoints.at(i)->name; } //Create the joints for(size_t i = 0; i < vJoints.size() ; i++) { //Move the joints to the positions specifieds by the urdf file C7Vector tmp; tmp.setIdentity(); tmp.X.set(vJoints.at(i)->origin_xyz); tmp.Q=getQuaternionFromRpy(vJoints.at(i)->origin_rpy); vJoints.at(i)->jointBaseFrame=vJoints.at(i)->jointBaseFrame*tmp; //Set name jointParent to each joint int nParentLink = getLinkPosition(vJoints.at(i)->parentLink); vJoints.at(i)->parentJoint = vLinks.at(nParentLink)->parent; //Create the joint/forceSensor/dummy: if (vJoints.at(i)->jointType==-1) vJoints.at(i)->nJoint = simCreateDummy(0.02f,NULL); // when joint type was not recognized if (vJoints.at(i)->jointType==0) vJoints.at(i)->nJoint = simCreateJoint(sim_joint_revolute_subtype,sim_jointmode_force,2,NULL,NULL,NULL); if (vJoints.at(i)->jointType==1) vJoints.at(i)->nJoint = simCreateJoint(sim_joint_prismatic_subtype,sim_jointmode_force,2,NULL,NULL,NULL); if (vJoints.at(i)->jointType==2) vJoints.at(i)->nJoint = simCreateJoint(sim_joint_spherical_subtype,sim_jointmode_force,2,NULL,NULL,NULL); if (vJoints.at(i)->jointType==3) vJoints.at(i)->nJoint = simCreateJoint(sim_joint_revolute_subtype,sim_jointmode_force,2,NULL,NULL,NULL); if (vJoints.at(i)->jointType==4) { // when joint type is "fixed" int intParams[5]={1,4,4,0,0}; float floatParams[5]={0.02f,1.0f,1.0f,0.0f,0.0f}; vJoints.at(i)->nJoint = simCreateForceSensor(0,intParams,floatParams,NULL); } if ( (vJoints.at(i)->jointType==0)||(vJoints.at(i)->jointType==1) ) { float interval[2]={vJoints.at(i)->lowerLimit,vJoints.at(i)->upperLimit-vJoints.at(i)->lowerLimit}; simSetJointInterval(vJoints.at(i)->nJoint,0,interval); if (vJoints.at(i)->jointType==0) { // revolute simSetJointForce(vJoints.at(i)->nJoint,vJoints.at(i)->effortLimitAngular); simSetObjectFloatParameter(vJoints.at(i)->nJoint,2017,vJoints.at(i)->velocityLimitAngular); } else { // prismatic simSetJointForce(vJoints.at(i)->nJoint,vJoints.at(i)->effortLimitLinear); simSetObjectFloatParameter(vJoints.at(i)->nJoint,2017,vJoints.at(i)->velocityLimitLinear); } // We turn the position control on: if (positionCtrl) { simSetObjectIntParameter(vJoints.at(i)->nJoint,2000,1); simSetObjectIntParameter(vJoints.at(i)->nJoint,2001,1); } } //Set the name: setVrepObjectName(vJoints.at(i)->nJoint,vJoints.at(i)->name.c_str()); if (hideJoints) simSetObjectIntParameter(vJoints.at(i)->nJoint,10,512); // layer 10 } //Set positions to joints from the 4x4matrix for(size_t i = 0; i < vJoints.size() ; i++) { simSetObjectPosition(vJoints.at(i)->nJoint,-1,vJoints.at(i)->jointBaseFrame.X.data); simSetObjectOrientation(vJoints.at(i)->nJoint,-1 ,vJoints.at(i)->jointBaseFrame.Q.getEulerAngles().data); } //Set joint parentship between them (thes parentship will be remove before adding the joints) for(size_t i = 0; i < vJoints.size() ; i++) { int parentJointIndex=getJointPosition(vJoints.at(i)->parentJoint); if ( parentJointIndex!= -1) { simInt nParentJoint = vJoints.at(parentJointIndex)->nJoint; simInt nJoint = vJoints.at(i)->nJoint; simSetObjectParent(nJoint,nParentJoint,false); } } //Delete all the partnership without moving the joints but after doing that update the transform matrix for(size_t i = 0; i < vJoints.size() ; i++) { C4X4Matrix tmp; simGetObjectPosition(vJoints.at(i)->nJoint,-1,tmp.X.data); C3Vector euler; simGetObjectOrientation(vJoints.at(i)->nJoint,-1,euler.data); tmp.M.setEulerAngles(euler); vJoints.at(i)->jointBaseFrame = tmp; simInt nJoint = vJoints.at(i)->nJoint; simSetObjectParent(nJoint,-1,true); } for(size_t i = 0; i < vJoints.size() ; i++) { C4X4Matrix jointAxisMatrix; jointAxisMatrix.setIdentity(); C3Vector axis(vJoints.at(i)->axis); C3Vector rotAxis; float rotAngle=0.0f; if (axis(2)<1.0f) { if (axis(2)<=-1.0f) rotAngle=3.14159265359f; else rotAngle=acosf(axis(2)); rotAxis(0)=-axis(1); rotAxis(1)=axis(0); rotAxis(2)=0.0f; rotAxis.normalize(); C7Vector m(jointAxisMatrix); float alpha=-atan2(rotAxis(1),rotAxis(0)); float beta=atan2(-sqrt(rotAxis(0)*rotAxis(0)+rotAxis(1)*rotAxis(1)),rotAxis(2)); C7Vector r; r.X.clear(); r.Q.setEulerAngles(0.0f,0.0f,alpha); m=r*m; r.Q.setEulerAngles(0.0f,beta,0.0f); m=r*m; r.Q.setEulerAngles(0.0f,0.0f,rotAngle); m=r*m; r.Q.setEulerAngles(0.0f,-beta,0.0f); m=r*m; r.Q.setEulerAngles(0.0f,0.0f,-alpha); m=r*m; jointAxisMatrix=m.getMatrix(); } C4Vector q((vJoints.at(i)->jointBaseFrame*jointAxisMatrix).Q); simSetObjectOrientation(vJoints.at(i)->nJoint,-1,q.getEulerAngles().data); } }
// 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); }