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 
57 #include <kdl_parser/model.h>
58 #include <urdf_parser/urdf_parser.h>
59 
60 #include <vector>
61 #include <fstream>
62 #include <iostream>
63 
64 #include <core/exceptions/system.h>
65 #include <kdl_parser/exceptions.h>
66 
67 using namespace fawkes;
68 
69 namespace urdf {
70 
71 static bool IsColladaData(const std::string& data)
72 {
73  return data.find("<COLLADA") != std::string::npos;
74 }
75 
76 
77 /** @class Model <kdl_parser/model.h>
78  * This class represents an URDF model. It can be initialized
79  * using either Strings or Files, which are parsed and saved in
80  * the Model objects.
81  * This class implements the ModelInterface defined in
82  * /usr/include/urdf/urdf_model/model.h
83  */
84 
85 /** Initialize the Model using a URDF file
86  * @param filename The filename of the URDF file
87  * @return true if the model was intialized successfully
88  */
89 bool Model::initFile(const std::string& filename)
90 {
91 
92  // get the entire file
93  std::string xml_string;
94  std::fstream xml_file(filename.c_str(), std::fstream::in);
95  if (xml_file.is_open())
96  {
97  while ( xml_file.good() )
98  {
99  std::string line;
100  std::getline( xml_file, line);
101  xml_string += (line + "\n");
102  }
103  xml_file.close();
104  return Model::initString(xml_string);
105  }
106  else
107  {
108  throw CouldNotOpenFileException(filename.c_str());
109  }
110 
111 }
112 
113 /** Initialize the model using a XML Document
114  * @param xml_doc The robot model as TiXmlDocument
115  * @return true if the model was intialized successfully
116  */
117 bool Model::initXml(TiXmlDocument *xml_doc)
118 {
119  if (!xml_doc)
120  {
122  }
123 
124  std::stringstream ss;
125  ss << *xml_doc;
126 
127  return Model::initString(ss.str());
128 }
129 
130 /** Initialize the model using a XML Element
131  * @param robot_xml The robot model as TiXmlElement
132  * @return true if the model was intialized successfully
133  */
134 bool Model::initXml(TiXmlElement *robot_xml)
135 {
136  if (!robot_xml)
137  {
139  }
140 
141  std::stringstream ss;
142  ss << (*robot_xml);
143 
144  return Model::initString(ss.str());
145 }
146 
147 
148 /** Initialize the model using an URDF string
149  * @param xml_string The robot description in URDF format
150  * @return true if the model was intialized successfully
151  */
152 bool Model::initString(const std::string& xml_string)
153 {
154  boost::shared_ptr<ModelInterface> model;
155 
156  if( IsColladaData(xml_string) ) {
157  // currently, support for Collada is not implemented
159  }
160  else {
161  model = parseURDF(xml_string);
162  }
163 
164  // copy data from model into this object
165  if (model){
166  this->links_ = model->links_;
167  this->joints_ = model->joints_;
168  this->materials_ = model->materials_;
169  this->name_ = model->name_;
170  this->root_link_ = model->root_link_;
171  return true;
172  }
173  else
174  return false;
175 }
176 
177 
178 }// namespace urdf
File could not be opened.
Definition: system.h:53
Fawkes library namespace.
Tried to to parse Collada data which is not supported.
Definition: exceptions.h:59
Definition: model.cpp:69
Failed to parse XML Element.
Definition: exceptions.h:83
Failed to parse XML Document.
Definition: exceptions.h:71