How to receive Octree?
Hello,
I am currently creating an Octomap through the Octomap Server. What I want to do next is to receive the octree for detecting obstacles while the octomap is being created. I've tried this:
octomap::OcTree* octree = NULL;
serv_name = "/octomap_binary";
while((nh_.ok() && !ros::service::call(serv_name, req, resp)) || resp.map.data.size()==0)
{
ROS_WARN("Request to %s failed; trying again...", nh_.resolveName(serv_name).c_str());
usleep(1000000);
}
//Receive Octree
msg_octree = octomap_msgs::msgToMap(resp.map);
if (msg_octree)
//Create object of type octomap::OcTree*
octree = dynamic_cast<octomap::OcTree*>(msg_octree);
if (octree)
{
ROS_INFO("%s: Octomap received %zu nodes, %f m of resolution", ros::this_node::getName().c_str(), octree->size(), octree->getResolution());
tree_fcl = (new fcl::OcTree(std::shared_ptr<const octomap::OcTree>(octree)));
} else
ROS_ERROR("Error reading OcTree from stream");
Which works! This piece of code is inside a callback which loops with ros::spinOnce()
. As this is the way it is done inside the octomap_server files, is it the best way for me to receive the Octree? Because while the vehicle is scanning the environment, the map keeps increasing, to the point where I am receiving more than 2 million nodes at once!!!.
Instead of updating the current Octree, I am receiving the whole Octree every time it loops through that code, right? My problem is that the code starts getting really slow. Is there a better way for me to receive the octree where I keep updating it instead of receiving all of it every time (with 2+ million nodes)?
Thank you in advance.