octree.cpp

Go to the documentation of this file.

 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <stdexcept>
 
#include <boost/iostreams/device/array.hpp>
#include <boost/iostreams/stream.hpp>
#include <tesseract_common/utils.h>
#include <tinyxml2.h>
#include <octomap/OcTree.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_geometry/impl/octree.h>
#include <tesseract_common/resource_locator.h>
#include <tesseract_urdf/octree.h>
#include <tesseract_urdf/utils.h>
 
namespace tesseract_urdf
{
tesseract_geometry::Octree::Ptr parseOctree(const tinyxml2::XMLElement* xml_element,
                                            const tesseract_common::ResourceLocator& locator,
                                            tesseract_geometry::OctreeSubType shape_type,
                                            bool prune)
{
  std::string filename;
  if (tesseract_common::QueryStringAttribute(xml_element, "filename", filename) != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("Octree: Missing or failed parsing attribute 'filename'!"));
 
  tesseract_common::Resource::Ptr resource = locator.locateResource(filename);
  if (!resource || !resource->isFile())
    std::throw_with_nested(std::runtime_error("Octree: Missing resource '" + filename + "'!"));
 
  auto ot = std::make_shared<octomap::OcTree>(resource->getFilePath());
 
  if (ot == nullptr || ot->size() == 0)
    std::throw_with_nested(std::runtime_error("Octree: Error importing from '" + filename + "'!"));
 
  if (prune)
    tesseract_geometry::Octree::prune(*ot);
 
  auto geom = std::make_shared<tesseract_geometry::Octree>(ot, shape_type);
  if (geom == nullptr)
    std::throw_with_nested(std::runtime_error("Octree: Error creating octree geometry type from octomap::octree!"));
 
  return geom;
}
 
tinyxml2::XMLElement* writeOctree(const tesseract_geometry::Octree::ConstPtr& octree,
                                  tinyxml2::XMLDocument& doc,
                                  const std::string& package_path,
                                  const std::string& filename)
{
  if (octree == nullptr)
    std::throw_with_nested(std::runtime_error("Octree is nullptr and cannot be converted to XML"));
  tinyxml2::XMLElement* xml_element = doc.NewElement(OCTREE_ELEMENT_NAME.data());
 
  std::string filepath = trailingSlash(package_path) + noLeadingSlash(filename);
 
  // This copy is unfortunate, but avoiding the copy requires flowing mutability up to a lot of
  // functions and their arguments. Don't know why writeBinary is non-const anyway, but we'll live.
  std::shared_ptr<octomap::OcTree> underlying_tree = std::make_shared<octomap::OcTree>(*(octree->getOctree()));
 
  if (!underlying_tree->writeBinary(filepath))
    std::throw_with_nested(std::runtime_error("Could not write octree to file `" + filepath + "`!"));
 
  xml_element->SetAttribute("filename", makeURDFFilePath(package_path, filename).c_str());
 
  return xml_element;
}
 
}  // namespace tesseract_urdf