Method openFile

octomap_server/src/octomap_server.cpp:342–398

}

bool OctomapServer::openFile(const std::string & filename)
{
  if (filename.length() <= 3) {
    return false;
  }

  std::string suffix = filename.substr(filename.length() - 3, 3);
  if (suffix == ".bt") {
    if (!octree_->readBinary(filename)) {
      return false;
    }
  } else if (suffix == ".ot") {
    std::unique_ptr<octomap::AbstractOcTree> tree{octomap::AbstractOcTree::read(filename)};
    if (!tree) {
      return false;
    }
    octree_ = std::unique_ptr<OcTreeT>(dynamic_cast<OcTreeT *>(tree.release()));
    if (!octree_) {
      RCLCPP_ERROR(
        get_logger(),
        "Could not read OcTree in file, currently there are no other types supported in .ot");
      return false;
    }

  } else {
    return false;
  }

  RCLCPP_INFO(
    get_logger(), "Octomap file %s loaded (%zu nodes).", filename.c_str(),
    octree_->size());

  tree_depth_ = octree_->getTreeDepth();
  max_tree_depth_ = tree_depth_;
  res_ = octree_->getResolution();
  gridmap_.info.resolution = res_;
  double min_x{};
  double min_y{};
  double min_z{};
  double max_x{};
  double max_y{};
  double max_z{};
  octree_->getMetricMin(min_x, min_y, min_z);
  octree_->getMetricMax(max_x, max_y, max_z);

  update_bbox_min_[0] = octree_->coordToKey(min_x);
  update_bbox_min_[1] = octree_->coordToKey(min_y);
  update_bbox_min_[2] = octree_->coordToKey(min_z);

  update_bbox_max_[0] = octree_->coordToKey(max_x);
  update_bbox_max_[1] = octree_->coordToKey(max_y);
  update_bbox_max_[2] = octree_->coordToKey(max_z);

  publishAll(now());

  return true;
}