rdf_loader.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan, Mathias L├╝dtke, Dave Coleman */
36 
37 // MoveIt!
40 
41 // ROS
42 #include <ros/ros.h>
43 #include <ros/package.h>
44 
45 // Boost
46 #include <boost/filesystem.hpp>
47 
48 // C++
49 #include <fstream>
50 #include <streambuf>
51 #include <algorithm>
52 
53 rdf_loader::RDFLoader::RDFLoader(const std::string& robot_description)
54 {
56  moveit::tools::Profiler::ScopedBlock prof_block("RDFLoader(robot_description)");
57 
59  ros::NodeHandle nh("~");
60  std::string content;
61 
62  if (!nh.searchParam(robot_description, robot_description_) || !nh.getParam(robot_description_, content))
63  {
64  ROS_ERROR_NAMED("rdf_loader", "Robot model parameter not found! Did you remap '%s'?", robot_description.c_str());
65  return;
66  }
67 
68  urdf::Model* umodel = new urdf::Model();
69  if (!umodel->initString(content))
70  {
71  ROS_ERROR_NAMED("rdf_loader", "Unable to parse URDF from parameter '%s'", robot_description_.c_str());
72  return;
73  }
74  urdf_.reset(umodel);
75 
76  const std::string srdf_description(robot_description_ + "_semantic");
77  std::string scontent;
78  if (!nh.getParam(srdf_description, scontent))
79  {
80  ROS_ERROR_NAMED("rdf_loader", "Robot semantic description not found. Did you forget to define or remap '%s'?",
81  srdf_description.c_str());
82  return;
83  }
84 
85  srdf_.reset(new srdf::Model());
86  if (!srdf_->initString(*urdf_, scontent))
87  {
88  ROS_ERROR_NAMED("rdf_loader", "Unable to parse SRDF from parameter '%s'", srdf_description.c_str());
89  srdf_.reset();
90  return;
91  }
92 
93  ROS_DEBUG_STREAM_NAMED("rdf", "Loaded robot model in " << (ros::WallTime::now() - start).toSec() << " seconds");
94 }
95 
96 rdf_loader::RDFLoader::RDFLoader(const std::string& urdf_string, const std::string& srdf_string)
97 {
99  moveit::tools::Profiler::ScopedBlock prof_block("RDFLoader(string)");
100 
101  urdf::Model* umodel = new urdf::Model();
102  urdf_.reset(umodel);
103  if (umodel->initString(urdf_string))
104  {
105  srdf_.reset(new srdf::Model());
106  if (!srdf_->initString(*urdf_, srdf_string))
107  {
108  ROS_ERROR_NAMED("rdf_loader", "Unable to parse SRDF");
109  srdf_.reset();
110  }
111  }
112  else
113  {
114  ROS_ERROR_NAMED("rdf_loader", "Unable to parse URDF");
115  urdf_.reset();
116  }
117 }
118 
119 rdf_loader::RDFLoader::RDFLoader(TiXmlDocument* urdf_doc, TiXmlDocument* srdf_doc)
120 {
122  moveit::tools::Profiler::ScopedBlock prof_block("RDFLoader(XML)");
123 
124  urdf::Model* umodel = new urdf::Model();
125  urdf_.reset(umodel);
126  if (umodel->initXml(urdf_doc))
127  {
128  srdf_.reset(new srdf::Model());
129  if (!srdf_->initXml(*urdf_, srdf_doc))
130  {
131  ROS_ERROR_NAMED("rdf_loader", "Unable to parse SRDF");
132  srdf_.reset();
133  }
134  }
135  else
136  {
137  ROS_ERROR_NAMED("rdf_loader", "Unable to parse URDF");
138  urdf_.reset();
139  }
140 }
141 
142 bool rdf_loader::RDFLoader::isXacroFile(const std::string& path)
143 {
144  std::string lower_path = path;
145  std::transform(lower_path.begin(), lower_path.end(), lower_path.begin(), ::tolower);
146 
147  return lower_path.find(".xacro") != std::string::npos;
148 }
149 
150 bool rdf_loader::RDFLoader::loadFileToString(std::string& buffer, const std::string& path)
151 {
152  if (path.empty())
153  {
154  ROS_ERROR_NAMED("rdf_loader", "Path is empty");
155  return false;
156  }
157 
158  if (!boost::filesystem::exists(path))
159  {
160  ROS_ERROR_NAMED("rdf_loader", "File does not exist");
161  return false;
162  }
163 
164  std::ifstream stream(path.c_str());
165  if (!stream.good())
166  {
167  ROS_ERROR_NAMED("rdf_loader", "Unable to load path");
168  return false;
169  }
170 
171  // Load the file to a string using an efficient memory allocation technique
172  stream.seekg(0, std::ios::end);
173  buffer.reserve(stream.tellg());
174  stream.seekg(0, std::ios::beg);
175  buffer.assign((std::istreambuf_iterator<char>(stream)), std::istreambuf_iterator<char>());
176  stream.close();
177 
178  return true;
179 }
180 
181 bool rdf_loader::RDFLoader::loadXacroFileToString(std::string& buffer, const std::string& path,
182  const std::vector<std::string>& xacro_args)
183 {
184  if (path.empty())
185  {
186  ROS_ERROR_NAMED("rdf_loader", "Path is empty");
187  return false;
188  }
189 
190  if (!boost::filesystem::exists(path))
191  {
192  ROS_ERROR_NAMED("rdf_loader", "File does not exist");
193  return false;
194  }
195 
196  std::string cmd = "rosrun xacro xacro --inorder ";
197  for (std::vector<std::string>::const_iterator it = xacro_args.begin(); it != xacro_args.end(); ++it)
198  cmd += *it + " ";
199  cmd += path;
200 
201  FILE* pipe = popen(cmd.c_str(), "r");
202  if (!pipe)
203  {
204  ROS_ERROR_NAMED("rdf_loader", "Unable to load path");
205  return false;
206  }
207 
208  char pipe_buffer[128];
209  while (!feof(pipe))
210  {
211  if (fgets(pipe_buffer, 128, pipe) != NULL)
212  buffer += pipe_buffer;
213  }
214  pclose(pipe);
215 
216  return true;
217 }
218 
219 bool rdf_loader::RDFLoader::loadXmlFileToString(std::string& buffer, const std::string& path,
220  const std::vector<std::string>& xacro_args)
221 {
222  if (isXacroFile(path))
223  {
224  return loadXacroFileToString(buffer, path, xacro_args);
225  }
226 
227  return loadFileToString(buffer, path);
228 }
229 
230 bool rdf_loader::RDFLoader::loadPkgFileToString(std::string& buffer, const std::string& package_name,
231  const std::string& relative_path,
232  const std::vector<std::string>& xacro_args)
233 {
234  std::string package_path = ros::package::getPath(package_name);
235 
236  if (package_path.empty())
237  {
238  ROS_ERROR_STREAM_NAMED("rdf_loader", "ROS was unable to find the package name '" << package_name << "'");
239  return false;
240  }
241 
242  boost::filesystem::path path(package_path);
243  // Use boost to cross-platform combine paths
244  path = path / relative_path;
245 
246  return loadXmlFileToString(buffer, path.string(), xacro_args);
247 }
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
bool initString(const std::string &xmlstring)
bool initXml(TiXmlElement *xml)
static bool loadPkgFileToString(std::string &buffer, const std::string &package_name, const std::string &relative_path, const std::vector< std::string > &xacro_args)
helper that generates a file path based on package name and relative file path to package ...
Definition: rdf_loader.cpp:230
static bool isXacroFile(const std::string &path)
determine if given path points to a xacro file
Definition: rdf_loader.cpp:142
RDFLoader(const std::string &robot_description="robot_description")
Default constructor.
Definition: rdf_loader.cpp:53
static bool loadFileToString(std::string &buffer, const std::string &path)
load file from given path into buffer
Definition: rdf_loader.cpp:150
bool searchParam(const std::string &key, std::string &result) const
ROSLIB_DECL std::string getPath(const std::string &package_name)
srdf::ModelSharedPtr srdf_
Definition: rdf_loader.h:106
static WallTime now()
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_NAMED(name,...)
static bool loadXmlFileToString(std::string &buffer, const std::string &path, const std::vector< std::string > &xacro_args)
helper that branches between loadFileToString() and loadXacroFileToString() based on result of isXacr...
Definition: rdf_loader.cpp:219
urdf::ModelInterfaceSharedPtr urdf_
Definition: rdf_loader.h:107
std::string robot_description_
Definition: rdf_loader.h:105
static bool loadXacroFileToString(std::string &buffer, const std::string &path, const std::vector< std::string > &xacro_args)
run xacro with the given args on the file, return result in buffer
Definition: rdf_loader.cpp:181


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Jan 15 2018 03:51:18