disabled_collisions.cpp

Go to the documentation of this file.

 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <console_bridge/console.h>
#include <tinyxml2.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_common/utils.h>
#include <tesseract_common/allowed_collision_matrix.h>
#include <tesseract_srdf/disabled_collisions.h>
#include <tesseract_scene_graph/graph.h>
 
namespace tesseract_srdf
{
tesseract_common::AllowedCollisionMatrix parseDisabledCollisions(const tesseract_scene_graph::SceneGraph& scene_graph,
                                                                 const tinyxml2::XMLElement* srdf_xml,
                                                                 const std::array<int, 3>& /*version*/)
{
  tesseract_common::AllowedCollisionMatrix acm;
 
  for (const tinyxml2::XMLElement* xml_element = srdf_xml->FirstChildElement("disable_collisions");
       xml_element != nullptr;
       xml_element = xml_element->NextSiblingElement("disable_collisions"))
  {
    std::string link1_name, link2_name, reason;
    int status = tesseract_common::QueryStringAttributeRequired(xml_element, "link1", link1_name);
    if (status != tinyxml2::XML_SUCCESS)
      std::throw_with_nested(std::runtime_error("DisabledCollisions: Missing or failed to parse attribute 'link1'!"));
 
    status = tesseract_common::QueryStringAttributeRequired(xml_element, "link2", link2_name);
    if (status != tinyxml2::XML_SUCCESS)
      std::throw_with_nested(std::runtime_error("DisabledCollisions: Missing or failed to parse attribute 'link2'!"));
 
    if (!scene_graph.getLink(link1_name))
    {
      CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot disable collisons.", link1_name.c_str());
      continue;
    }
    if (!scene_graph.getLink(link2_name))
    {
      CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot disable collisons.", link2_name.c_str());
      continue;
    }
 
    status = tesseract_common::QueryStringAttribute(xml_element, "reason", reason);
    // LCOV_EXCL_START
    if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
      std::throw_with_nested(std::runtime_error("DisabledCollisions: Missing or failed to parse attribute 'reason'!"));
    // LCOV_EXCL_STOP
 
    acm.addAllowedCollision(link1_name, link2_name, reason);
  }
 
  return acm;
}
}  // namespace tesseract_srdf