Merge pull request #84 from compas-dev/fix/dae-and-multi-mesh-support

Add support for DAE files and multiple meshes per mesh descriptor

Author: gonzalocasas

CHANGELOG.rst

@@ -24,6 +24,7 @@ Unreleased
 
 * Use WorldXY's origin as default for robots that are have no parent join on their base
 * Fixed parsing of semantics (SRDF) containing nested groups
+* Fixed DAE support on ROS File loader
 
 **Deprecated**
 

docs/backends/files/panda-demo/docker-compose.yml

@@ -38,6 +38,29 @@ services:
       - file_server
       - file_server.launch
 
+  panda-tf-panda_link0:
+    image: gramaziokohler/ros-panda-planner:19.04
+    container_name: panda-tf-panda_link0
+    environment:
+      - ROS_HOSTNAME=panda-tf-panda_link0
+      - ROS_MASTER_URI=http://ros-master:11311
+    depends_on:
+      - ros-master
+    command:
+      - rosrun
+      - tf
+      - static_transform_publisher
+      - '0'
+      - '0'
+      - '0'
+      - '0'
+      - '0'
+      - '0'
+      - '1'
+      - world
+      - panda_link0
+      - '100'
+
   panda-demo:
     image: gramaziokohler/ros-panda-planner:19.04
     container_name: panda-demo

src/compas_fab/artists/base.py

@@ -89,14 +89,34 @@ def create(self, link=None):
             link = self.robot.root
 
         for item in itertools.chain(link.visual, link.collision):
-            if item.geometry.geo:
+            # NOTE: Currently, shapes assign their meshes to an
+            # attribute called `geometry`, but this will change soon to `meshes`.
+            # This code handles the situation in a forward-compatible
+            # manner. Eventually, this can be simplified to use only `meshes` attr
+            if hasattr(item.geometry.shape, 'meshes'):
+                meshes = item.geometry.shape.meshes
+            else:
+                meshes = item.geometry.shape.geometry
+
+            if meshes:
+                # Coerce meshes into an iteratable (a tuple if not natively iterable)
+                if not hasattr(meshes, '__iter__'):
+                    meshes = (meshes,)
+
                 color = None
                 if hasattr(item, 'get_color'):
                     color = item.get_color()
-                # create native geometry
-                item.native_geometry = self.draw_geometry(item.geometry.geo, color)
-                # transform native geometry based on saved init transform
-                self.transform(item.native_geometry, item.init_transformation)
+
+                native_geometry = []
+                for mesh in meshes:
+                    # create native geometry
+                    native_mesh = self.draw_geometry(mesh, color)
+                    # transform native geometry based on saved init transform
+                    self.transform(native_mesh, item.init_transformation)
+                    # append to list
+                    native_geometry.append(native_mesh)
+
+                item.native_geometry = native_geometry
                 item.current_transformation = Transformation()
 
         for child_joint in link.joints:
@@ -128,7 +148,8 @@ def scale_link(self, link, transformation):
             # Some links have only collision geometry, not visual. These meshes
             # have not been loaded.
             if item.native_geometry:
-                self.transform(item.native_geometry, transformation)
+                for geometry in item.native_geometry:
+                    self.transform(geometry, transformation)
 
         for child_joint in link.joints:
             # Recursive call
@@ -152,7 +173,8 @@ def _apply_transformation_on_transformed_link(self, item, transformation):
         None
         """
         relative_transformation = transformation * item.current_transformation.inverse()
-        self.transform(item.native_geometry, relative_transformation)
+        for native_geometry in item.native_geometry:
+            self.transform(native_geometry, relative_transformation)
         item.current_transformation = transformation
 
     def update(self, configuration, names, visual=True, collision=True):
@@ -190,11 +212,14 @@ def draw_visual(self):
         """Draws all visual geometry of the robot."""
         for link in self.robot.iter_links():
             for item in link.visual:
-                yield item.native_geometry
+                if item.native_geometry:
+                    for native_geometry in item.native_geometry:
+                        yield native_geometry
 
     def draw_collision(self):
         """Draws all collision geometry of the robot."""
         for link in self.robot.iter_links():
             for item in link.collision:
                 if item.native_geometry:
-                    yield item.native_geometry
+                    for native_geometry in item.native_geometry:
+                        yield native_geometry

src/compas_fab/backends/ros/fileserver_loader.py

@@ -6,14 +6,17 @@
 import logging
 import os
 import tempfile
+from collections import OrderedDict
 
 import roslibpy
 from compas.datastructures import Mesh
-from compas.datastructures import meshes_join
+from compas.datastructures import mesh_transform
 from compas.files import XML
+from compas.geometry import Transformation
 from compas.robots.resources.basic import _get_file_format
 from compas.robots.resources.basic import _mesh_import
 from compas.utilities import await_callback
+from compas.utilities import geometric_key
 
 LOGGER = logging.getLogger('compas_fab.backends.ros')
 
@@ -61,14 +64,17 @@ class RosFileServerLoader(object):
     local_cache_directory : str, optional
         Directory name to store the cached files. Only used if
         ``local_cache`` is ``True``. Defaults to ``~/robot_description``.
+    precision : float
+        Defines precision for importing/loading meshes. Defaults to ``compas.PRECISION``.
     """
 
-    def __init__(self, ros=None, local_cache=False, local_cache_directory=None):
+    def __init__(self, ros=None, local_cache=False, local_cache_directory=None, precision=None):
         self.robot_name = None
         self.schema_prefix = 'package://'
         self.ros = ros
         self.local_cache_directory = None
         self.local_cache_enabled = local_cache
+        self.precision = precision
 
         if self.local_cache_enabled:
             self.local_cache_directory = local_cache_directory or os.path.join(
@@ -184,7 +190,7 @@ def load_mesh(self, url):
 
         Returns
         -------
-        :class:`Mesh`
+        :class:`Mesh` or list of :class:`Mesh`
             Instance of a mesh.
         """
         use_local_file = False
@@ -217,61 +223,85 @@ def load_mesh(self, url):
             # Nothing to do here, the file will be read by the mesh importer
             LOGGER.debug('Loading mesh file %s from local cache dir', local_filename)
 
-        return _fileserver_mesh_import(url, local_filename)
+        return _fileserver_mesh_import(url, local_filename, self.precision)
 
     def _local_mesh_filename(self, url):
         return os.path.abspath(os.path.join(self._robot_resource_path, url[len('package://'):]))
 
 
-def _dae_mesh_importer(filename):
-    """This is a very simple implementation of a DAE/Collada parser.
-    It merges all solids of the DAE file into one mesh, because
-    several other parts of the framework don't support multi-meshes per file."""
+def _dae_mesh_importer(filename, precision):
+    """This is a very simple implementation of a DAE/Collada parser."""
     dae = XML.from_file(filename)
     meshes = []
+    visual_scenes = dae.root.find('library_visual_scenes')
 
-    for mesh_xml in dae.root.findall('.//mesh'):
-        for triangle_set in mesh_xml.findall('triangles'):
-            triangle_set_data = triangle_set.find('p').text.split()
+    for geometry in dae.root.findall('.//geometry'):
+        mesh_xml = geometry.find('mesh')
+        mesh_id = geometry.attrib['id']
+        matrix_node = visual_scenes.find('visual_scene/node/instance_geometry[@url="#{}"]/../matrix'.format(mesh_id))
+        transform = None
 
-            # Parse vertices
-            vertices_input = triangle_set.find('input[@semantic="VERTEX"]')
-            vertices_link = mesh_xml.find('vertices[@id="{}"]/input'.format(vertices_input.attrib['source'][1:]))
-            positions = mesh_xml.find('source[@id="{}"]/float_array'.format(vertices_link.attrib['source'][1:]))
-            positions = positions.text.split()
+        if matrix_node is not None:
+            M = [float(i) for i in matrix_node.text.split()]
 
-            vertices = list(map(float, positions[i:i + 3]) for i in range(0, len(positions), 3))
+            # If it's the identity matrix, then ignore, we don't need to transform it
+            if M != [1., 0., 0., 0.,
+                     0., 1., 0., 0.,
+                     0., 0., 1., 0.,
+                     0., 0., 0., 1.]:
+                M = M[0:4], M[4:8], M[8:12], M[12:16]
+                transform = Transformation.from_matrix(M)
 
-            # Parse faces
-            faces = list(map(int, triangle_set_data[::2]))  # Ignore normals (ever second item is normal index)
-            faces = list(faces[i:i + 3] for i in range(0, len(faces), 3))
+        triangle_set = mesh_xml.find('triangles')
+        triangle_set_data = triangle_set.find('p').text.split()
 
-            mesh = Mesh.from_vertices_and_faces(vertices, faces)
+        # Parse vertices
+        vertices_input = triangle_set.find('input[@semantic="VERTEX"]')
+        vertices_link = mesh_xml.find('vertices[@id="{}"]/input'.format(vertices_input.attrib['source'][1:]))
+        positions = mesh_xml.find('source[@id="{}"]/float_array'.format(vertices_link.attrib['source'][1:]))
+        positions = positions.text.split()
 
-            meshes.append(mesh)
+        vertices = [[float(p) for p in positions[i:i + 3]] for i in range(0, len(positions), 3)]
 
-    combined_mesh = meshes_join(meshes)
+        # Parse faces
+        faces = [int(f) for f in triangle_set_data[::2]]  # Ignore normals (every second item is normal index)
+        faces = [faces[i:i + 3] for i in range(0, len(faces), 3)]
 
-    # from compas.datastructures import mesh_transform
-    # from compas.geometry import Frame
-    # from compas.geometry import Transformation
+        # Rebuild vertices and faces using the same logic that other importers
+        # use remapping everything based on a selected precision
+        index_key = OrderedDict()
+        vertex = OrderedDict()
 
-    # former DAE files have yaxis and zaxis swapped
-    # frame = Frame([0, 0, 0], [1, 0, 0], [0, 0, 1])
-    # T = Transformation.from_frame(frame)
-    # mesh_transform(combined_mesh, T)
-    return combined_mesh
+        for i, xyz in enumerate(vertices):
+            key = geometric_key(xyz, precision)
+            index_key[i] = key
+            vertex[key] = xyz
 
+        key_index = {key: index for index, key in enumerate(vertex)}
+        index_index = {index: key_index[key] for index, key in iter(index_key.items())}
+        vertices = [xyz for xyz in iter(vertex.values())]
+        faces = [[index_index[index] for index in face] for face in faces]
 
-def _fileserver_mesh_import(url, filename):
+        mesh = Mesh.from_vertices_and_faces(vertices, faces)
+
+        if transform:
+            mesh_transform(mesh, transform)
+
+        meshes.append(mesh)
+
+    return meshes
+
+
+def _fileserver_mesh_import(url, filename, precision=None):
     """Internal function that adds primitive support for DAE files
     to the _mesh_import function of compas.robots."""
     file_extension = _get_file_format(url)
 
     if file_extension == 'dae':
         # Magic!
-        return _dae_mesh_importer(filename)
+        return _dae_mesh_importer(filename, precision)
     else:
+        # TODO: This _mesh_import should also add support for precision
         return _mesh_import(url, filename)