ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How to receive Octree?

asked 2018-06-27 19:39:58 -0500

jpde.lopes gravatar image

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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-07-06 05:07:13 -0500

jpde.lopes gravatar image

I found this link which answers my question.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2018-06-27 19:39:58 -0500

Seen: 566 times

Last updated: Jul 06 '18