1 /*
2 * Copyright 2012 Open Source Robotics Foundation
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *
16 */
17
18 #include <iostream>
19 #include <cstdlib>
20 #include <map>
21 #include <string>
22
23 #include <ignition/math/SemanticVersion.hh>
24
25 #include "sdf/Console.hh"
26 #include "sdf/Converter.hh"
27 #include "sdf/Filesystem.hh"
28 #include "sdf/Param.hh"
29 #include "sdf/SDFImpl.hh"
30 #include "sdf/parser.hh"
31 #include "sdf/parser_private.hh"
32 #include "sdf/parser_urdf.hh"
33 #include "sdf/sdf_config.h"
34
35 namespace sdf
36 {
37 inline namespace SDF_VERSION_NAMESPACE {
38 //////////////////////////////////////////////////
39 template <typename TPtr>
_initFile(const std::string & _filename,TPtr _sdf)40 static inline bool _initFile(const std::string &_filename, TPtr _sdf)
41 {
42 TiXmlDocument xmlDoc;
43 if (xmlDoc.LoadFile(_filename))
44 {
45 return initDoc(&xmlDoc, _sdf);
46 }
47 else
48 {
49 sdferr << "Unable to load file[" << _filename << "]\n";
50 }
51
52 return false;
53 }
54
55 //////////////////////////////////////////////////
init(SDFPtr _sdf)56 bool init(SDFPtr _sdf)
57 {
58 std::string xmldata = SDF::EmbeddedSpec("root.sdf", false);
59 TiXmlDocument xmlDoc;
60 xmlDoc.Parse(xmldata.c_str());
61 return initDoc(&xmlDoc, _sdf);
62 }
63
64 //////////////////////////////////////////////////
initFile(const std::string & _filename,SDFPtr _sdf)65 bool initFile(const std::string &_filename, SDFPtr _sdf)
66 {
67 std::string xmldata = SDF::EmbeddedSpec(_filename, true);
68 if (!xmldata.empty())
69 {
70 TiXmlDocument xmlDoc;
71 xmlDoc.Parse(xmldata.c_str());
72 return initDoc(&xmlDoc, _sdf);
73 }
74 return _initFile(sdf::findFile(_filename), _sdf);
75 }
76
77 //////////////////////////////////////////////////
initFile(const std::string & _filename,ElementPtr _sdf)78 bool initFile(const std::string &_filename, ElementPtr _sdf)
79 {
80 std::string xmldata = SDF::EmbeddedSpec(_filename, true);
81 if (!xmldata.empty())
82 {
83 TiXmlDocument xmlDoc;
84 xmlDoc.Parse(xmldata.c_str());
85 return initDoc(&xmlDoc, _sdf);
86 }
87 return _initFile(sdf::findFile(_filename), _sdf);
88 }
89
90 //////////////////////////////////////////////////
initString(const std::string & _xmlString,SDFPtr _sdf)91 bool initString(const std::string &_xmlString, SDFPtr _sdf)
92 {
93 TiXmlDocument xmlDoc;
94 xmlDoc.Parse(_xmlString.c_str());
95 if (xmlDoc.Error())
96 {
97 sdferr << "Failed to parse string as XML: " << xmlDoc.ErrorDesc() << '\n';
98 return false;
99 }
100
101 return initDoc(&xmlDoc, _sdf);
102 }
103
104 //////////////////////////////////////////////////
_initDocGetElement(TiXmlDocument * _xmlDoc)105 inline TiXmlElement *_initDocGetElement(TiXmlDocument *_xmlDoc)
106 {
107 if (!_xmlDoc)
108 {
109 sdferr << "Could not parse the xml\n";
110 return nullptr;
111 }
112
113 TiXmlElement *element = _xmlDoc->FirstChildElement("element");
114 if (!element)
115 {
116 sdferr << "Could not find the 'element' element in the xml file\n";
117 return nullptr;
118 }
119
120 return element;
121 }
122
123 //////////////////////////////////////////////////
initDoc(TiXmlDocument * _xmlDoc,SDFPtr _sdf)124 bool initDoc(TiXmlDocument *_xmlDoc, SDFPtr _sdf)
125 {
126 auto element = _initDocGetElement(_xmlDoc);
127 if (!element)
128 {
129 return false;
130 }
131
132 return initXml(element, _sdf->Root());
133 }
134
135 //////////////////////////////////////////////////
initDoc(TiXmlDocument * _xmlDoc,ElementPtr _sdf)136 bool initDoc(TiXmlDocument *_xmlDoc, ElementPtr _sdf)
137 {
138 auto element = _initDocGetElement(_xmlDoc);
139 if (!element)
140 {
141 return false;
142 }
143
144 return initXml(element, _sdf);
145 }
146
147 //////////////////////////////////////////////////
initXml(TiXmlElement * _xml,ElementPtr _sdf)148 bool initXml(TiXmlElement *_xml, ElementPtr _sdf)
149 {
150 const char *refString = _xml->Attribute("ref");
151 if (refString)
152 {
153 _sdf->SetReferenceSDF(std::string(refString));
154 }
155
156 const char *nameString = _xml->Attribute("name");
157 if (!nameString)
158 {
159 sdferr << "Element is missing the name attribute\n";
160 return false;
161 }
162 _sdf->SetName(std::string(nameString));
163
164 const char *requiredString = _xml->Attribute("required");
165 if (!requiredString)
166 {
167 sdferr << "Element is missing the required attributed\n";
168 return false;
169 }
170 _sdf->SetRequired(requiredString);
171
172 const char *elemTypeString = _xml->Attribute("type");
173 if (elemTypeString)
174 {
175 bool required = std::string(requiredString) == "1" ? true : false;
176 const char *elemDefaultValue = _xml->Attribute("default");
177 std::string description;
178 TiXmlElement *descChild = _xml->FirstChildElement("description");
179 if (descChild && descChild->GetText())
180 {
181 description = descChild->GetText();
182 }
183
184 _sdf->AddValue(elemTypeString, elemDefaultValue, required, description);
185 }
186
187 // Get all attributes
188 for (TiXmlElement *child = _xml->FirstChildElement("attribute");
189 child; child = child->NextSiblingElement("attribute"))
190 {
191 TiXmlElement *descriptionChild = child->FirstChildElement("description");
192 const char *name = child->Attribute("name");
193 const char *type = child->Attribute("type");
194 const char *defaultValue = child->Attribute("default");
195
196 requiredString = child->Attribute("required");
197
198 if (!name)
199 {
200 sdferr << "Attribute is missing a name\n";
201 return false;
202 }
203 if (!type)
204 {
205 sdferr << "Attribute is missing a type\n";
206 return false;
207 }
208 if (!defaultValue)
209 {
210 sdferr << "Attribute[" << name << "] is missing a default\n";
211 return false;
212 }
213 if (!requiredString)
214 {
215 sdferr << "Attribute is missing a required string\n";
216 return false;
217 }
218 std::string requiredStr = sdf::trim(requiredString);
219 bool required = requiredStr == "1" ? true : false;
220 std::string description;
221
222 if (descriptionChild && descriptionChild->GetText())
223 {
224 description = descriptionChild->GetText();
225 }
226
227 _sdf->AddAttribute(name, type, defaultValue, required, description);
228 }
229
230 // Read the element description
231 TiXmlElement *descChild = _xml->FirstChildElement("description");
232 if (descChild && descChild->GetText())
233 {
234 _sdf->SetDescription(descChild->GetText());
235 }
236
237 // Get all child elements
238 for (TiXmlElement *child = _xml->FirstChildElement("element");
239 child; child = child->NextSiblingElement("element"))
240 {
241 const char *copyDataString = child->Attribute("copy_data");
242 if (copyDataString &&
243 (std::string(copyDataString) == "true" ||
244 std::string(copyDataString) == "1"))
245 {
246 _sdf->SetCopyChildren(true);
247 }
248 else
249 {
250 ElementPtr element(new Element);
251 initXml(child, element);
252 _sdf->AddElementDescription(element);
253 }
254 }
255
256 // Get all include elements
257 for (TiXmlElement *child = _xml->FirstChildElement("include");
258 child; child = child->NextSiblingElement("include"))
259 {
260 std::string filename = child->Attribute("filename");
261
262 ElementPtr element(new Element);
263
264 initFile(filename, element);
265
266 // override description for include elements
267 TiXmlElement *description = child->FirstChildElement("description");
268 if (description)
269 {
270 element->SetDescription(description->GetText());
271 }
272
273 _sdf->AddElementDescription(element);
274 }
275
276 return true;
277 }
278
279 //////////////////////////////////////////////////
readFile(const std::string & _filename,Errors & _errors)280 SDFPtr readFile(const std::string &_filename, Errors &_errors)
281 {
282 // Create and initialize the data structure that will hold the parsed SDF data
283 sdf::SDFPtr sdfParsed(new sdf::SDF());
284 sdf::init(sdfParsed);
285
286 // Read an SDF file, and store the result in sdfParsed.
287 if (!sdf::readFile(_filename, sdfParsed, _errors))
288 {
289 return SDFPtr();
290 }
291
292 return sdfParsed;
293 }
294
295 //////////////////////////////////////////////////
readFile(const std::string & _filename)296 SDFPtr readFile(const std::string &_filename)
297 {
298 Errors errors;
299 SDFPtr result = readFile(_filename, errors);
300
301 // Output errors
302 for (auto const &e : errors)
303 std::cerr << e << std::endl;
304
305 return result;
306 }
307
308 //////////////////////////////////////////////////
readFile(const std::string & _filename,SDFPtr _sdf)309 bool readFile(const std::string &_filename, SDFPtr _sdf)
310 {
311 Errors errors;
312 bool result = readFile(_filename, _sdf, errors);
313
314 // Output errors
315 for (auto const &e : errors)
316 std::cerr << e << std::endl;
317
318 return result;
319 }
320
321 //////////////////////////////////////////////////
readFile(const std::string & _filename,SDFPtr _sdf,Errors & _errors)322 bool readFile(const std::string &_filename, SDFPtr _sdf, Errors &_errors)
323 {
324 TiXmlDocument xmlDoc;
325 std::string filename = sdf::findFile(_filename);
326
327 if (filename.empty())
328 {
329 sdferr << "Error finding file [" << _filename << "].\n";
330 return false;
331 }
332
333 if (!xmlDoc.LoadFile(filename))
334 {
335 sdferr << "Error parsing XML in file [" << filename << "]: "
336 << xmlDoc.ErrorDesc() << '\n';
337 return false;
338 }
339
340 if (readDoc(&xmlDoc, _sdf, filename, true, _errors))
341 {
342 return true;
343 }
344 else if (sdf::URDF2SDF::IsURDF(filename))
345 {
346 sdf::URDF2SDF u2g;
347 TiXmlDocument doc = u2g.InitModelFile(filename);
348 if (sdf::readDoc(&doc, _sdf, "urdf file", true, _errors))
349 {
350 sdfdbg << "parse from urdf file [" << _filename << "].\n";
351 return true;
352 }
353 else
354 {
355 sdferr << "parse as old deprecated model file failed.\n";
356 return false;
357 }
358 }
359
360 return false;
361 }
362
363 //////////////////////////////////////////////////
readString(const std::string & _xmlString,SDFPtr _sdf)364 bool readString(const std::string &_xmlString, SDFPtr _sdf)
365 {
366 Errors errors;
367 bool result = readString(_xmlString, _sdf, errors);
368
369 // Output errors
370 for (auto const &e : errors)
371 std::cerr << e << std::endl;
372
373 return result;
374 }
375
376 //////////////////////////////////////////////////
readString(const std::string & _xmlString,SDFPtr _sdf,Errors & _errors)377 bool readString(const std::string &_xmlString, SDFPtr _sdf, Errors &_errors)
378 {
379 TiXmlDocument xmlDoc;
380 xmlDoc.Parse(_xmlString.c_str());
381 if (xmlDoc.Error())
382 {
383 sdferr << "Error parsing XML from string: " << xmlDoc.ErrorDesc() << '\n';
384 return false;
385 }
386 if (readDoc(&xmlDoc, _sdf, "data-string", true, _errors))
387 {
388 return true;
389 }
390 else
391 {
392 sdf::URDF2SDF u2g;
393 TiXmlDocument doc = u2g.InitModelString(_xmlString);
394 if (sdf::readDoc(&doc, _sdf, "urdf string", true, _errors))
395 {
396 sdfdbg << "Parsing from urdf.\n";
397 return true;
398 }
399 else
400 {
401 sdferr << "parse as old deprecated model file failed.\n";
402 return false;
403 }
404 }
405
406 return false;
407 }
408
409 //////////////////////////////////////////////////
readString(const std::string & _xmlString,ElementPtr _sdf)410 bool readString(const std::string &_xmlString, ElementPtr _sdf)
411 {
412 Errors errors;
413 bool result = readString(_xmlString, _sdf, errors);
414
415 // Output errors
416 for (auto const &e : errors)
417 std::cerr << e << std::endl;
418
419 return result;
420 }
421
422 //////////////////////////////////////////////////
readString(const std::string & _xmlString,ElementPtr _sdf,Errors & _errors)423 bool readString(const std::string &_xmlString, ElementPtr _sdf, Errors &_errors)
424 {
425 TiXmlDocument xmlDoc;
426 xmlDoc.Parse(_xmlString.c_str());
427 if (xmlDoc.Error())
428 {
429 sdferr << "Error parsing XML from string: " << xmlDoc.ErrorDesc() << '\n';
430 return false;
431 }
432 if (readDoc(&xmlDoc, _sdf, "data-string", true, _errors))
433 {
434 return true;
435 }
436 else
437 {
438 sdferr << "parse as sdf version " << SDF::Version() << " failed, "
439 << "should try to parse as old deprecated format\n";
440 return false;
441 }
442 }
443
444 //////////////////////////////////////////////////
readDoc(TiXmlDocument * _xmlDoc,SDFPtr _sdf,const std::string & _source,bool _convert,Errors & _errors)445 bool readDoc(TiXmlDocument *_xmlDoc, SDFPtr _sdf,
446 const std::string &_source, bool _convert, Errors &_errors)
447 {
448 if (!_xmlDoc)
449 {
450 sdfwarn << "Could not parse the xml from source[" << _source << "]\n";
451 return false;
452 }
453
454 // check sdf version
455 TiXmlElement *sdfNode = _xmlDoc->FirstChildElement("sdf");
456 if (!sdfNode)
457 {
458 sdferr << "Missing <sdf> element.\n";
459 return false;
460 }
461
462 if (sdfNode && sdfNode->Attribute("version"))
463 {
464 if (_convert
465 && strcmp(sdfNode->Attribute("version"), SDF::Version().c_str()) != 0)
466 {
467 sdfdbg << "Converting a deprecated source[" << _source << "].\n";
468 Converter::Convert(_xmlDoc, SDF::Version());
469 }
470
471 // parse new sdf xml
472 TiXmlElement *elemXml = _xmlDoc->FirstChildElement(_sdf->Root()->GetName());
473 if (!readXml(elemXml, _sdf->Root(), _errors))
474 {
475 _errors.push_back({ErrorCode::ELEMENT_INVALID,
476 "Error reading element <" + _sdf->Root()->GetName() + ">"});
477 return false;
478 }
479 }
480 else
481 {
482 // try to use the old deprecated parser
483 if (!sdfNode)
484 {
485 sdfdbg << "No <sdf> element in file[" << _source << "]\n";
486 }
487 else if (!sdfNode->Attribute("version"))
488 {
489 sdfdbg << "SDF <sdf> element has no version in file["
490 << _source << "]\n";
491 }
492 else if (strcmp(sdfNode->Attribute("version"),
493 SDF::Version().c_str()) != 0)
494 {
495 sdfdbg << "SDF version ["
496 << sdfNode->Attribute("version")
497 << "] is not " << SDF::Version() << "\n";
498 }
499 return false;
500 }
501
502 return true;
503 }
504
505 //////////////////////////////////////////////////
readDoc(TiXmlDocument * _xmlDoc,ElementPtr _sdf,const std::string & _source,bool _convert,Errors & _errors)506 bool readDoc(TiXmlDocument *_xmlDoc, ElementPtr _sdf,
507 const std::string &_source, bool _convert, Errors &_errors)
508 {
509 if (!_xmlDoc)
510 {
511 sdfwarn << "Could not parse the xml\n";
512 return false;
513 }
514
515 // check sdf version
516 TiXmlElement *sdfNode = _xmlDoc->FirstChildElement("sdf");
517 if (!sdfNode)
518 {
519 sdferr << "Missing <sdf> element.\n";
520 return false;
521 }
522
523 if (sdfNode && sdfNode->Attribute("version"))
524 {
525 if (_convert
526 && strcmp(sdfNode->Attribute("version"), SDF::Version().c_str()) != 0)
527 {
528 sdfwarn << "Converting a deprecated SDF source[" << _source << "].\n";
529 Converter::Convert(_xmlDoc, SDF::Version());
530 }
531
532 TiXmlElement *elemXml = sdfNode;
533 if (sdfNode->Value() != _sdf->GetName() &&
534 sdfNode->FirstChildElement(_sdf->GetName()))
535 {
536 elemXml = sdfNode->FirstChildElement(_sdf->GetName());
537 }
538
539 // parse new sdf xml
540 if (!readXml(elemXml, _sdf, _errors))
541 {
542 _errors.push_back({ErrorCode::ELEMENT_INVALID,
543 "Unable to parse sdf element["+ _sdf->GetName() + "]"});
544 return false;
545 }
546 }
547 else
548 {
549 // try to use the old deprecated parser
550 if (!sdfNode)
551 {
552 sdfdbg << "SDF has no <sdf> element\n";
553 }
554 else if (!sdfNode->Attribute("version"))
555 {
556 sdfdbg << "<sdf> element has no version\n";
557 }
558 else if (strcmp(sdfNode->Attribute("version"),
559 SDF::Version().c_str()) != 0)
560 {
561 sdfdbg << "SDF version ["
562 << sdfNode->Attribute("version")
563 << "] is not " << SDF::Version() << "\n";
564 }
565 return false;
566 }
567
568 return true;
569 }
570
571 //////////////////////////////////////////////////
getBestSupportedModelVersion(TiXmlElement * _modelXML,std::string & _modelFileName)572 std::string getBestSupportedModelVersion(TiXmlElement *_modelXML,
573 std::string &_modelFileName)
574 {
575 TiXmlElement *sdfXML = _modelXML->FirstChildElement("sdf");
576 TiXmlElement *nameSearch = _modelXML->FirstChildElement("name");
577
578 // If a match is not found, use the latest version of the element
579 // that is not older than the SDF parser.
580 ignition::math::SemanticVersion sdfParserVersion(SDF_VERSION);
581 std::string bestVersionStr = "0.0";
582
583 TiXmlElement *sdfSearch = sdfXML;
584 while (sdfSearch)
585 {
586 if (sdfSearch->Attribute("version"))
587 {
588 auto version = std::string(sdfSearch->Attribute("version"));
589 ignition::math::SemanticVersion modelVersion(version);
590 ignition::math::SemanticVersion bestVersion(bestVersionStr);
591 if (modelVersion > bestVersion)
592 {
593 // this model is better than the previous one
594 if (modelVersion <= sdfParserVersion)
595 {
596 // the parser can read it
597 sdfXML = sdfSearch;
598 bestVersionStr = version;
599 }
600 else
601 {
602 sdfwarn << "Ignoring version " << version
603 << " for model " << nameSearch->GetText()
604 << " because is newer than this sdf parser"
605 << " (version " << SDF_VERSION << ")\n";
606 }
607 }
608 }
609 sdfSearch = sdfSearch->NextSiblingElement("sdf");
610 }
611
612 if (!sdfXML || !sdfXML->GetText())
613 {
614 sdferr << "Failure to detect an sdf tag in the model config file"
615 << " for model: " << nameSearch->GetText() << "\n";
616
617 _modelFileName = "";
618 return "";
619 }
620
621 if (!sdfXML->Attribute("version"))
622 {
623 sdfwarn << "Can not find the XML attribute 'version'"
624 << " in sdf XML tag for model: " << nameSearch->GetText() << "."
625 << " Please specify the SDF protocol supported in the model"
626 << " configuration file. The first sdf tag in the config file"
627 << " will be used \n";
628 }
629
630 _modelFileName = sdfXML->GetText();
631 return bestVersionStr;
632 }
633
634 //////////////////////////////////////////////////
getModelFilePath(const std::string & _modelDirPath)635 std::string getModelFilePath(const std::string &_modelDirPath)
636 {
637 std::string configFilePath;
638
639 /// \todo This hardcoded bit is very Gazebo centric. It should
640 /// be abstracted away, possibly through a plugin to SDF.
641 configFilePath = sdf::filesystem::append(_modelDirPath, "model.config");
642 if (!sdf::filesystem::exists(configFilePath))
643 {
644 // We didn't find model.config, look for manifest.xml instead
645 configFilePath = sdf::filesystem::append(_modelDirPath, "manifest.xml");
646 if (!sdf::filesystem::exists(configFilePath))
647 {
648 // We didn't find manifest.xml either, output an error and get out.
649 sdferr << "Could not find model.config or manifest.xml for the model\n";
650 return std::string();
651 }
652 else
653 {
654 // We found manifest.xml, but since it is deprecated print a warning.
655 sdfwarn << "The manifest.xml for a model is deprecated. "
656 << "Please rename manifest.xml to "
657 << "model.config" << ".\n";
658 }
659 }
660
661 TiXmlDocument configFileDoc;
662 if (!configFileDoc.LoadFile(configFilePath))
663 {
664 sdferr << "Error parsing XML in file ["
665 << configFilePath << "]: "
666 << configFileDoc.ErrorDesc() << '\n';
667 return std::string();
668 }
669
670 TiXmlElement *modelXML = configFileDoc.FirstChildElement("model");
671
672 if (!modelXML)
673 {
674 sdferr << "No <model> element in configFile[" << configFilePath << "]\n";
675 return std::string();
676 }
677
678 std::string modelFileName;
679 if (getBestSupportedModelVersion(modelXML, modelFileName).empty())
680 {
681 return std::string();
682 }
683
684 return sdf::filesystem::append(_modelDirPath, modelFileName);
685 }
686
687 //////////////////////////////////////////////////
readXml(TiXmlElement * _xml,ElementPtr _sdf,Errors & _errors)688 bool readXml(TiXmlElement *_xml, ElementPtr _sdf, Errors &_errors)
689 {
690 // Check if the element pointer is deprecated.
691 if (_sdf->GetRequired() == "-1")
692 {
693 _errors.push_back({ErrorCode::ELEMENT_DEPRECATED,
694 "SDF Element[" + _sdf->GetName() + "] is deprecated"});
695 return true;
696 }
697
698 if (!_xml)
699 {
700 if (_sdf->GetRequired() == "1" || _sdf->GetRequired() =="+")
701 {
702 _errors.push_back({ErrorCode::ELEMENT_MISSING,
703 "SDF Element<" + _sdf->GetName() + "> is missing"});
704 return false;
705 }
706 else
707 {
708 return true;
709 }
710 }
711
712 if (_xml->GetText() != nullptr && _sdf->GetValue())
713 {
714 if (!_sdf->GetValue()->SetFromString(_xml->GetText()))
715 return false;
716 }
717
718 // check for nested sdf
719 std::string refSDFStr = _sdf->ReferenceSDF();
720 if (!refSDFStr.empty())
721 {
722 ElementPtr refSDF;
723 refSDF.reset(new Element);
724 std::string refFilename = refSDFStr + ".sdf";
725 initFile(refFilename, refSDF);
726 _sdf->RemoveFromParent();
727 _sdf->Copy(refSDF);
728 }
729
730 TiXmlAttribute *attribute = _xml->FirstAttribute();
731
732 unsigned int i = 0;
733
734 // Iterate over all the attributes defined in the give XML element
735 while (attribute)
736 {
737 // Find the matching attribute in SDF
738 for (i = 0; i < _sdf->GetAttributeCount(); ++i)
739 {
740 ParamPtr p = _sdf->GetAttribute(i);
741 if (p->GetKey() == attribute->Name())
742 {
743 // Set the value of the SDF attribute
744 if (!p->SetFromString(attribute->ValueStr()))
745 {
746 _errors.push_back({ErrorCode::ATTRIBUTE_INVALID,
747 "Unable to read attribute[" + p->GetKey() + "]"});
748 return false;
749 }
750 break;
751 }
752 }
753
754 if (i == _sdf->GetAttributeCount())
755 {
756 sdfwarn << "XML Attribute[" << attribute->Name()
757 << "] in element[" << _xml->Value()
758 << "] not defined in SDF, ignoring.\n";
759 }
760
761 attribute = attribute->Next();
762 }
763
764 // Check that all required attributes have been set
765 for (i = 0; i < _sdf->GetAttributeCount(); ++i)
766 {
767 ParamPtr p = _sdf->GetAttribute(i);
768 if (p->GetRequired() && !p->GetSet())
769 {
770 _errors.push_back({ErrorCode::ATTRIBUTE_MISSING,
771 "Required attribute[" + p->GetKey() + "] in element[" + _xml->Value()
772 + "] is not specified in SDF."});
773 return false;
774 }
775 }
776
777 if (_sdf->GetCopyChildren())
778 {
779 copyChildren(_sdf, _xml, false);
780 }
781 else
782 {
783 std::string filename;
784
785 // Iterate over all the child elements
786 TiXmlElement *elemXml = nullptr;
787 for (elemXml = _xml->FirstChildElement(); elemXml;
788 elemXml = elemXml->NextSiblingElement())
789 {
790 if (std::string("include") == elemXml->Value())
791 {
792 std::string modelPath;
793
794 if (elemXml->FirstChildElement("uri"))
795 {
796 std::string uri = elemXml->FirstChildElement("uri")->GetText();
797 modelPath = sdf::findFile(uri, true, true);
798
799 // Test the model path
800 if (modelPath.empty())
801 {
802 _errors.push_back({ErrorCode::URI_LOOKUP,
803 "Unable to find uri[" + uri + "]"});
804
805 size_t modelFound = uri.find("model://");
806 if (modelFound != 0u)
807 {
808 _errors.push_back({ErrorCode::URI_INVALID,
809 "Invalid uri[" + uri + "]. Should be model://" + uri});
810 }
811 continue;
812 }
813 else
814 {
815 if (!sdf::filesystem::is_directory(modelPath))
816 {
817 _errors.push_back({ErrorCode::DIRECTORY_NONEXISTANT,
818 "Directory doesn't exist[" + modelPath + "]"});
819 continue;
820 }
821 }
822
823 // Get the config.xml filename
824 filename = getModelFilePath(modelPath);
825 }
826 else
827 {
828 _errors.push_back({ErrorCode::ATTRIBUTE_MISSING,
829 "<include> element missing 'uri' attribute"});
830 continue;
831 }
832
833 // NOTE: sdf::init is an expensive call. For performance reason,
834 // a new sdf pointer is created here by cloning a fresh sdf template
835 // pointer instead of calling init every iteration.
836 // SDFPtr includeSDF(new SDF);
837 // init(includeSDF);
838 static SDFPtr includeSDFTemplate;
839 if (!includeSDFTemplate)
840 {
841 includeSDFTemplate.reset(new SDF);
842 init(includeSDFTemplate);
843 }
844 SDFPtr includeSDF(new SDF);
845 includeSDF->Root(includeSDFTemplate->Root()->Clone());
846
847 if (!readFile(filename, includeSDF))
848 {
849 _errors.push_back({ErrorCode::FILE_READ,
850 "Unable to read file[" + filename + "]"});
851 return false;
852 }
853
854 if (elemXml->FirstChildElement("name"))
855 {
856 includeSDF->Root()->GetElement("model")->GetAttribute(
857 "name")->SetFromString(
858 elemXml->FirstChildElement("name")->GetText());
859 }
860
861 TiXmlElement *poseElemXml = elemXml->FirstChildElement("pose");
862 if (poseElemXml)
863 {
864 sdf::ElementPtr poseElem =
865 includeSDF->Root()->GetElement("model")->GetElement("pose");
866
867 poseElem->GetValue()->SetFromString(poseElemXml->GetText());
868
869 const char *frame = poseElemXml->Attribute("frame");
870 if (frame)
871 {
872 poseElem->GetAttribute("frame")->SetFromString(frame);
873 }
874 }
875
876 if (elemXml->FirstChildElement("static"))
877 {
878 includeSDF->Root()->GetElement("model")->GetElement(
879 "static")->GetValue()->SetFromString(
880 elemXml->FirstChildElement("static")->GetText());
881 }
882
883 for (TiXmlElement *childElemXml = elemXml->FirstChildElement();
884 childElemXml; childElemXml = childElemXml->NextSiblingElement())
885 {
886 if (std::string("plugin") == childElemXml->Value())
887 {
888 sdf::ElementPtr pluginElem;
889 pluginElem = includeSDF->Root()->GetElement(
890 "model")->AddElement("plugin");
891
892 if (!readXml(childElemXml, pluginElem, _errors))
893 {
894 _errors.push_back({ErrorCode::ELEMENT_INVALID,
895 "Error reading plugin element"});
896 return false;
897 }
898 }
899 }
900
901 if (_sdf->GetName() == "model")
902 {
903 addNestedModel(_sdf, includeSDF->Root());
904 }
905 else
906 {
907 includeSDF->Root()->GetFirstElement()->SetParent(_sdf);
908 _sdf->InsertElement(includeSDF->Root()->GetFirstElement());
909 // TODO: This was used to store the included filename so that when
910 // a world is saved, the included model's SDF is not stored in the
911 // world file. This highlights the need to make model inclusion
912 // a core feature of SDF, and not a hack that that parser handles
913 // includeSDF->Root()->GetFirstElement()->SetInclude(
914 // elemXml->Attribute("filename"));
915 }
916
917 continue;
918 }
919
920 // Find the matching element in SDF
921 unsigned int descCounter = 0;
922 for (descCounter = 0;
923 descCounter != _sdf->GetElementDescriptionCount(); ++descCounter)
924 {
925 ElementPtr elemDesc = _sdf->GetElementDescription(descCounter);
926 if (elemDesc->GetName() == elemXml->Value())
927 {
928 ElementPtr element = elemDesc->Clone();
929 element->SetParent(_sdf);
930 if (readXml(elemXml, element, _errors))
931 {
932 _sdf->InsertElement(element);
933 }
934 else
935 {
936 _errors.push_back({ErrorCode::ELEMENT_INVALID,
937 std::string("Error reading element <") +
938 elemXml->Value() + ">"});
939 return false;
940 }
941 break;
942 }
943 }
944
945 if (descCounter == _sdf->GetElementDescriptionCount())
946 {
947 sdfdbg << "XML Element[" << elemXml->Value()
948 << "], child of element[" << _xml->Value()
949 << "], not defined in SDF. Copying[" << elemXml->Value() << "] "
950 << "as children of [" << _xml->Value() << "].\n";
951 continue;
952 }
953 }
954
955 // Copy unknown elements outside the loop so it only happens one time
956 copyChildren(_sdf, _xml, true);
957
958 // Check that all required elements have been set
959 for (unsigned int descCounter = 0;
960 descCounter != _sdf->GetElementDescriptionCount(); ++descCounter)
961 {
962 ElementPtr elemDesc = _sdf->GetElementDescription(descCounter);
963
964 if (elemDesc->GetRequired() == "1" || elemDesc->GetRequired() == "+")
965 {
966 if (!_sdf->HasElement(elemDesc->GetName()))
967 {
968 if (_sdf->GetName() == "joint" &&
969 _sdf->Get<std::string>("type") != "ball")
970 {
971 _errors.push_back({ErrorCode::ELEMENT_MISSING,
972 "XML Missing required element[" + elemDesc->GetName() +
973 "], child of element[" + _sdf->GetName() + "]"});
974 return false;
975 }
976 else
977 {
978 // Add default element
979 _sdf->AddElement(elemDesc->GetName());
980 }
981 }
982 }
983 }
984 }
985
986 return true;
987 }
988
989 /////////////////////////////////////////////////
replace_all(std::string & _str,const std::string & _from,const std::string & _to)990 static void replace_all(std::string &_str,
991 const std::string &_from,
992 const std::string &_to)
993 {
994 if (_from.empty())
995 {
996 return;
997 }
998 size_t start_pos = 0;
999 while ((start_pos = _str.find(_from, start_pos)) != std::string::npos)
1000 {
1001 _str.replace(start_pos, _from.length(), _to);
1002 // We need to advance our starting position beyond what we
1003 // just replaced to deal with the case where the '_to' string
1004 // happens to contain a piece of '_from'.
1005 start_pos += _to.length();
1006 }
1007 }
1008
1009 /////////////////////////////////////////////////
copyChildren(ElementPtr _sdf,TiXmlElement * _xml,const bool _onlyUnknown)1010 void copyChildren(ElementPtr _sdf, TiXmlElement *_xml, const bool _onlyUnknown)
1011 {
1012 // Iterate over all the child elements
1013 TiXmlElement *elemXml = nullptr;
1014 for (elemXml = _xml->FirstChildElement(); elemXml;
1015 elemXml = elemXml->NextSiblingElement())
1016 {
1017 std::string elem_name = elemXml->ValueStr();
1018
1019 if (_sdf->HasElementDescription(elem_name))
1020 {
1021 if (!_onlyUnknown)
1022 {
1023 sdf::ElementPtr element = _sdf->AddElement(elem_name);
1024
1025 // FIXME: copy attributes
1026 for (TiXmlAttribute *attribute = elemXml->FirstAttribute();
1027 attribute; attribute = attribute->Next())
1028 {
1029 element->GetAttribute(attribute->Name())->SetFromString(
1030 attribute->ValueStr());
1031 }
1032
1033 // copy value
1034 std::string value = elemXml->GetText();
1035 if (!value.empty())
1036 {
1037 element->GetValue()->SetFromString(value);
1038 }
1039 copyChildren(element, elemXml, _onlyUnknown);
1040 }
1041 }
1042 else
1043 {
1044 ElementPtr element(new Element);
1045 element->SetParent(_sdf);
1046 element->SetName(elem_name);
1047 if (elemXml->GetText() != nullptr)
1048 {
1049 element->AddValue("string", elemXml->GetText(), "1");
1050 }
1051
1052 for (TiXmlAttribute *attribute = elemXml->FirstAttribute();
1053 attribute; attribute = attribute->Next())
1054 {
1055 element->AddAttribute(attribute->Name(), "string", "", 1, "");
1056 element->GetAttribute(attribute->Name())->SetFromString(
1057 attribute->ValueStr());
1058 }
1059
1060 copyChildren(element, elemXml, _onlyUnknown);
1061 _sdf->InsertElement(element);
1062 }
1063 }
1064 }
1065
1066 /////////////////////////////////////////////////
addNestedModel(ElementPtr _sdf,ElementPtr _includeSDF)1067 void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF)
1068 {
1069 ElementPtr modelPtr = _includeSDF->GetElement("model");
1070 ElementPtr elem = modelPtr->GetFirstElement();
1071 std::map<std::string, std::string> replace;
1072
1073 ignition::math::Pose3d modelPose =
1074 modelPtr->Get<ignition::math::Pose3d>("pose");
1075
1076 std::string modelName = modelPtr->Get<std::string>("name");
1077 while (elem)
1078 {
1079 if (elem->GetName() == "link")
1080 {
1081 std::string elemName = elem->Get<std::string>("name");
1082 std::string newName = modelName + "::" + elemName;
1083 replace[elemName] = newName;
1084 if (elem->HasElementDescription("pose"))
1085 {
1086 ignition::math::Pose3d offsetPose =
1087 elem->Get<ignition::math::Pose3d>("pose");
1088 ignition::math::Pose3d newPose = ignition::math::Pose3d(
1089 modelPose.Pos() +
1090 modelPose.Rot().RotateVector(offsetPose.Pos()),
1091 modelPose.Rot() * offsetPose.Rot());
1092 elem->GetElement("pose")->Set(newPose);
1093 }
1094 }
1095 else if (elem->GetName() == "joint")
1096 {
1097 // for joints, we need to
1098 // prefix name like we did with links, and
1099 std::string elemName = elem->Get<std::string>("name");
1100 std::string newName = modelName + "::" + elemName;
1101 replace[elemName] = newName;
1102 // rotate the joint axis because they are model-global
1103 if (elem->HasElement("axis"))
1104 {
1105 ElementPtr axisElem = elem->GetElement("axis");
1106 ignition::math::Vector3d newAxis = modelPose.Rot().RotateVector(
1107 axisElem->Get<ignition::math::Vector3d>("xyz"));
1108 axisElem->GetElement("xyz")->Set(newAxis);
1109 }
1110 }
1111 elem = elem->GetNextElement();
1112 }
1113
1114 std::string str = _includeSDF->ToString("");
1115 for (std::map<std::string, std::string>::iterator iter = replace.begin();
1116 iter != replace.end(); ++iter)
1117 {
1118 replace_all(str, std::string("\"") + iter->first + "\"",
1119 std::string("\"") + iter->second + "\"");
1120 replace_all(str, std::string("'") + iter->first + "'",
1121 std::string("'") + iter->second + "'");
1122 replace_all(str, std::string(">") + iter->first + "<",
1123 std::string(">") + iter->second + "<");
1124 }
1125
1126 _includeSDF->ClearElements();
1127 readString(str, _includeSDF);
1128
1129 elem = _includeSDF->GetElement("model")->GetFirstElement();
1130 ElementPtr nextElem;
1131 while (elem)
1132 {
1133 nextElem = elem->GetNextElement();
1134
1135 if (elem->GetName() != "pose")
1136 {
1137 elem->SetParent(_sdf);
1138 _sdf->InsertElement(elem);
1139 }
1140 elem = nextElem;
1141 }
1142 }
1143
1144 /////////////////////////////////////////////////
convertFile(const std::string & _filename,const std::string & _version,SDFPtr _sdf)1145 bool convertFile(const std::string &_filename, const std::string &_version,
1146 SDFPtr _sdf)
1147 {
1148 std::string filename = sdf::findFile(_filename);
1149
1150 if (filename.empty())
1151 {
1152 sdferr << "Error finding file [" << _filename << "].\n";
1153 return false;
1154 }
1155
1156 TiXmlDocument xmlDoc;
1157 if (xmlDoc.LoadFile(filename))
1158 {
1159 if (sdf::Converter::Convert(&xmlDoc, _version, true))
1160 {
1161 Errors errors;
1162 bool result = sdf::readDoc(&xmlDoc, _sdf, filename, false, errors);
1163
1164 // Output errors
1165 for (auto const &e : errors)
1166 std::cerr << e << std::endl;
1167
1168 return result;
1169 }
1170 }
1171 else
1172 {
1173 sdferr << "Error parsing file[" << filename << "]\n";
1174 }
1175
1176 return false;
1177 }
1178
1179 /////////////////////////////////////////////////
convertString(const std::string & _sdfString,const std::string & _version,SDFPtr _sdf)1180 bool convertString(const std::string &_sdfString, const std::string &_version,
1181 SDFPtr _sdf)
1182 {
1183 if (_sdfString.empty())
1184 {
1185 sdferr << "SDF string is empty.\n";
1186 return false;
1187 }
1188
1189 TiXmlDocument xmlDoc;
1190 xmlDoc.Parse(_sdfString.c_str());
1191
1192 if (!xmlDoc.Error())
1193 {
1194 if (sdf::Converter::Convert(&xmlDoc, _version, true))
1195 {
1196 Errors errors;
1197 bool result = sdf::readDoc(&xmlDoc, _sdf, "data-string", false, errors);
1198
1199 // Output errors
1200 for (auto const &e : errors)
1201 std::cerr << e << std::endl;
1202
1203 return result;
1204 }
1205 }
1206 else
1207 {
1208 sdferr << "Error parsing XML from string[" << _sdfString << "]\n";
1209 }
1210
1211 return false;
1212 }
1213 }
1214 }
1215