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