octomap insertRay is slow

edit

Hi, everyone,

I'm working on a project on simple sense-and-avoid for a mobile robot equipped with a LiDAR scanner. My goal is for an untrained operator to manually RC the robot, and if the robot detect obstacle with unsafe range in the moving direction, it'll stop.

After a bit research, I decided to use the octomap package developped by Hornung to build the 3D occupancy grid in real-time. Then I query the occupancy-grid if the cells in my moving direction if free or not to make decision.

To achieve that, I wrote a simple code which subscribes to the sensor_msgs/Pointcloud2 topic published by the LiDAR driver, and insert the point cloud to the OcTree every time one cloud is available. Here is how I inserted the point cloud in the callback function of the point cloud topic:

  for(pcl::PointCloud< pcl::PointXYZ >::const_iterator itr = myCloud.begin(); itr != myCloud.end(); ++itr)
  {
    point3d endpoint(itr->x, itr->y, itr->z);
    m_octree->insertRay(sensorOrigin, endpoint);
  }
  my_pub1.publish(debug_message);

The problem is that this code cannot run in real time due to insertRay(). I found this by publishing a debug_message every time after the insertion as an implication of the insertion frequency and using "rostopic hz" to compare the frequencies of my point cloud and the debug_message. The difference is drastic: the point cloud topic is published at 750 Hz, with about 300 points in each cloud, while my debug_message frequency is only about 60Hz.

However, if instead of using insertRay, I use "m_octree->updateNode(endpoint, true);", the frequency of insertion is boost to 750 Hz.

The problem is, using updateNode, I only know that the cells containing the points are occupied, but the space between my sensor and the points is free space.

Can someone give me some help of how to solve this problem: either speed up "insertRay" to directly get information of free cells, or use the fast "updateNode" and then figure out the free cells by myself? (One solution in my mind is to initialize the octomap with all cells free. )

Thank you so much in advance!

Jerry