From 19bf8c222c79cd75bdd1a930ed24bb03c05c9ba5 Mon Sep 17 00:00:00 2001
From: John Hsu <johnhsu@willowgarage.com>
Date: Sat, 23 Apr 2011 18:20:36 -0700
Subject: [PATCH] check for missing mesh filenames #4864:
 https://code.ros.org/trac/ros-pkg/ticket/4864

---
 urdf_parser/include/urdf_parser/link.h |  1 +
 urdf_parser/src/link.cpp               | 36 ++++++++++++++++++++++++++
 2 files changed, 37 insertions(+)

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 <ros/ros.h>
+#include <ros/package.h>
+#include <fstream>
 #include <boost/lexical_cast.hpp>
 #include <algorithm>
 
@@ -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")))