capsule.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/capsule.h>
#include <tesseract_urdf/capsule.h>
#include <tesseract_urdf/utils.h>
 
namespace tesseract_urdf
{
tesseract_geometry::Capsule::Ptr parseCapsule(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("Capsule: Missing or failed parsing attribute 'length'!"));
 
  if (xml_element->QueryDoubleAttribute("radius", &(r)) != tinyxml2::XML_SUCCESS || !(r > 0))
    std::throw_with_nested(std::runtime_error("Capsule: Missing or failed parsing attribute 'radius'!"));
 
  return std::make_shared<tesseract_geometry::Capsule>(r, l);
}
 
tinyxml2::XMLElement* writeCapsule(const std::shared_ptr<const tesseract_geometry::Capsule>& capsule,
                                   tinyxml2::XMLDocument& doc)
{
  if (capsule == nullptr)
    std::throw_with_nested(std::runtime_error("Capsule is nullptr and cannot be written to XML file"));
  tinyxml2::XMLElement* xml_element = doc.NewElement(CAPSULE_ELEMENT_NAME.data());
  xml_element->SetAttribute("length", toString(capsule->getLength()).c_str());
  xml_element->SetAttribute("radius", toString(capsule->getRadius()).c_str());
  return xml_element;
}
 
}  // namespace tesseract_urdf