У меня есть два объекта. Первый объект - это мой робот, который я хочу представить в виде шпере, а второй объект - это препятствие, имеющее неконструктивную форму. Я хочу изобразить форму препятствия октодеревом.
Как я могу использовать api fcl для проверки столкновения между этими двумя объектами (true или false) с помощью библиотек fcl с использованием api из вики ROS? Давая, что робот движется.
Кроме того, препятствие обнаруживается с помощью данных лазерного сканирования ?? Как его залить в объект октодерева ??
если я создал чистый объект вроде
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);
Итак, есть предложения ???