I have two objects. The first object is my robot which I want to represent it as a shpere and the second object is the obstacle that has unkonwn shape. I want to represent the shape of the obstacle with an octree.
How can I use the api of the fcl to check collision between these two objects ( true or false) using fcl libraries using the api from ROS wiki? Giving that the robot is moving.
Also, the obstacle is detected using laser scan data?? How to fill it in octree object ??
if i created a shere object like
boost::shared_ptr<Sphere> Shpere0(new Sphere(1));
how can i spacify that the center of this sphere is the center of the robot ?
EDIT:
I wrote the following code but I dont know how to fill the octree
boost::shared_ptr<Sphere> Shpere0(new Sphere(1));
OcTree* tree = new OcTree(boost::shared_ptr<const octomap::OcTree>(generateOcTree()));
// GJKSolver_indep solver;
GJKSolver_libccd solver;
Vec3f contact_points;
FCL_REAL penetration_depth;
Vec3f normal;
Transform3f tf0, tf1;
tf0.setIdentity();
// is this the postion in x,y and x
tf0.setTranslation(Vec3f(robotpose(0),robotpose(1),robotpose(2)));
tf0.setQuatRotation(Quaternion3f(0, 0, 0, 0));
// HOW TO FILL the OCTREE HERE with point cloud data ???
tf1.setIdentity();
bool res = solver.shapeIntersect(*Shpere0, tf0, *box1, tf1, &contact_points, &penetration_depth, &normal);
cout << "contact points: " << contact_points << endl;
cout << "pen depth: " << penetration_depth << endl;
cout << "normal: " << normal << endl;
cout << "result: " << res << endl;
static const int num_max_contacts = std::numeric_limits<int>::max();
static const bool enable_contact = true;
fcl::CollisionResult result;
fcl::CollisionRequest request(num_max_contacts, enable_contact);
CollisionObject co0(Shpere0, tf0);
CollisionObject co1(tree, tf1);
bool res_1 = fcl::collide(&co0, &co1, request, result);
So, Any suggestions ???
