Merge pull request #35 from yosuke/master

improve reading of ill formatted model file and fix issue

simtrans/sdf.py

@@ -42,6 +42,7 @@
     from .thirdparty import transformations as tf
 import jinja2
 import re
+import tempfile
 from . import model
 from . import collada
 from . import stl
@@ -63,8 +64,19 @@ def read(self, fname, assethandler=None, options=None):
         Read SDF model data given the model file
         '''
         self._assethandler = assethandler
+        fd, sdffile = tempfile.mkstemp(suffix='.sdf')
+        # use gz sdf utility as a filter to beautify input file
+        # also used to convert urdf to sdf
+        try:
+            d = subprocess.check_output(['gz', 'sdf', '-p', utils.resolveFile(fname)])
+            os.write(fd, d)
+        finally:
+            os.close(fd)
+        try:
+            d = lxml.etree.parse(open(sdffile))
+        finally:
+            os.unlink(sdffile)
         bm = model.BodyModel()
-        d = lxml.etree.parse(open(utils.resolveFile(fname)))
         dm = d.find('model')
         bm.name = self._rootname = dm.attrib['name']
 
@@ -245,8 +257,8 @@ def readShape(self, d):
                 continue
             if g.tag == 'mesh':
                 m.shapeType = model.ShapeModel.SP_MESH
-                # print "reading mesh " + mesh.attrib['filename']
                 filename = utils.resolveFile(g.find('uri').text)
+                logging.info("reading mesh " + filename)
                 fileext = os.path.splitext(filename)[1].lower()
                 if fileext == '.dae':
                     reader = collada.ColladaReader()

simtrans/urdf.py

@@ -42,7 +42,6 @@
     from .thirdparty import transformations as tf
 import jinja2
 import uuid
-import tempfile
 import subprocess
 import logging
 from . import model
@@ -64,17 +63,8 @@ def read(self, fname, assethandler=None, options=None):
         Read simulation model in urdf format
         (internally convert to sdf using gz sdf utility)
         '''
-        fd, sdffile = tempfile.mkstemp(suffix='.sdf')
-        try:
-            d = subprocess.check_output(['gz', 'sdf', '-p', utils.resolveFile(fname)])
-            os.write(fd, d)
-        finally:
-            os.close(fd)
-        try:
-            reader = sdf.SDFReader()
-            m = reader.read(sdffile, assethandler)
-        finally:
-            os.unlink(sdffile)
+        reader = sdf.SDFReader()
+        m = reader.read(fname, assethandler)
         return m
 
     def read2(self, fname, assethandler=None, options=None):

simtrans/vrml.py

@@ -364,7 +364,23 @@ def write(self, mdata, fname, options=None):
                 mdata.joints.append(nj)
                 j.jointType = model.JointModel.J_REVOLUTE
                 j.child = nl.name
-
+
+        # check for same names in visuals or collisions
+        usednames = {}
+        for l in mdata.links:
+            for v in l.visuals:
+                if v.name in usednames:
+                    v.name = l.name + "-visual"
+                    if v.name in usednames:
+                        v.name = l.name + "-visual-" + str(uuid.uuid1()).replace('-', '')
+                usednames[v.name] = True
+            for c in l.collisions:
+                if c.name in usednames:
+                    c.name = l.name + "-collision"
+                    if c.name in usednames:
+                        c.name = l.name + "-collision-" + str(uuid.uuid1()).replace('-', '')
+                usednames[c.name] = True
+
         # find root joint (including local peaks)
         self._roots = utils.findroot(mdata)