46 #include <boost/filesystem.hpp> 64 ROS_ERROR_NAMED(
"rdf_loader",
"Robot model parameter not found! Did you remap '%s'?", robot_description.c_str());
78 if (!nh.
getParam(srdf_description, scontent))
80 ROS_ERROR_NAMED(
"rdf_loader",
"Robot semantic description not found. Did you forget to define or remap '%s'?",
81 srdf_description.c_str());
88 ROS_ERROR_NAMED(
"rdf_loader",
"Unable to parse SRDF from parameter '%s'", srdf_description.c_str());
144 std::string lower_path = path;
145 std::transform(lower_path.begin(), lower_path.end(), lower_path.begin(), ::tolower);
147 return lower_path.find(
".xacro") != std::string::npos;
158 if (!boost::filesystem::exists(path))
164 std::ifstream stream(path.c_str());
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>());
182 const std::vector<std::string>& xacro_args)
190 if (!boost::filesystem::exists(path))
196 std::string
cmd =
"rosrun xacro xacro ";
197 for (std::vector<std::string>::const_iterator it = xacro_args.begin(); it != xacro_args.end(); ++it)
201 FILE* pipe = popen(cmd.c_str(),
"r");
208 char pipe_buffer[128];
211 if (fgets(pipe_buffer, 128, pipe) != NULL)
212 buffer += pipe_buffer;
220 const std::vector<std::string>& xacro_args)
231 const std::string& relative_path,
232 const std::vector<std::string>& xacro_args)
236 if (package_path.empty())
242 boost::filesystem::path path(package_path);
244 path = path / relative_path;
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
URDF_EXPORT bool initString(const std::string &xmlstring)
URDF_EXPORT 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 ...
static bool isXacroFile(const std::string &path)
determine if given path points to a xacro file
RDFLoader(const std::string &robot_description="robot_description")
Default constructor.
static bool loadFileToString(std::string &buffer, const std::string &path)
load file from given path into buffer
bool searchParam(const std::string &key, std::string &result) const
ROSLIB_DECL std::string getPath(const std::string &package_name)
srdf::ModelSharedPtr srdf_
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...
urdf::ModelInterfaceSharedPtr urdf_
std::string robot_description_
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