Improved mesh parsing to include normals, vertex color, material and texture (#490)

johnwason · Levi-Armstrong · commit 1c98a3341137 · 2021-01-07T10:53:49.000-06:00
diff --git a/tesseract_python/swig/tesseract_common_python.i b/tesseract_python/swig/tesseract_common_python.i
@@ -87,6 +87,7 @@
 
 %tesseract_aligned_vector(VectorIsometry3d, Eigen::Isometry3d);
 %template(VectorVector3d) std::vector<Eigen::Vector3d>;
+%tesseract_aligned_vector(VectorVector2d, Eigen::Vector2d);
 %tesseract_aligned_vector(VectorVector4d, Eigen::Vector4d);
 %tesseract_aligned_map(TransformMap, std::string, Eigen::Isometry3d);
 
@@ -106,6 +107,7 @@ using name = std::map<Key, Value, std::less<Key>, Eigen::aligned_allocator<std::
 namespace tesseract_common
 {
 %tesseract_aligned_vector_using(VectorIsometry3d, Eigen::Isometry3d);
+%tesseract_aligned_vector_using(VectorVector2d, Eigen::Vector2d);
 %tesseract_aligned_vector_using(VectorVector4d, Eigen::Vector4d);
 %tesseract_aligned_map_using(TransformMap, std::string, Eigen::Isometry3d);
 }
diff --git a/tesseract_python/swig/tesseract_geometry_python.i b/tesseract_python/swig/tesseract_geometry_python.i
@@ -42,6 +42,81 @@
 #include <tesseract_geometry/mesh_parser.h>
 %}
 
+// Define typemaps for types used by Mesh classes
+
+%define %tesseract_vector_eigen_shared_ptr_adaptor(TYPE)
+%typemap(in, noblock=0) std::shared_ptr<const TYPE > (void  *argp = 0, int res = 0, TYPE* temp1) {
+
+  // Override for "in" std::shared_ptr<const TYPE> by value instead of shared_ptr
+  res = SWIG_ConvertPtr($input, &argp,$descriptor(TYPE*), $disown | %convertptr_flags);
+  if (!SWIG_IsOK(res)) { 
+    %argument_fail(res, "$type", $symname, $argnum); 
+  }
+   temp1 = %reinterpret_cast(argp, TYPE*);
+   
+   $1 = std::make_shared< TYPE >(*temp1);
+}
+
+%typemap(typecheck,precedence=SWIG_TYPECHECK_POINTER,noblock=1) std::shared_ptr<const TYPE > {
+  void *vptr = 0;
+  int res = SWIG_ConvertPtr($input, &vptr, $descriptor(TYPE&), SWIG_POINTER_NO_NULL);
+  $1 = SWIG_CheckState(res);
+}
+
+%typemap(out) std::shared_ptr<const TYPE> const & {
+    // Override to return std::shared_ptr<const TYPE> by value instead of shared_ptr
+    if (!*$1)
+    {
+        %set_output(SWIG_Py_Void());  
+    }
+    else
+    {
+        const TYPE& temp_out = **$1;
+        %set_output(SWIG_NewPointerObj(%new_copy(temp_out,TYPE), $descriptor(TYPE&), SWIG_POINTER_OWN | %newpointer_flags));
+    }
+}
+%enddef
+
+%tesseract_vector_eigen_shared_ptr_adaptor(%arg(std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> >))
+%tesseract_vector_eigen_shared_ptr_adaptor(std::vector< Eigen::Vector3d >)
+%tesseract_vector_eigen_shared_ptr_adaptor(%arg(std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> >))
+%tesseract_vector_eigen_shared_ptr_adaptor(std::vector< std::shared_ptr<tesseract_geometry::MeshTexture> >)
+
+%define %tesseract_eigen_shared_ptr_adaptor(TYPE)
+
+%typemap(in, fragment="Eigen_Fragments") std::shared_ptr<const TYPE > (TYPE temp)
+{
+  // Override for "in" TYPE by value instead of shared_ptr
+  if (!ConvertFromNumpyToEigenMatrix< TYPE >(&temp, $input))
+    SWIG_fail;
+  $1 = std::make_shared< TYPE >(temp);
+}
+
+%typemap(typecheck,precedence=SWIG_TYPECHECK_POINTER,noblock=1) std::shared_ptr<const TYPE >
+{
+$1 = is_array($input);
+}
+
+%typemap(out, fragment="Eigen_Fragments") std::shared_ptr<const TYPE > const & {
+
+    // Override to return TYPE by value instead of shared_ptr
+    if (!*$1)
+    {
+        %set_output(SWIG_Py_Void());
+    }
+    else
+    {
+        TYPE temp_out = **$1;
+        if (!ConvertFromEigenToNumPyMatrix< TYPE >(&$result, &temp_out))
+            SWIG_fail;
+    }
+}
+%enddef
+
+%tesseract_eigen_shared_ptr_adaptor( Eigen::VectorXi )
+
+
+
 // tesseract_geometry
 #define TESSERACT_GEOMETRY_PUBLIC
 %include "tesseract_geometry/geometry.h"
diff --git a/tesseract_viewer_python/tesseract_viewer/resources/static/tesseract_viewer.js b/tesseract_viewer_python/tesseract_viewer/resources/static/tesseract_viewer.js
@@ -45,6 +45,7 @@ class TesseractViewer {
             this._camera.setPosition(new BABYLON.Vector3(2.5, 1.5, -1));
             // Add lights to the scene
             this._light = new BABYLON.HemisphericLight("light1", new BABYLON.Vector3(0, -1, 0), this._scene);
+            this._light.intensity = 0.5
             this._root = new BABYLON.TransformNode("root0");
             this._root.rotation.x = -1.5707963267948966;
             yield this.updateScene();
diff --git a/tesseract_viewer_python/tesseract_viewer/resources/static/tesseract_viewer.ts b/tesseract_viewer_python/tesseract_viewer/resources/static/tesseract_viewer.ts
@@ -57,6 +57,7 @@ class TesseractViewer {
 
         // Add lights to the scene
         this._light = new BABYLON.HemisphericLight("light1", new BABYLON.Vector3(0, -1, 0), this._scene);
+        this._light.intensity = 0.5
                     
         this._root = new BABYLON.TransformNode("root0");
         
diff --git a/tesseract_viewer_python/tesseract_viewer/tesseract_env_to_babylon_json.py b/tesseract_viewer_python/tesseract_viewer/tesseract_env_to_babylon_json.py
@@ -21,6 +21,8 @@
 from tesseract import tesseract_geometry
 from tesseract.tesseract_common import Quaterniond
 import pkgutil
+import re
+import base64
 
 def tesseract_env_to_babylon_json(t_env, origin_offset=[0,0,0]):
     return json.dumps(tesseract_env_to_babylon_json_dict(t_env,origin_offset))
@@ -74,8 +76,12 @@ def _process_link_recursive(link_map, joint_map, link_name, parent_joint_name):
         tf_link["parentId"] = "root"
     transform_nodes.append(tf_link)
     
+    visual_i = 0
+
     for visual in link.visual:
-        visual_name = "link_" + link_name + "_visual_" + visual.name
+        visual_i += 1
+        visual_u_name = visual.name + str(visual_i)
+        visual_name = "link_" + link_name + "_visual_" + visual_u_name
         np_tf_visual = visual.origin
         visual_p, visual_q = _np_transform_to_babylon(np_tf_visual)
         tf_visual = {"name": visual_name, "isVisible": "true", "isEnabled": "true", 
@@ -84,6 +90,8 @@ def _process_link_recursive(link_map, joint_map, link_name, parent_joint_name):
         tf_visual["rotationQuaternion"] = visual_q
         tf_visual["billboardMode"] = 0
 
+        tf_material = None
+
         visual_geom = visual.geometry
         if (isinstance(visual_geom,tesseract_geometry.Mesh)):
         
@@ -115,6 +123,57 @@ def _process_link_recursive(link_map, joint_map, link_name, parent_joint_name):
             submesh["indexStart"] = 0
             submesh["indexCount"] = len(indices)
             tf_visual["subMeshes"] = [submesh]
+
+            if not mesh.getResource().getUrl().lower().endswith('.stl'):
+                mesh_material = mesh.getMaterial()
+                if mesh_material is not None:
+                
+                    tf_material = {"name": "material_" + visual_name}
+                
+                    color = mesh_material.getBaseColorFactor().flatten()
+                    tf_color = [color[0], color[1], color[2]]
+                    tf_color2 = [color[0]*0.5, color[1]*0.5, color[2]*0.5]
+
+                    """tf_material["ambient"] = tf_color
+                    tf_material["diffuse"] = tf_color
+                    tf_material["specular"] = tf_color2
+                    tf_material["alpha"] = 1
+                    tf_material["backFaceCulling"] = True"""
+                    
+                    tf_material["customType"] = "BABYLON.PBRMaterial"
+                    tf_material["albedo"] = tf_color
+                    tf_material["alpha"] = color[3]
+                    tf_material["alphaCutOff"] = 0.4
+                    tf_material["backFaceCulling"] = True
+                    tf_material["roughness"] = mesh_material.getRoughnessFactor()
+                    tf_material["metallic"] = mesh_material.getMetallicFactor()
+                    tf_material["maxSimultaneousLights"] = 4
+                    tf_material["environmentIntensity"]  = 0.1
+
+                    mesh_textures = mesh.getTextures()
+                    if mesh_textures is not None and len(mesh_textures) > 0:
+                        mesh_tex = mesh_textures[0]
+                        mesh_tex_image = mesh_tex.getTextureImage()
+                        tex_name = mesh_tex_image.getUrl()
+                        tex_mimetype = "image/jpg"
+                        if tex_name.endswith('png'):
+                            tex_mimetype = "image/png"
+                        tex_data = base64.b64encode(bytearray(mesh_tex_image.getResourceContents())).decode('ascii')
+                        tex_data_url = "data:" + tex_mimetype + ";base64," + tex_data
+                        
+                        tf_texture = {"name": "material_texture_" + visual_name, "url":  "material_texture_" + visual_name, "level": 1,"hasAlpha": True,"coordinatesMode":0,"uOffset":0,"vOffset":0,"uScale":1,\
+                            "vScale":1,"uAng":0,"vAng":0,"wAng":0,"wrapU":0,"wrapV":0,"coordinatesIndex":0, "isRenderTarget":False, \
+                            "base64String": tex_data_url}
+
+                        #tf_material["useAlphaFromAlbedoTexture"] = True
+                        tf_material["albedoTexture"] = tf_texture
+                        mesh_uvs = mesh_tex.getUVs()
+                        tf_uvs = [0]*(len(mesh_uvs)*2)
+                        for i in range(len(mesh_uvs)):
+                            mesh_uv = mesh_uvs[i].flatten()
+                            tf_uvs[i*2] = mesh_uv[0]
+                            tf_uvs[i*2+1] = mesh_uv[1]
+                        tf_visual["uvs"] = tf_uvs
             
             
         elif (isinstance(visual_geom,tesseract_geometry.Box)):
@@ -160,21 +219,22 @@ def _process_link_recursive(link_map, joint_map, link_name, parent_joint_name):
 
         meshes.append(tf_visual)
 
-        tf_material = {"name": "material_" + visual_name}
+        if tf_material is None:
+            tf_material = {"name": "material_" + visual_name}
 
-        material = visual.material
+            material = visual.material
 
-        if material is None:
-            tf_color = [0.5,0.5,0.5]
-        else:
-            color = material.color.flatten()
-            tf_color = [color[0], color[1], color[2]]
-
-        tf_material["ambient"] = tf_color
-        tf_material["diffuse"] = tf_color
-        tf_material["specular"] = tf_color
-        tf_material["alpha"] = 1
-        tf_material["backFaceCulling"] = True
+            if material is None:
+                tf_color = [0.5,0.5,0.5]
+            else:
+                color = material.color.flatten()
+                tf_color = [color[0], color[1], color[2]]
+
+            tf_material["ambient"] = tf_color
+            tf_material["diffuse"] = tf_color
+            tf_material["specular"] = tf_color
+            tf_material["alpha"] = 1
+            tf_material["backFaceCulling"] = True
 
         materials.append(tf_material)
 
diff --git a/tesseract_viewer_python/tesseract_viewer/tesseract_viewer.py b/tesseract_viewer_python/tesseract_viewer/tesseract_viewer.py
@@ -131,7 +131,7 @@ def update_joint_positions(self, joint_names, joint_positions):
         # Create "infinite" animation with constant joint angles
 
         trajectory_json = dict()
-        trajectory_json["use_time"] = False
+        trajectory_json["use_time"] = True
         trajectory_json["loop_time"] = 10000
 
         assert joint_names and all(isinstance(s,str) for s in joint_names), "Joint names must all be strings"

Original file line number	Diff line number	Diff line change
`@@ -87,6 +87,7 @@`
`87`	`87`
`88`	`88`	`%tesseract_aligned_vector(VectorIsometry3d, Eigen::Isometry3d);`
`89`	`89`	`%template(VectorVector3d) std::vector<Eigen::Vector3d>;`
	`90`	`+%tesseract_aligned_vector(VectorVector2d, Eigen::Vector2d);`
`90`	`91`	`%tesseract_aligned_vector(VectorVector4d, Eigen::Vector4d);`
`91`	`92`	`%tesseract_aligned_map(TransformMap, std::string, Eigen::Isometry3d);`
`92`	`93`
`@@ -106,6 +107,7 @@ using name = std::map<Key, Value, std::less<Key>, Eigen::aligned_allocator<std::`
`106`	`107`	`namespace tesseract_common`
`107`	`108`	`{`
`108`	`109`	`%tesseract_aligned_vector_using(VectorIsometry3d, Eigen::Isometry3d);`
	`110`	`+%tesseract_aligned_vector_using(VectorVector2d, Eigen::Vector2d);`
`109`	`111`	`%tesseract_aligned_vector_using(VectorVector4d, Eigen::Vector4d);`
`110`	`112`	`%tesseract_aligned_map_using(TransformMap, std::string, Eigen::Isometry3d);`
`111`	`113`	`}`