Method insertScan

octomap_server/src/octomap_server.cpp:498–623

}

void OctomapServer::insertScan(
  const tf2::Vector3 & sensor_origin_tf, const PCLPointCloud & ground,
  const PCLPointCloud & nonground)
{
  const auto sensor_origin = octomap::pointTfToOctomap(sensor_origin_tf);

  if (!octree_->coordToKeyChecked(sensor_origin, update_bbox_min_) ||
    !octree_->coordToKeyChecked(sensor_origin, update_bbox_max_))
  {
    RCLCPP_ERROR_STREAM(get_logger(), "Could not generate Key for origin " << sensor_origin);
  }

  // instead of direct scan insertion, compute update to filter ground:
  octomap::KeySet free_cells, occupied_cells;
  // insert ground points only as free:
  for (PCLPointCloud::const_iterator it = ground.begin(); it != ground.end(); ++it) {
    octomap::point3d point(it->x, it->y, it->z);
    // maxrange check
    if ((max_range_ > 0.0) && ((point - sensor_origin).norm() > max_range_) ) {
      point = sensor_origin + (point - sensor_origin).normalized() * max_range_;
    }

    // only clear space (ground points)
    if (octree_->computeRayKeys(sensor_origin, point, key_ray_)) {
      free_cells.insert(key_ray_.begin(), key_ray_.end());
    }

    octomap::OcTreeKey end_key;
    if (octree_->coordToKeyChecked(point, end_key)) {
      updateMinKey(end_key, update_bbox_min_);
      updateMaxKey(end_key, update_bbox_max_);
    } else {
      RCLCPP_ERROR_STREAM(get_logger(), "Could not generate Key for endpoint " << point);
    }
  }

  // all other points: free on ray, occupied on endpoint:
  for (PCLPointCloud::const_iterator it = nonground.begin(); it != nonground.end(); ++it) {
    octomap::point3d point(it->x, it->y, it->z);
    // maxrange check
    if ((max_range_ < 0.0) || ((point - sensor_origin).norm() <= max_range_) ) {
      // free cells
      if (octree_->computeRayKeys(sensor_origin, point, key_ray_)) {
        free_cells.insert(key_ray_.begin(), key_ray_.end());
      }
      // occupied endpoint
      octomap::OcTreeKey key;
      if (octree_->coordToKeyChecked(point, key)) {
        occupied_cells.insert(key);

        updateMinKey(key, update_bbox_min_);
        updateMaxKey(key, update_bbox_max_);

#ifdef COLOR_OCTOMAP_SERVER  // NB: Only read and interpret color if it's an occupied node
        octree_->averageNodeColor(it->x, it->y, it->z, /*r=*/ it->r, /*g=*/ it->g, /*b=*/ it->b);
#endif
      }
    } else {  // ray longer than maxrange