Fawkes API  Fawkes Development Version
model.cpp
1 /***************************************************************************
2  * model.cpp - URDF Model
3  *
4  * Created: Fri Feb 14 17:35:15 2014
5  * Copyright 2014 Till Hofmann
6  *
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 /* This code is based on ROS robot_model with the following copyright and license:
23  * Software License Agreement (BSD License)
24  *
25  * Copyright (c) 2008, Willow Garage, Inc.
26  * All rights reserved.
27  *
28  * Redistribution and use in source and binary forms, with or without
29  * modification, are permitted provided that the following conditions
30  * are met:
31  *
32  * * Redistributions of source code must retain the above copyright
33  * notice, this list of conditions and the following disclaimer.
34  * * Redistributions in binary form must reproduce the above
35  * copyright notice, this list of conditions and the following
36  * disclaimer in the documentation and/or other materials provided
37  * with the distribution.
38  * * Neither the name of the Willow Garage nor the names of its
39  * contributors may be used to endorse or promote products derived
40  * from this software without specific prior written permission.
41  *
42  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
43  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
44  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
45  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
46  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
47  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
48  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
49  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
50  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
51  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
52  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
53  * POSSIBILITY OF SUCH DAMAGE.
54  */
55 
56 #include "model.h"
57 
58 #include <core/exceptions/system.h>
59 #include <kdl_parser/exceptions.h>
60 #include <urdf_parser/urdf_parser.h>
61 
62 #include <fstream>
63 #include <iostream>
64 #include <vector>
65 
66 using namespace fawkes;
67 
68 namespace urdf {
69 
70 static bool
71 IsColladaData(const std::string &data)
72 {
73  return data.find("<COLLADA") != std::string::npos;
74 }
75 
76 /** @class Model <kdl_parser/model.h>
77  * This class represents an URDF model. It can be initialized
78  * using either Strings or Files, which are parsed and saved in
79  * the Model objects.
80  * This class implements the ModelInterface defined in
81  * /usr/include/urdf/urdf_model/model.h
82  */
83 
84 /** Initialize the Model using a URDF file
85  * @param filename The filename of the URDF file
86  * @return true if the model was intialized successfully
87  */
88 bool
89 Model::initFile(const std::string &filename)
90 {
91  // get the entire file
92  std::string xml_string;
93  std::fstream xml_file(filename.c_str(), std::fstream::in);
94  if (xml_file.is_open()) {
95  while (xml_file.good()) {
96  std::string line;
97  std::getline(xml_file, line);
98  xml_string += (line + "\n");
99  }
100  xml_file.close();
101  return Model::initString(xml_string);
102  } else {
103  throw CouldNotOpenFileException(filename.c_str());
104  }
105 }
106 
107 /** Initialize the model using a XML Document
108  * @param xml_doc The robot model as TiXmlDocument
109  * @return true if the model was intialized successfully
110  */
111 bool
112 Model::initXml(TiXmlDocument *xml_doc)
113 {
114  if (!xml_doc) {
116  }
117 
118  std::stringstream ss;
119  ss << *xml_doc;
120 
121  return Model::initString(ss.str());
122 }
123 
124 /** Initialize the model using a XML Element
125  * @param robot_xml The robot model as TiXmlElement
126  * @return true if the model was intialized successfully
127  */
128 bool
129 Model::initXml(TiXmlElement *robot_xml)
130 {
131  if (!robot_xml) {
133  }
134 
135  std::stringstream ss;
136  ss << (*robot_xml);
137 
138  return Model::initString(ss.str());
139 }
140 
141 /** Initialize the model using an URDF string
142  * @param xml_string The robot description in URDF format
143  * @return true if the model was intialized successfully
144  */
145 bool
146 Model::initString(const std::string &xml_string)
147 {
148  ModelInterfaceSharedPtr model;
149 
150  if (IsColladaData(xml_string)) {
151  // currently, support for Collada is not implemented
153  } else {
154  model = parseURDF(xml_string);
155  }
156 
157  // copy data from model into this object
158  if (model) {
159  this->links_ = model->links_;
160  this->joints_ = model->joints_;
161  this->materials_ = model->materials_;
162  this->name_ = model->name_;
163  this->root_link_ = model->root_link_;
164  return true;
165  } else
166  return false;
167 }
168 
169 } // namespace urdf
fawkes::URDFColladaNotSupportedException
Tried to to parse Collada data which is not supported.
Definition: exceptions.h:63
fawkes::URDFXMLDocumentParseErrorException
Failed to parse XML Document.
Definition: exceptions.h:75
fawkes::CouldNotOpenFileException
Definition: system.h:56
fawkes
fawkes::URDFXMLElementParseErrorException
Failed to parse XML Element.
Definition: exceptions.h:87