void sonarCallback(const sensor_msgs::PointCloud::ConstPtr& msg)
{
double pf_Start = ros::Time::now().toSec();
std::cout << "I was called : (ms)" << (pf_Start - lastTimeCalled)*1000 << std::endl ;
lastTimeCalled = pf_Start;
obstacles_positions_current.clear();
for(int i=0; i< msg->points.size(); ++i)
{
Eigen::Vector3d obstacle(msg->points[i].x,msg->points[i].y,0.0);
if(obstacle.norm()<laser_max_distance-0.01)
//if((obstacle.norm()>robot_radius)&&(obstacle.norm()<laser_max_distance-0.01)) // check if measurement is between the laser range and the robot
{
//ROS_INFO_STREAM("INSIDE THE LIMITS:"<<obstacle.norm());
obstacles_positions_current.push_back(obstacle);
}
}
if(!init_flag)
{
init_flag=true;
obstacles_positions_previous=obstacles_positions_current;
return;
}
//ROS_INFO_STREAM("obstacles:" << obstacles_positions.size());
//ROS_INFO("I heard sensor data : [%f, %f , %f]", msg->points[0].x , msg->points[0].y , msg->points[0].z );
computeForceField();
// feedbackMaster();
//feedbackSlave();
obstacles_positions_previous=obstacles_positions_current;
double pf_End = ros::Time::now().toSec();
//std::cout << "Call back took : (ms)" << (pf_End - pf_Start)*1000 << std::endl ;
}
// Computes acceleration to every control point of the jello cube,
// which is in state given by 'jello'.
// Returns result in array 'a'.
void computeAcceleration(struct world * jello, struct point a[8][8][8])
{
// for you to implement ...
int x = 0, y = 0, z = 0; // Index variables for looping through all 512 points
point typeP, curP; // typeP = one of the 32 nodes to which the curP point is connected
int collided = 0;
memset( (void*)a, 0, sizeof(a));
memset( (void*)&typeP, 0, sizeof(typeP));
memset( (void*)&curP, 0, sizeof(curP));
memset( (void*)force, 0, sizeof(force));
// FORCE FIELD COMPUTATION
if(ActivateFF)
{
// Check for force field
if(jello->resolution)
{
// compute the force field cube size and store it in the global variable
forceFieldCubeSize = ((double)4)/jello->resolution;
noOfCubesInRow = (int)(((double)jello->resolution) / forceFieldCubeSize);
// Compute the effect of force field and update the forces due to it on all the nodes
computeForceField(jello);
}
}
// COLLISION DETECTION AND RESPONSE
for(z = 0; z < 8; z++)
{
for(y = 0; y < 8; y++)
{
for(x = 0; x < 8; x++)
{
memset( (void*)&curP, 0, sizeof(curP));
curP.x = x;
curP.y = y;
curP.z = z;
// Checks whether any of the nodes is colliding with the bounding box and Sphere
checkForCollision(curP, jello);
}
}
}
for(z = 0; z < 8; z++)
{
for(y = 0; y < 8; y++)
{
for(x = 0; x < 8; x++)
{
curP.x = x;
curP.y = y;
curP.z = z;
// Check for all the springs possible = 32
// SAME HORIZONTAL LAYER
//-----------------------------------------------------------------
// TYPE 1 -> (x+1, y, z) structural spring
typeP.x = x + 1;
typeP.y = y;
typeP.z = z;
updateForce(curP, typeP, jello, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 2 -> (x-1, y, z)
typeP.x = x - 1;
typeP.y = y;
typeP.z = z;
updateForce(curP, typeP, jello, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 3 -> (x, y, z+1)
typeP.x = x;
typeP.y = y;
typeP.z = z + 1;
updateForce(curP, typeP, jello, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 4 -> (x, y, z-1)
typeP.x = x;
typeP.y = y;
typeP.z = z - 1;
updateForce(curP, typeP, jello, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 5 -> (x+1, y, z+1)
typeP.x = x + 1;
typeP.y = y;
typeP.z = z + 1;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 6 -> (x+1, y, z-1)
typeP.x = x + 1;
typeP.y = y;
typeP.z = z - 1;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 7 -> (x-1, y, z+1)
typeP.x = x - 1;
typeP.y = y;
typeP.z = z + 1;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 8 -> (x-1, y, z-1)
typeP.x = x - 1;
typeP.y = y;
typeP.z = z - 1;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// BOTTOM LAYER
//-----------------------------------------------------------------
// TYPE 9 -> (x+1, y+1, z)
typeP.x = x + 1;
typeP.y = y + 1;
typeP.z = z;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 10 -> (x-1, y+1, z)
typeP.x = x - 1;
typeP.y = y + 1;
typeP.z = z;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 11 -> (x, y+1, z+1)
typeP.x = x;
typeP.y = y + 1;
typeP.z = z + 1;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 12 -> (x, y+1, z-1)
typeP.x = x;
typeP.y = y + 1;
typeP.z = z - 1;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 13 -> (x+1, y+1, z+1)
typeP.x = x + 1;
typeP.y = y + 1;
typeP.z = z + 1;
updateForce(curP, typeP, jello, SHEARDIAGONAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 14 -> (x+1, y+1, z-1)
typeP.x = x + 1;
typeP.y = y + 1;
typeP.z = z - 1;
updateForce(curP, typeP, jello, SHEARDIAGONAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 15 -> (x-1, y+1, z+1)
typeP.x = x - 1;
typeP.y = y + 1;
typeP.z = z + 1;
updateForce(curP, typeP, jello, SHEARDIAGONAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 16 -> (x-1, y+1, z-1)
typeP.x = x - 1;
typeP.y = y + 1;
typeP.z = z - 1;
updateForce(curP, typeP, jello, SHEARDIAGONAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 17 -> (x, y+1, z)
typeP.x = x;
typeP.y = y + 1;
typeP.z = z;
updateForce(curP, typeP, jello, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TOP LAYER
// TYPE 18 -> (x+1, y-1, z)
typeP.x = x + 1;
typeP.y = y - 1;
typeP.z = z;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 19 -> (x-1, y-1, z)
typeP.x = x - 1;
typeP.y = y - 1;
typeP.z = z;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 20 -> (x, y-1, z+1)
typeP.x = x;
typeP.y = y - 1;
typeP.z = z + 1;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 21 -> (x, y-1, z-1)
typeP.x = x;
typeP.y = y - 1;
typeP.z = z - 1;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 22 -> (x+1, y-1, z+1)
typeP.x = x + 1;
typeP.y = y - 1;
typeP.z = z + 1;
updateForce(curP, typeP, jello, SHEARDIAGONAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 23 -> (x+1, y-1, z-1)
typeP.x = x + 1;
typeP.y = y - 1;
typeP.z = z - 1;
updateForce(curP, typeP, jello, SHEARDIAGONAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 24 -> (x-1, y-1, z+1)
typeP.x = x - 1;
typeP.y = y - 1;
typeP.z = z + 1;
updateForce(curP, typeP, jello, SHEARDIAGONAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 25 -> (x-1, y-1, z-1)
typeP.x = x - 1;
typeP.y = y - 1;
typeP.z = z - 1;
updateForce(curP, typeP, jello, SHEARDIAGONAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 26 -> (x, y-1, z)
typeP.x = x;
typeP.y = y - 1;
typeP.z = z;
updateForce(curP, typeP, jello, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// BEND SPRINGS
//-----------------------------------------------------------------
// TYPE 27 -> (x+2, y, z)
typeP.x = x + 2;
typeP.y = y;
typeP.z = z;
updateForce(curP, typeP, jello, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 28 -> (x-2, y, z)
typeP.x = x - 2;
typeP.y = y;
typeP.z = z;
updateForce(curP, typeP, jello, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 29 -> (x, y+2, z)
typeP.x = x;
typeP.y = y + 2;
typeP.z = z;
updateForce(curP, typeP, jello, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 30 -> (x, y-2, z)
typeP.x = x;
typeP.y = y - 2;
typeP.z = z;
updateForce(curP, typeP, jello, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 31 -> (x, y, z+2)
typeP.x = x;
typeP.y = y;
typeP.z = z + 2;
updateForce(curP, typeP, jello, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 32 -> (x, y, z-2)
typeP.x = x;
typeP.y = y;
typeP.z = z - 2;
updateForce(curP, typeP, jello, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// Check if the point obtained in this type is valid -> inside cube check
// blah blah
// check if the point obtained in this type is already parsed -> check for the x,y,z loop comparison
// blah blah
// if not parsed then find the hooks force and damping force.
// add the force value to both the end points, i.e; current looping point and the type obtained point
} // for indexX
} // for indexY
} // for indexZ
memset( (void*)a, 0, sizeof(a));
for(z = 0; z < 8; z++)
{
for(y = 0; y < 8; y++)
{
for(x = 0; x < 8; x++)
{
a[x][y][z].x = force[x][y][z].x / jello->mass ;
a[x][y][z].y = force[x][y][z].y / jello->mass ;
a[x][y][z].z = force[x][y][z].z / jello->mass ;
}
}
}
}
