bool Parser::parseURDF()

in src/esp/io/URDFParser.cpp [160:296]
112 lines of code
19 McCabe index (conditional complexity)

bool Parser::parseURDF(std::shared_ptr<Model>& urdfModel,
                       const std::string& filename) {
  // override the previous model with a fresh one
  urdfModel = std::make_shared<Model>();
  sourceFilePath_ = filename;
  urdfModel->m_sourceFile = filename;

  std::string xmlString = Corrade::Utility::Directory::readString(filename);

  XMLDocument xml_doc;
  xml_doc.Parse(xmlString.c_str());
  if (xml_doc.Error()) {
    ESP_ERROR() << "XML parse error, aborting URDF parse/load for" << filename;
    return false;
  }
  ESP_VERY_VERBOSE() << "XML parsed starting URDF parse/load.";

  const XMLElement* robot_xml = xml_doc.FirstChildElement("robot");
  if (!robot_xml) {
    ESP_ERROR() << "Expected a <robot> element. Aborting URDF parse/load for"
                << filename;
    return false;
  }

  // Get robot name
  const char* name = robot_xml->Attribute("name");
  if (!name) {
    ESP_ERROR() << "Expected a name for robot. Aborting URDF parse/load for"
                << filename;
    return false;
  }
  urdfModel->m_name = name;

  // Get all Material elements
  for (const XMLElement* material_xml =
           robot_xml->FirstChildElement("material");
       material_xml;
       material_xml = material_xml->NextSiblingElement("material")) {
    Material material;

    parseMaterial(material, material_xml);

    if (urdfModel->m_materials.count(material.m_name) == 0) {
      urdfModel->m_materials[material.m_name] =
          std::make_shared<Material>(material);
    } else {
      ESP_VERY_VERBOSE() << "W - Duplicate material";
    }
  }

  // Get all link elements including shapes
  int link_index = 0;
  for (const XMLElement* link_xml = robot_xml->FirstChildElement("link");
       link_xml; link_xml = link_xml->NextSiblingElement("link")) {
    std::shared_ptr<Link> link = std::make_shared<Link>();

    if (parseLink(urdfModel, *link, link_xml)) {
      if (urdfModel->m_links.count(link->m_name) != 0u) {
        ESP_ERROR()
            << "Link name  (" << link->m_name
            << ") is not unique, link names "
               "in the same model have to be unique. Aborting parse/load for"
            << filename;
        return false;
      } else {
        // copy model material into link material, if link has no local material
        for (size_t i = 0; i < link->m_visualArray.size(); i++) {
          VisualShape& vis = link->m_visualArray.at(i);
          if (!vis.m_geometry.m_hasLocalMaterial &&
              !vis.m_materialName.empty()) {
            auto mat_itr = urdfModel->m_materials.find(vis.m_materialName);
            if (mat_itr != urdfModel->m_materials.end()) {
              vis.m_geometry.m_localMaterial = mat_itr->second;
            } else {
              ESP_ERROR() << "Cannot find material with name:"
                          << vis.m_materialName << ". Aborting parse/load for"
                          << filename;
            }
          }
        }

        // register the new link
        urdfModel->m_links[link->m_name] = link;
        link->m_linkIndex = link_index;
        urdfModel->m_linkIndicesToNames[link->m_linkIndex] = link->m_name;
        link_index++;
      }
    } else {
      ESP_ERROR() << "Failed to parse link. Aborting parse/load for"
                  << filename;
      return false;
    }
  }
  if (urdfModel->m_links.empty()) {
    ESP_ERROR() << "No links found in URDF file. Aborting parse/load for"
                << filename;
    return false;
  }

  // Get all Joint elements
  for (const XMLElement* joint_xml = robot_xml->FirstChildElement("joint");
       joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) {
    std::shared_ptr<Joint> joint = std::make_shared<Joint>();

    if (parseJoint(*joint, joint_xml)) {
      if (urdfModel->m_joints.count(joint->m_name) != 0u) {
        ESP_ERROR() << "Joint" << joint->m_name
                    << "is not unique. Aborting parse/load for" << filename;
        return false;
      } else {
        urdfModel->m_joints[joint->m_name] = joint;
      }
    } else {
      ESP_ERROR() << "Joint xml is not initialized correctly. Aborting "
                     "parse/load for"
                  << filename;
      return false;
    }
  }

  // TODO: parse sensors here

  if (!initTreeAndRoot(urdfModel)) {
    return false;
  }

  ESP_VERY_VERBOSE() << "Done parsing URDF for" << filename;

  // attempt to load JSON config for this Model
  if (urdfModel->loadJsonAttributes(filename)) {
    ESP_VERY_VERBOSE() << "Loading JSON Attributes successful for this model.";
  } else {
    ESP_VERY_VERBOSE()
        << "No extra JSON configuration data found for this model.";
  }
  return true;
}