diff --git a/urdf_parser/include/urdf_parser/link.h b/urdf_parser/include/urdf_parser/link.h index 3731676..96f4528 100644 --- a/urdf_parser/include/urdf_parser/link.h +++ b/urdf_parser/include/urdf_parser/link.h @@ -113,6 +113,7 @@ public: scale.z = 1; }; bool initXml(TiXmlElement *); + bool fileExists(std::string filename); }; class Material diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index 2b78ea8..da39cc1 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -37,6 +37,8 @@ #include "urdf_parser/link.h" #include +#include +#include #include #include @@ -395,6 +397,36 @@ bool Cylinder::initXml(TiXmlElement *c) return true; } +bool Mesh::fileExists(std::string filename) +{ + std::string fullname = filename; + if (fullname.find("package://") == 0) + { + fullname.erase(0, strlen("package://")); + size_t pos = fullname.find("/"); + if (pos == std::string::npos) + { + ROS_FATAL("Could not parse package:// format for [%s]",filename.c_str()); + } + + std::string package = fullname.substr(0, pos); + fullname.erase(0, pos); + std::string package_path = ros::package::getPath(package); + + if (package_path.empty()) + { + ROS_FATAL("%s Package[%s] does not exist",filename.c_str(),package.c_str()); + return false; + } + + fullname = package_path + fullname; + } + std::ifstream fin; fin.open(fullname.c_str(), std::ios::in); fin.close(); + if (fin.fail()) + return false; + return true; +} + bool Mesh::initXml(TiXmlElement *c) { this->clear(); @@ -408,6 +440,10 @@ bool Mesh::initXml(TiXmlElement *c) filename = c->Attribute("filename"); + // check if filename exists, is this really necessary? + if (!fileExists(filename)) + ROS_WARN("filename referred by mesh [%s] does not appear to exist.",filename.c_str()); + if (c->Attribute("scale")) { if (!this->scale.init(c->Attribute("scale")))