GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/parser/parser.cc Lines: 131 170 77.1 %
Date: 2024-05-05 11:05:40 Branches: 97 276 35.1 %

Line Branch Exec Source
1
// Copyright (c) 2014, LAAS-CNRS
2
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3
//
4
5
// Redistribution and use in source and binary forms, with or without
6
// modification, are permitted provided that the following conditions are
7
// met:
8
//
9
// 1. Redistributions of source code must retain the above copyright
10
//    notice, this list of conditions and the following disclaimer.
11
//
12
// 2. Redistributions in binary form must reproduce the above copyright
13
// notice, this list of conditions and the following disclaimer in the
14
// documentation and/or other materials provided with the distribution.
15
//
16
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27
// DAMAGE.
28
29
#include <hpp/util/debug.hh>
30
#include <stdexcept>
31
// #include <resource_retriever/retriever.h>
32
33
#include <pinocchio/parsers/utils.hpp>  // ::pinocchio::retrieveResourcePath
34
#include <pinocchio/utils/file-explorer.hpp>  // ::pinocchio::rosPaths
35
36
#include "hpp/manipulation/parser/parser.hh"
37
#include "hpp/manipulation/srdf/factories.hh"
38
39
namespace hpp {
40
namespace manipulation {
41
namespace parser {
42
7
Parser::Parser(bool fillWithDefaultFactories, FactoryType defaultFactory)
43
7
    : root_(NULL), defaultFactory_(defaultFactory) {
44
7
  if (fillWithDefaultFactories) {
45
    addObjectFactory("robot", create<srdf::RobotFactory>);
46
    addObjectFactory("handle", create<srdf::HandleFactory>);
47
    addObjectFactory("gripper", create<srdf::GripperFactory>);
48
    addObjectFactory("position", create<srdf::PositionFactory>);
49
    addObjectFactory("mask", create<SequenceFactory<bool> >);
50
    addObjectFactory("mask_complement", create<SequenceFactory<bool> >);
51
    addObjectFactory("contact", create<srdf::ContactFactory>);
52
    addObjectFactory("point", create<srdf::ContactFactory::PointFactory>);
53
    addObjectFactory("triangle", create<srdf::ContactFactory::TriangleFactory>);
54
    addObjectFactory("shape", create<srdf::ContactFactory::ShapeFactory>);
55
56
    /// This removes warnings
57
    addObjectFactory("link", create<ObjectFactory>);
58
    addObjectFactory("disable_collisions", create<IgnoreTagFactory>);
59
    addObjectFactory("material", create<IgnoreTagFactory>);
60
    addObjectFactory("texture", create<IgnoreTagFactory>);
61
  }
62
7
}
63
64
7
Parser::~Parser() {
65
26
  for (ObjectFactoryList::iterator it = objectFactories_.begin();
66
45
       it != objectFactories_.end(); ++it)
67
19
    delete *it;
68

7
  if (root_ != NULL) delete root_;
69
7
}
70
71
6
void Parser::parseString(const std::string& xmlString, DevicePtr_t robot) {
72
6
  device_ = robot;
73
74
6
  loadString(xmlString.c_str());
75
  try {
76
6
    parse();
77
  } catch (const std::exception& exc) {
78
    std::ostringstream oss;
79
    oss << "in XML string, " << exc.what();
80
    throw std::runtime_error(oss.str().c_str());
81
  }
82
6
}
83
84
1
void Parser::parseFile(const std::string& filename, DevicePtr_t robot) {
85
1
  device_ = robot;
86
87
  std::string fn =
88

2
      ::pinocchio::retrieveResourcePath(filename, ::pinocchio::rosPaths());
89
1
  loadFile(fn.c_str());
90
  try {
91
1
    parse();
92
  } catch (const std::exception& exc) {
93
    std::ostringstream oss;
94
    oss << "in " << filename << ", " << exc.what();
95
    throw std::runtime_error(oss.str().c_str());
96
  }
97
1
}
98
99
1
void Parser::loadFile(const char* filename) {
100
1
  doc_.LoadFile(filename);
101
1
  if (doc_.Error()) {
102
    std::cerr << doc_.ErrorStr() << std::endl;
103
    return;
104
  }
105
}
106
107
6
void Parser::loadString(const char* xmlstring) {
108
6
  doc_.Parse(xmlstring);
109
6
  if (doc_.Error()) {
110
    std::cerr << doc_.ErrorStr() << std::endl;
111
  }
112
6
}
113
114
7
void Parser::parse() {
115
7
  const XMLElement* el = doc_.RootElement();
116

7
  root_ = new RootFactory(device_);
117
7
  root_->prefix(prefix_);
118
14
  while (el != NULL) {
119
7
    parseElement(el, root_);
120
7
    el = el->NextSiblingElement();
121
  }
122
26
  for (ObjectFactoryList::iterator it = objectFactories_.begin();
123
45
       it != objectFactories_.end(); ++it)
124
19
    (*it)->finishFile();
125
7
}
126
127
9
void Parser::addObjectFactory(const std::string& tagname, FactoryType factory) {
128
  ObjectFactoryInsertRet ret =
129

9
      objFactoryMap_.insert(ObjectFactoryPair(tagname, factory));
130

9
  if (!ret.second) throw std::logic_error("This tagname already exist");
131
9
}
132
133
19
void Parser::parseElement(const XMLElement* element, ObjectFactory* parent) {
134
19
  if (element == NULL) return;
135
136
19
  ObjectFactory* o = NULL;
137
  /// Look for this element in the map
138

19
  ObjectFactoryMap::const_iterator it = objFactoryMap_.find(element->Value());
139
19
  if (it != objFactoryMap_.end()) {
140
13
    o = it->second(parent, element);
141
  } else {
142
6
    o = defaultFactory_(parent, element);
143
    hppDout(warning, "I have no factory for tag " << o->tagName());
144
  }
145
19
  objectFactories_.push_back(o);
146

19
  if (!o->init()) return;
147
19
  const XMLAttribute* attr = element->FirstAttribute();
148
37
  while (attr != NULL) {
149
18
    o->setAttribute(attr);
150
18
    attr = attr->Next();
151
  }
152

19
  if (!o->finishAttributes()) return;
153
154
  /// Loop over is child tags
155
19
  const XMLNode* el = element->FirstChild();
156
36
  while (el != NULL) {
157

17
    if (el->ToElement() != NULL) {
158

12
      parseElement(el->ToElement(), o);
159

5
    } else if (el->ToUnknown() != NULL) {
160
      hppDout(warning, "Unknown Node in XML file: " << el->Value());
161

5
    } else if (el->ToText() != NULL) {
162

5
      o->addTextChild(el->ToText());
163
    } else if (el->ToComment() != NULL) {
164
    }
165
17
    el = el->NextSibling();
166
  }
167
19
  o->finishTags();
168
}
169
170
1
std::ostream& Parser::print(std::ostream& os) const {
171
1
  os << "Parser with " << objectFactories_.size() << " object." << std::endl;
172
1
  if (root_ != NULL) os << *root_;
173
1
  return os;
174
}
175
176
1
std::ostream& operator<<(std::ostream& os, const Parser& p) {
177
1
  return p.print(os);
178
}
179
180
8
std::ostream& operator<<(std::ostream& os, const ObjectFactory& o) {
181
8
  return o.print(os);
182
}
183
184
19
ObjectFactory::ObjectFactory(ObjectFactory* parent, const XMLElement* element)
185
19
    : parent_(parent), root_(NULL), element_(element), id_(-1) {
186
19
  if (parent_ == NULL) {
187
    root_ = dynamic_cast<RootFactory*>(this);
188
    if (root_ == NULL)
189
      throw std::logic_error(
190
          "ObjectFactory with no parent must be RootFactory");
191
  } else {
192
19
    root_ = parent_->root();
193

19
    if (element_ != NULL) parent_->addChild(this);
194
  }
195
19
}
196
197
7
ObjectFactory::ObjectFactory(RootFactory* root)
198
7
    : parent_(NULL), root_(root), element_(NULL), id_(-1) {}
199
200
19
bool ObjectFactory::init() { return true; }
201
202
19
bool ObjectFactory::finishAttributes() { return true; }
203
204
8
void ObjectFactory::finishTags() {}
205
206
19
void ObjectFactory::finishFile() {}
207
208
void ObjectFactory::addTextChild(const XMLText* /* text */) {}
209
210
27
std::string ObjectFactory::tagName() const {
211

27
  if (element_ != NULL) return element_->Value();
212
1
  return "No element";
213
}
214
215
10
std::string ObjectFactory::name() const { return name_; }
216
217
10
void ObjectFactory::name(const std::string& n) { name_ = n; }
218
219

10
void ObjectFactory::name(const char* n) { name(std::string(n)); }
220
221
ObjectFactory* ObjectFactory::parent() { return parent_; }
222
223
23
RootFactory* ObjectFactory::root() {
224
  // if (parent_ == NULL)
225
  // return this;
226
23
  return root_;
227
}
228
229
bool ObjectFactory::hasParent() const { return parent_ != NULL; }
230
231
const XMLElement* ObjectFactory::XMLelement() { return element_; }
232
233
15
void ObjectFactory::impl_setAttribute(const XMLAttribute* /* attr */) {}
234
235
19
void ObjectFactory::addChild(ObjectFactory* child) {
236

19
  children_[child->tagName()].push_back(child);
237
19
}
238
239
12
ObjectFactory::ObjectFactoryList ObjectFactory::getChildrenOfType(
240
    std::string type) {
241
12
  return children_[type];
242
}
243
244
2
bool ObjectFactory::getChildOfType(std::string type, ObjectFactory*& o) {
245

4
  ObjectFactoryList l = getChildrenOfType(type);
246
2
  if (l.empty()) {
247
    throw std::invalid_argument("Tag " + tagName() + " has no child of type " +
248
                                type);
249
  }
250
2
  o = l.front();
251
2
  if (l.size() != 1) {
252
    hppDout(warning, "Tag " << tagName() << " has several children of type "
253
                            << type << ". All but the first will be ignored.");
254
    return false;
255
  }
256
2
  return true;
257
}
258
259
8
std::ostream& ObjectFactory::print(std::ostream& os) const {
260


8
  os << "ObjectFactory " << tagName() << " with name " << name() << std::endl;
261
20
  for (ChildrenMap::const_iterator itTagName = children_.begin();
262
32
       itTagName != children_.end(); ++itTagName)
263
19
    for (ObjectFactoryList::const_iterator itObj = itTagName->second.begin();
264
26
         itObj != itTagName->second.end(); ++itObj)
265
7
      os << **itObj;
266
8
  return os;
267
}
268
269
18
void ObjectFactory::setAttribute(const XMLAttribute* attr) {
270

36
  std::string n = std::string(attr->Name());
271
18
  if (n == "name")
272

10
    name(attr->Value());
273
8
  else if (n == "id") {
274
    int v;
275
    if (attr->QueryIntValue(&v) != tinyxml2::XML_SUCCESS) {
276
      hppDout(error, "Attribute ID " << attr->Value() << " is incorrect.");
277
    } else {
278
      id_ = (int)v;
279
    }
280
  }
281

18
  attrMap_[n] = attr->Value();
282
18
  impl_setAttribute(attr);
283
18
}
284
285
18
bool ObjectFactory::hasAttribute(const std::string& attr) const {
286
18
  return attrMap_.find(attr) != attrMap_.end();
287
}
288
289
7
std::string ObjectFactory::getAttribute(const std::string& attr) const {
290
7
  AttributeMap::const_iterator it = attrMap_.find(attr);
291
7
  if (it == attrMap_.end()) {
292
    hppDout(error, "Asking for attribute " << attr);
293
    return std::string();
294
  }
295
7
  return it->second;
296
}
297
298
7
RootFactory::RootFactory(const DevicePtr_t dev)
299
7
    : ObjectFactory(this), device_(dev), prefix_("") {}
300
301
2
DevicePtr_t RootFactory::device() const { return device_; }
302
}  // namespace parser
303
}  // namespace manipulation
304
}  // namespace hpp