search

ethz-asl / nbvplanner

A real-time capable exploration and inspection path planner (next best view planning)
342 stars 120 forks source link

insertPointCloud takes forever after (most probably) ros octomap update #16

Closed bingjui closed 6 years ago

bingjui commented 7 years ago

@birchera nbvplanner worked great few months ago but after I update some ros package which I believe most likely due to octomap related packages, the planner gets stuck (it can still work) when running flat_exploration example with firefly. After some invertigation,

I think it is most likely due #to void OctomapWorld::insertPointcloudIntoMapImpl() ... octomap::KeySet free_cells, occupied_cells; for (pcl::PointCloud::const_iterator it = cloud->begin(); it != cloud->end(); ++it) { const octomap::point3d p_Gpoint(it->x, it->y, it->z); // First, check if we've already checked this. octomap::OcTreeKey key = octree->coordToKey(p_G_point);

if (occupied_cells.find(key) == occupied_cells.end()) {
  // Check if this is within the allowed sensor range.
  castRay(p_G_sensor, p_G_point, &free_cells, &occupied_cells);
}

} ...

The above for loop takes unreasonably long time (more than a minute) to complete for each pointcloud message callback starting from the first pointcloud msg. As you can see, it mostly calls octomap API.

Can you please take a look into this problem? Thank you very much.

It is tested with both kinetic and indigo setup with Ubuntu 16.xx and 14.xx respectively.

birchera commented 7 years ago

Thanks for reporting. I will have a look. I am quite busy right now and do not know yet when I will get to it though.

tongbaiming commented 7 years ago

@bingjui @birchera ,I have the same problem, and I change the api to insert point cloud, then it seems to work. std::cout << "I am in OctomapWorld::insertPointcloudIntoMapImpl(), flag 1" << std::endl; octomap::Pointcloud cloud_octo; for (pcl::PointCloud::const_iterator it = cloud->begin(); it != cloud->end(); ++it) { const octomap::point3d p_G_point(it->x, it->y, it->z); cloud_octo.push_back( p_Gpoint ); /* // First, check if we've already checked this. octomap::OcTreeKey key = octree->coordToKey(p_G_point); if (occupied_cells.find(key) == occupied_cells.end()) { // Check if this is within the allowed sensor range. castRay(p_G_sensor, p_G_point, &free_cells, &occupied_cells); } */ } // Apply the new free cells and occupied cells from //updateOccupancy(&free_cells, &occupiedcells); std::cout << "I am in OctomapWorld::insertPointcloudIntoMapImpl(), flag 2" << std::endl; octree->insertPointCloud( cloud_octo, p_Gsensor); octree->updateInnerOccupancy();

birchera commented 6 years ago

Hi @bingjui and @tongbaiming, I have looked into the issue (sorry for taking so long) and found issue https://github.com/ethz-asl/volumetric_mapping/issues/64, which could be related?

Anyway, I have updated the volumetric mapping submodule to latest master and am currently working on fixing some remaining porting issues. Will come back shortly once I have a potentially fixed branch to test.

tongbaiming commented 6 years ago

Thank you, @birchera , the way you said in email send to me resolves the problems. "Updating the volumetric mapping to latest master to pull in the resizing fix."