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
66using namespace fawkes;
67
68namespace urdf {
69
70static bool
71IsColladaData(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 */
88bool
89Model::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 */
111bool
112Model::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 */
128bool
129Model::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 */
145bool
146Model::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
File could not be opened.
Definition: system.h:53
Tried to to parse Collada data which is not supported.
Definition: exceptions.h:60
Failed to parse XML Document.
Definition: exceptions.h:72
Failed to parse XML Element.
Definition: exceptions.h:84
bool initFile(const std::string &filename)
Initialize the Model using a URDF file.
Definition: model.cpp:89
bool initString(const std::string &xmlstring)
Initialize the model using an URDF string.
Definition: model.cpp:146
bool initXml(TiXmlElement *xml)
Initialize the model using a XML Element.
Definition: model.cpp:129
Fawkes library namespace.