0

私は2つのオブジェクトを持っています。最初のオブジェクトは、球として表現したい私のロボットで、2 番目のオブジェクトは、形状が不明な障害物です。障害物の形状を八分木で表現したい。

ROS wiki の api を使用して fcl ライブラリを使用して、これら 2 つのオブジェクト間の衝突 (true または false) をチェックするために fcl の api を使用するにはどうすればよいですか? ロボットが動いているとします。

また、レーザースキャンデータを使って障害物を検知?? octreeオブジェクトにそれを埋める方法??

次のようなシェアオブジェクトを作成した場合

   boost::shared_ptr<Sphere> Shpere0(new Sphere(1));

この球体の中心がロボットの中心であることをどのように特定できますか?

編集：

次のコードを書きましたが、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);

それで、何か提案はありますか???

4

1 に答える 1

0

これを確認してください: https://github.com/kuri-kustar/laser_collision_detection/blob/master/src/laser_obstacle_detect.cpp#L135-L228 このコードがガイドになります。

于 2015-05-12T13:04:53.203 に答える