使用fcl和ros wiki对八叉树和常规对象进行碰撞检测

时间:2015-02-12 08:22:33

标签: collision-detection collision ros octree fcl

我有两个对象。第一个对象是我的机器人,我想将其表示为shpere,第二个对象是具有未知形状的障碍物。我想用八叉树来表示障碍物的形状。

如何使用fcl库中的api使用来自ROS wiki的api来检查这两个对象(true或false)之间的冲突?给机器人移动。

此外,使用激光扫描数据检测障碍物?如何填充八叉树对象?

如果我创建了像

这样的对象
   boost::shared_ptr<Sphere> Shpere0(new Sphere(1));

我怎样才能说明这个球体的中心是机器人的中心?

编辑:

我写了下面的代码,但我不知道如何填充八叉树

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

所以,任何建议???

1 个答案:

答案 0 :(得分:0)