cone.cpp

Go to the documentation of this file.

 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <stdexcept>
 
#include <tinyxml2.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_geometry/impl/cone.h>
#include <tesseract_urdf/cone.h>
#include <tesseract_urdf/utils.h>
 
namespace tesseract_urdf
{
tesseract_geometry::Cone::Ptr parseCone(const tinyxml2::XMLElement* xml_element)
{
  double r{ 0 }, l{ 0 };
  if (xml_element->QueryDoubleAttribute("length", &(l)) != tinyxml2::XML_SUCCESS || !(l > 0))
    std::throw_with_nested(std::runtime_error("Cone: Missing or failed parsing attribute 'length'!"));
 
  if (xml_element->QueryDoubleAttribute("radius", &(r)) != tinyxml2::XML_SUCCESS || !(r > 0))
    std::throw_with_nested(std::runtime_error("Cone: Missing or failed parsing attribute 'radius'!"));
 
  return std::make_shared<tesseract_geometry::Cone>(r, l);
}
 
tinyxml2::XMLElement* writeCone(const std::shared_ptr<const tesseract_geometry::Cone>& cone, tinyxml2::XMLDocument& doc)
{
  if (cone == nullptr)
    std::throw_with_nested(std::runtime_error("Cone is nullptr and cannot be converted to XML"));
  tinyxml2::XMLElement* xml_element = doc.NewElement(CONE_ELEMENT_NAME.data());
  xml_element->SetAttribute("length", toString(cone->getLength()).c_str());
  xml_element->SetAttribute("radius", toString(cone->getRadius()).c_str());
  return xml_element;
}
 
}  // namespace tesseract_urdf