groups.cpp

Go to the documentation of this file.

 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <tinyxml2.h>
#include <iostream>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_common/utils.h>
#include <tesseract_scene_graph/graph.h>
#include <tesseract_srdf/groups.h>
 
namespace tesseract_srdf
{
std::tuple<GroupNames, ChainGroups, JointGroups, LinkGroups>
parseGroups(const tesseract_scene_graph::SceneGraph& scene_graph,
            const tinyxml2::XMLElement* srdf_xml,
            const std::array<int, 3>& /*version*/)
{
  using tesseract_common::strFormat;
 
  GroupNames group_names;
  ChainGroups chain_groups;
  LinkGroups link_groups;
  JointGroups joint_groups;
 
  for (const tinyxml2::XMLElement* xml_element = srdf_xml->FirstChildElement("group"); xml_element != nullptr;
       xml_element = xml_element->NextSiblingElement("group"))
  {
    std::string group_name;
    int status = tesseract_common::QueryStringAttributeRequired(xml_element, "name", group_name);
    if (status != tinyxml2::XML_SUCCESS)
      std::throw_with_nested(std::runtime_error("Group: Missing or failed to parse attribute 'name'!"));
 
    std::vector<std::string> links;
    std::vector<std::string> joints;
    std::vector<std::pair<std::string, std::string>> chains;
 
    // get the links in the groups
    for (const tinyxml2::XMLElement* link_xml = xml_element->FirstChildElement("link"); link_xml != nullptr;
         link_xml = link_xml->NextSiblingElement("link"))
    {
      std::string link_name;
      int status = tesseract_common::QueryStringAttributeRequired(link_xml, "name", link_name);
      if (status != tinyxml2::XML_SUCCESS)
        std::throw_with_nested(std::runtime_error(
            strFormat("Group: '%s' link element is missing or failed to parse attribute 'name'!", group_name.c_str())));
 
      if (!scene_graph.getLink(link_name))
        std::throw_with_nested(std::runtime_error(strFormat(
            "Group: '%s' link '%s' is not known to the Scene Graph!", group_name.c_str(), link_name.c_str())));
 
      links.push_back(link_name);
    }
 
    // get the joints in the groups
    for (const tinyxml2::XMLElement* joint_xml = xml_element->FirstChildElement("joint"); joint_xml != nullptr;
         joint_xml = joint_xml->NextSiblingElement("joint"))
    {
      std::string joint_name;
      int status = tesseract_common::QueryStringAttributeRequired(joint_xml, "name", joint_name);
      if (status != tinyxml2::XML_SUCCESS)
        std::throw_with_nested(std::runtime_error(strFormat("Group: '%s' joint element is missing or failed to parse "
                                                            "attribute 'name'!",
                                                            group_name.c_str())));
 
      if (!scene_graph.getJoint(joint_name))
        std::throw_with_nested(std::runtime_error(strFormat(
            "Group: '%s' joint '%s' is not known to the Scene Graph!", group_name.c_str(), joint_name.c_str())));
 
      joints.push_back(joint_name);
    }
 
    // get the chains in the groups
    for (const tinyxml2::XMLElement* chain_xml = xml_element->FirstChildElement("chain"); chain_xml != nullptr;
         chain_xml = chain_xml->NextSiblingElement("chain"))
    {
      std::string base_link_name, tip_link_name;
      int status = tesseract_common::QueryStringAttributeRequired(chain_xml, "base_link", base_link_name);
      if (status != tinyxml2::XML_SUCCESS)
        std::throw_with_nested(std::runtime_error(strFormat("Group: '%s' chain element is missing or failed to parse "
                                                            "attribute "
                                                            "'base_link'!",
                                                            group_name.c_str())));
 
      status = tesseract_common::QueryStringAttributeRequired(chain_xml, "tip_link", tip_link_name);
      if (status != tinyxml2::XML_SUCCESS)
        std::throw_with_nested(std::runtime_error(strFormat("Group: '%s' chain element is missing or failed to parse "
                                                            "attribute "
                                                            "'tip_link'!",
                                                            group_name.c_str())));
 
      if (!scene_graph.getLink(base_link_name))
        std::throw_with_nested(std::runtime_error(strFormat("Group: '%s' chain element base link '%s' is not known to "
                                                            "the Scene Graph!",
                                                            group_name.c_str(),
                                                            base_link_name.c_str())));
 
      if (!scene_graph.getLink(tip_link_name))
        std::throw_with_nested(std::runtime_error(strFormat("Group: '%s' chain element tip link '%s' is not known to "
                                                            "the Scene Graph!",
                                                            group_name.c_str(),
                                                            tip_link_name.c_str())));
 
      chains.emplace_back(base_link_name, tip_link_name);
    }
 
    if (!chains.empty() && links.empty() && joints.empty())
    {
      chain_groups[group_name] = chains;
      group_names.insert(group_name);
    }
    else if (chains.empty() && !links.empty() && joints.empty())
    {
      link_groups[group_name] = links;
      group_names.insert(group_name);
    }
    else if (chains.empty() && links.empty() && !joints.empty())
    {
      joint_groups[group_name] = joints;
      group_names.insert(group_name);
    }
    else
    {
      std::throw_with_nested(
          std::runtime_error(strFormat("Group: '%s' is empty or multiple types were provided!", group_name.c_str())));
    }
  }
 
  return std::make_tuple(group_names, chain_groups, joint_groups, link_groups);
}
}  // namespace tesseract_srdf