box.cpp

Go to the documentation of this file.

 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <stdexcept>
 
#include <boost/algorithm/string.hpp>
#include <tesseract_common/utils.h>
#include <tinyxml2.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_geometry/impl/box.h>
#include <tesseract_urdf/box.h>
 
namespace tesseract_urdf
{
tesseract_geometry::Box::Ptr parseBox(const tinyxml2::XMLElement* xml_element)
{
  std::string size_string;
  if (tesseract_common::QueryStringAttribute(xml_element, "size", size_string) != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("Box: Missing or failed parsing box attribute size!"));
 
  std::vector<std::string> tokens;
  boost::split(tokens, size_string, boost::is_any_of(" "), boost::token_compress_on);
  if (tokens.size() != 3 || !tesseract_common::isNumeric(tokens))
    std::throw_with_nested(std::runtime_error("Box: Failed converting box attribute size to vector!"));
 
  double l{ 0 }, w{ 0 }, h{ 0 };
  // No need to check return values because the tokens are verified above
  tesseract_common::toNumeric<double>(tokens[0], l);
  tesseract_common::toNumeric<double>(tokens[1], w);
  tesseract_common::toNumeric<double>(tokens[2], h);
 
  if (!(l > 0))
    std::throw_with_nested(std::runtime_error("Box: The length must be greater than zero!"));
 
  if (!(w > 0))
    std::throw_with_nested(std::runtime_error("Box: The width must be greater than zero!"));
 
  if (!(h > 0))
    std::throw_with_nested(std::runtime_error("Box: The height must be greater than zero!"));
 
  return std::make_shared<tesseract_geometry::Box>(l, w, h);
}
 
tinyxml2::XMLElement* writeBox(const std::shared_ptr<const tesseract_geometry::Box>& box, tinyxml2::XMLDocument& doc)
{
  if (box == nullptr)
    std::throw_with_nested(std::runtime_error("Box is nullptr and cannot be converted to XML"));
  tinyxml2::XMLElement* xml_element = doc.NewElement(BOX_ELEMENT_NAME.data());
  Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " ");
  std::stringstream size_string;
  size_string << Eigen::Vector3d(box->getX(), box->getY(), box->getZ()).format(eigen_format);
  xml_element->SetAttribute("size", size_string.str().c_str());
  return xml_element;
}
 
}  // namespace tesseract_urdf