octomap.cpp

Go to the documentation of this file.

 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <stdexcept>
 
#include <tesseract_common/utils.h>
#include <tinyxml2.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_geometry/impl/octree.h>
#include <tesseract_urdf/octomap.h>
#include <tesseract_urdf/octree.h>
#include <tesseract_common/resource_locator.h>
#include <tesseract_urdf/utils.h>
 
#ifdef TESSERACT_PARSE_POINT_CLOUDS
#include <tesseract_urdf/point_cloud.h>
#endif
 
namespace tesseract_urdf
{
tesseract_geometry::Octree::Ptr parseOctomap(const tinyxml2::XMLElement* xml_element,
                                             const tesseract_common::ResourceLocator& locator,
                                             const bool /*visual*/)
{
  std::string shape_type;
  if (tesseract_common::QueryStringAttribute(xml_element, "shape_type", shape_type) != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("Octomap: Missing or failed parsing attribute 'shape_type'!"));
 
  tesseract_geometry::OctreeSubType sub_type{ tesseract_geometry::OctreeSubType::BOX };
  if (shape_type == "box")
    sub_type = tesseract_geometry::OctreeSubType::BOX;
  else if (shape_type == "sphere_inside")
    sub_type = tesseract_geometry::OctreeSubType::SPHERE_INSIDE;
  else if (shape_type == "sphere_outside")
    sub_type = tesseract_geometry::OctreeSubType::SPHERE_OUTSIDE;
  else
    std::throw_with_nested(std::runtime_error("Octomap: Invalid sub shape type, must be 'box', 'sphere_inside', or "
                                              "'sphere_outside'!"));
 
  bool prune = false;
  xml_element->QueryBoolAttribute("prune", &prune);
 
  const tinyxml2::XMLElement* octree_element = xml_element->FirstChildElement(OCTREE_ELEMENT_NAME.data());
  if (octree_element != nullptr)
  {
    try
    {
      return parseOctree(octree_element, locator, sub_type, prune);
    }
    catch (...)
    {
      std::throw_with_nested(std::runtime_error("Octomap:"));
    }
  }
 
#ifdef TESSERACT_PARSE_POINT_CLOUDS
  const tinyxml2::XMLElement* pcd_element = xml_element->FirstChildElement(POINT_CLOUD_ELEMENT_NAME.data());
  if (pcd_element != nullptr)
  {
    try
    {
      return parsePointCloud(pcd_element, locator, sub_type, prune);
    }
    catch (...)
    {
      std::throw_with_nested(std::runtime_error("Octomap:"));
    }
  }
#endif
 
  std::throw_with_nested(std::runtime_error("Octomap: Missing element '" + std::string(OCTREE_ELEMENT_NAME) + "' or '" +
                                            std::string(POINT_CLOUD_ELEMENT_NAME) + "'; must define one!"));
}
 
tinyxml2::XMLElement* writeOctomap(const std::shared_ptr<const tesseract_geometry::Octree>& 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(OCTOMAP_ELEMENT_NAME.data());
 
  std::string type_string;
  if (octree->getSubType() == tesseract_geometry::OctreeSubType::BOX)
    type_string = "box";
  else if (octree->getSubType() == tesseract_geometry::OctreeSubType::SPHERE_INSIDE)
    type_string = "sphere_inside";
  else if (octree->getSubType() == tesseract_geometry::OctreeSubType::SPHERE_OUTSIDE)
    type_string = "sphere_outside";
  else
    std::throw_with_nested(std::runtime_error("Octree subtype is invalid and cannot be converted to XML"));
  xml_element->SetAttribute("shape_type", type_string.c_str());
 
  xml_element->SetAttribute("prune", octree->getPruned());
 
  try
  {
    tinyxml2::XMLElement* xml_octree = writeOctree(octree, doc, package_path, filename);
    xml_element->InsertEndChild(xml_octree);
  }
  catch (...)
  {
    std::throw_with_nested(std::runtime_error("Octomap: Could not write octree to file"));
  }
 
  return xml_element;
}
 
}  // namespace tesseract_urdf