mimic.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 <tesseract_common/utils.h>
#include <tinyxml2.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_scene_graph/joint.h>
#include <tesseract_urdf/mimic.h>
#include <tesseract_urdf/utils.h>
 
namespace tesseract_urdf
{
tesseract_scene_graph::JointMimic::Ptr parseMimic(const tinyxml2::XMLElement* xml_element)
{
  auto m = std::make_shared<tesseract_scene_graph::JointMimic>();
  if (tesseract_common::QueryStringAttribute(xml_element, "joint", m->joint_name) != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("Mimic: Missing or failed to parse mimic attribute 'joint'!"));
 
  if (xml_element->Attribute("offset") == nullptr && xml_element->Attribute("multiplier") == nullptr)
    CONSOLE_BRIDGE_logDebug("Mimic: Missing attribute 'offset' and 'multiplier', using default value 0 and 1!");
  else if (xml_element->Attribute("offset") != nullptr && xml_element->Attribute("multiplier") == nullptr)
    CONSOLE_BRIDGE_logDebug("Mimic: Missing attribute 'multiplier', using default value 1!");
  else if (xml_element->Attribute("offset") == nullptr && xml_element->Attribute("multiplier") != nullptr)
    CONSOLE_BRIDGE_logDebug("Mimic: Missing attribute 'offset', using default value 1!");
 
  int s = xml_element->QueryDoubleAttribute("offset", &(m->offset));
  if (s != tinyxml2::XML_NO_ATTRIBUTE && s != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("Mimic: Error parsing attribute 'offset'!"));
 
  s = xml_element->QueryDoubleAttribute("multiplier", &(m->multiplier));
  if (s != tinyxml2::XML_NO_ATTRIBUTE && s != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("Mimic: Error parsing attribute 'multiplier'!"));
 
  return m;
}
 
tinyxml2::XMLElement* writeMimic(const std::shared_ptr<const tesseract_scene_graph::JointMimic>& mimic,
                                 tinyxml2::XMLDocument& doc)
{
  if (mimic == nullptr)
    std::throw_with_nested(std::runtime_error("Mimic Joint is nullptr and cannot be converted to XML"));
  tinyxml2::XMLElement* xml_element = doc.NewElement(MIMIC_ELEMENT_NAME.data());
 
  xml_element->SetAttribute("joint", mimic->joint_name.c_str());
  xml_element->SetAttribute("offset", toString(mimic->offset).c_str());
  xml_element->SetAttribute("multiplier", toString(mimic->multiplier).c_str());
 
  return xml_element;
}
 
}  // namespace tesseract_urdf