calibration.cpp

Go to the documentation of this file.

 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <stdexcept>
 
#include <console_bridge/console.h>
#include <tinyxml2.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_scene_graph/joint.h>
#include <tesseract_urdf/calibration.h>
#include <tesseract_urdf/utils.h>
 
namespace tesseract_urdf
{
tesseract_scene_graph::JointCalibration::Ptr parseCalibration(const tinyxml2::XMLElement* xml_element)
{
  if (xml_element->Attribute("rising") == nullptr && xml_element->Attribute("falling") == nullptr)
    std::throw_with_nested(std::runtime_error("Calibration: Missing both attribute 'rising' and 'falling', either "
                                              "remove tag add attributes and values!"));
 
  auto calibration = std::make_shared<tesseract_scene_graph::JointCalibration>();
  if (xml_element->Attribute("rising") == nullptr && xml_element->Attribute("falling") != nullptr)
    CONSOLE_BRIDGE_logDebug("Calibration: Missing attribute 'rising', using default value 0!");
 
  if (xml_element->Attribute("rising") != nullptr && xml_element->Attribute("falling") == nullptr)
    CONSOLE_BRIDGE_logDebug("Calibration: Missing attribute 'falling', using default value 0!");
 
  auto xml_status = xml_element->QueryDoubleAttribute("rising", &(calibration->rising));
  if (xml_status != tinyxml2::XML_NO_ATTRIBUTE && xml_status != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("Calibration: Error parsing attribute 'rising'!"));
 
  xml_status = xml_element->QueryDoubleAttribute("falling", &(calibration->falling));
  if (xml_status != tinyxml2::XML_NO_ATTRIBUTE && xml_status != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("Calibration: Error parsing attribute 'falling'!"));
 
  return calibration;
}
 
tinyxml2::XMLElement*
writeCalibration(const std::shared_ptr<const tesseract_scene_graph::JointCalibration>& calibration,
                 tinyxml2::XMLDocument& doc)
{
  if (calibration == nullptr)
    std::throw_with_nested(std::runtime_error("Calibration is nullptr and cannot be converted to XML"));
  tinyxml2::XMLElement* xml_element = doc.NewElement(CALIBRATION_ELEMENT_NAME.data());
  xml_element->SetAttribute("rising", toString(calibration->rising).c_str());
  xml_element->SetAttribute("falling", toString(calibration->falling).c_str());
  return xml_element;
}
 
}  // namespace tesseract_urdf