safety_controller.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/safety_controller.h>
#include <tesseract_urdf/utils.h>
 
namespace tesseract_urdf
{
tesseract_scene_graph::JointSafety::Ptr parseSafetyController(const tinyxml2::XMLElement* xml_element)
{
  auto s = std::make_shared<tesseract_scene_graph::JointSafety>();
  if (xml_element->QueryDoubleAttribute("k_velocity", &(s->k_velocity)) != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("SafetyController: Missing or failed to parse attribute 'k_velocity'!"));
 
  if (xml_element->Attribute("soft_upper_limit") == nullptr && xml_element->Attribute("soft_lower_limit") == nullptr &&
      xml_element->Attribute("k_position") == nullptr)
  {
    CONSOLE_BRIDGE_logDebug("SafetyController: Missing attributes 'soft_upper_limit', 'soft_lower_limit', and "
                            "'k_position', using default value 0, 0, and 0!");
  }
  else if (xml_element->Attribute("soft_upper_limit") == nullptr ||
           xml_element->Attribute("soft_lower_limit") == nullptr || xml_element->Attribute("k_position") == nullptr)
  {
    if (xml_element->Attribute("soft_upper_limit") == nullptr)
      CONSOLE_BRIDGE_logDebug("SafetyController: Missing attribute 'soft_upper_limit', using default value 0!");
 
    if (xml_element->Attribute("soft_lower_limit") == nullptr)
      CONSOLE_BRIDGE_logDebug("SafetyController: Missing attribute 'soft_lower_limit', using default value 0!");
 
    if (xml_element->Attribute("k_position") == nullptr)
      CONSOLE_BRIDGE_logDebug("SafetyController: Missing attribute 'k_position', using default value 0!");
  }
 
  s->soft_upper_limit = 0;
  s->soft_lower_limit = 0;
  s->k_position = 0;
  xml_element->QueryDoubleAttribute("soft_upper_limit", &s->soft_upper_limit);
  xml_element->QueryDoubleAttribute("soft_lower_limit", &s->soft_lower_limit);
  xml_element->QueryDoubleAttribute("k_position", &s->k_position);
 
  return s;
}
 
tinyxml2::XMLElement* writeSafetyController(const std::shared_ptr<const tesseract_scene_graph::JointSafety>& safety,
                                            tinyxml2::XMLDocument& doc)
{
  if (safety == nullptr)
    std::throw_with_nested(std::runtime_error("Safety Controller is nullptr and cannot be converted to XML"));
  tinyxml2::XMLElement* xml_element = doc.NewElement(SAFETY_CONTROLLER_ELEMENT_NAME.data());
 
  xml_element->SetAttribute("k_velocity", toString(safety->k_velocity).c_str());
 
  // Could potentially check for and not write out zero.
  xml_element->SetAttribute("soft_upper_limit", toString(safety->soft_upper_limit).c_str());
  xml_element->SetAttribute("soft_lower_limit", toString(safety->soft_lower_limit).c_str());
  xml_element->SetAttribute("k_position", toString(safety->k_position).c_str());
 
  return xml_element;
}
 
}  // namespace tesseract_urdf