generating boxes from octomap::octree error
I am using the fcl where I have an octree and I want to generate boxes from an octree function I used the following code:
octomap::OcTree* st_tree = new octomap::OcTree(0.1);
octomap::Pointcloud st_cld;
// I fill the tree
for(int i = 0;i<msg.points.size();i++){
point3d endpoint((float) msg.points[i].x,(float) msg.points[i].y,(float) msg.points[i].z);
st_cld.push_back(endpoint);
}
point3d origin(0.0,0.0,0.0);
st_tree->insertPointCloud(st_cld,origin);
st_tree->updateInnerOccupancy();
st_tree->writeBinary("static_occ.bt");
std::vector<CollisionObject*> boxes;
generateBoxesFromOctomap(boxes, *st_tree);
The function is defined as the following:
void generateBoxesFromOctomap(std::vector<CollisionObject*>& boxes, octomap::OcTree& tree)
{
std::vector<boost::array<FCL_REAL, 6> > boxes_ = tree.toBoxes();
for(std::size_t i = 0; i < boxes_.size(); ++i)
{
FCL_REAL x = boxes_[i][0];
FCL_REAL y = boxes_[i][1];
FCL_REAL z = boxes_[i][2];
FCL_REAL size = boxes_[i][3];
FCL_REAL cost = boxes_[i][4];
FCL_REAL threshold = boxes_[i][5];
Box* box = new Box(size, size, size);
box->cost_density = cost;
box->threshold_occupied = threshold;
CollisionObject* obj = new CollisionObject(boost::shared_ptr<CollisionGeometry>(box), Transform3f(Vec3f(x, y, z)));
boxes.push_back(obj);
}
std::cout << "boxes size: " << boxes.size() << std::endl;
}
I get the following error:
error: ‘class octomap::OcTree’ has no member named ‘toBoxes’
std::vector<boost::array<FCL_REAL, 6> > boxes_ = tree.toBoxes();
But I included the all the required libraries??
How can I solve it ?