Fix CompoundMesh

tesseract_python/swig/tesseract_geometry_python.i

@@ -145,7 +145,8 @@ $1 = is_array($input);
     tesseract_geometry::PolygonMesh,
     tesseract_geometry::Mesh,
     tesseract_geometry::SDFMesh,
-    tesseract_geometry::Sphere
+    tesseract_geometry::Sphere,
+    tesseract_geometry::CompoundMesh
 )
 
 
@@ -194,6 +195,9 @@ class OcTree {};
 %shared_ptr(tesseract_geometry::Sphere)
 %include <tesseract_geometry/impl/sphere.h>
 
+%shared_ptr(tesseract_geometry::CompoundMesh)
+%include <tesseract_geometry/impl/compound_mesh.h>
+
 %include "tesseract_geometry/geometries.h"
 %include "tesseract_geometry/utils.h"
 

tesseract_python/tests/tesseract_environment/test_mesh_material_loading.py

@@ -41,66 +41,71 @@ def get_scene_graph():
 def test_mesh_material_loading():
     scene = get_scene_graph()
     visual = scene.getLink("mesh_dae_link").visual
-    assert len(visual) == 4
+    assert len(visual) == 1
 
-    mesh0 = visual[1].geometry
-    mesh1 = visual[2].geometry
-    mesh2 = visual[3].geometry
-    mesh3 = visual[0].geometry
+    print("Geometry type")
+    print(visual[0].geometry.getType())
 
-    assert mesh0.getFaceCount() == 34
-    assert mesh0.getVertexCount() == 68
-    assert mesh1.getFaceCount() == 15
-    assert mesh1.getVertexCount() == 17
+    meshes = visual[0].geometry.getMeshes()
+
+    mesh0 = meshes[0]
+    mesh1 = meshes[1]
+    mesh2 = meshes[2]
+    mesh3 = meshes[3]
+
+    assert mesh0.getFaceCount() == 2
+    assert mesh0.getVertexCount() == 4
+    assert mesh1.getFaceCount() == 34
+    assert mesh1.getVertexCount() == 68
     assert mesh2.getFaceCount() == 15
     assert mesh2.getVertexCount() == 17
-    assert mesh3.getFaceCount() == 2
-    assert mesh3.getVertexCount() == 4
+    assert mesh3.getFaceCount() == 15
+    assert mesh3.getVertexCount() == 17
 
     mesh0_normals = mesh0.getNormals()
     assert mesh0_normals is not None
-    assert len(mesh0_normals) == 68
+    assert len(mesh0_normals) == 4
     mesh1_normals = mesh1.getNormals()
     assert mesh1_normals is not None
-    assert len(mesh1_normals) ==  17
+    assert len(mesh1_normals) ==  68
     mesh2_normals = mesh2.getNormals()
     assert mesh2_normals is not None
     assert len(mesh2_normals) == 17
     mesh3_normals = mesh3.getNormals()
     assert mesh3_normals is not None
-    assert len(mesh3_normals) == 4
+    assert len(mesh3_normals) == 17
 
     mesh0_material = mesh0.getMaterial()
-    nptest.assert_allclose(mesh0_material.getBaseColorFactor().flatten(),[0.7,0.7,0.7,1], atol=0.01)
+    nptest.assert_allclose(mesh0_material.getBaseColorFactor().flatten(),[1,1,1,1], atol=0.01)
     nptest.assert_almost_equal(mesh0_material.getMetallicFactor(), 0.0)
     nptest.assert_almost_equal(mesh0_material.getRoughnessFactor(), 0.5)
     nptest.assert_allclose(mesh0_material.getEmissiveFactor().flatten(), [0,0,0,1], atol = 0.01)
 
     mesh1_material = mesh1.getMaterial()
-    nptest.assert_allclose(mesh1_material.getBaseColorFactor().flatten(),[0.8,0.0,0.0,1], atol=0.01)
+    nptest.assert_allclose(mesh1_material.getBaseColorFactor().flatten(),[0.7,0.7,0.7,1], atol=0.01)
     nptest.assert_almost_equal(mesh1_material.getMetallicFactor(), 0.0)
     nptest.assert_almost_equal(mesh1_material.getRoughnessFactor(), 0.5)
     nptest.assert_allclose(mesh1_material.getEmissiveFactor().flatten(), [0,0,0,1], atol = 0.01)
 
     mesh2_material = mesh2.getMaterial()
-    nptest.assert_allclose(mesh2_material.getBaseColorFactor().flatten(),[0.05,0.8,0.05,1], atol=0.01)
+    nptest.assert_allclose(mesh2_material.getBaseColorFactor().flatten(),[0.8,0 ,0, 1], atol=0.01)
     nptest.assert_almost_equal(mesh2_material.getMetallicFactor(), 0.0)
     nptest.assert_almost_equal(mesh2_material.getRoughnessFactor(), 0.5)
-    nptest.assert_allclose(mesh2_material.getEmissiveFactor().flatten(), [0.1,0.1,0.5,1], atol = 0.01)
+    nptest.assert_allclose(mesh2_material.getEmissiveFactor().flatten(), [0,0,0,1], atol = 0.01)
 
     mesh3_material = mesh3.getMaterial()
-    nptest.assert_allclose(mesh3_material.getBaseColorFactor().flatten(),[1,1,1,1], atol=0.01)
+    nptest.assert_allclose(mesh3_material.getBaseColorFactor().flatten(),[0.05,0.8,0.05,1], atol=0.01)
     nptest.assert_almost_equal(mesh3_material.getMetallicFactor(), 0.0)
     nptest.assert_almost_equal(mesh3_material.getRoughnessFactor(), 0.5)
-    nptest.assert_allclose(mesh3_material.getEmissiveFactor().flatten(), [0,0,0,1], atol = 0.01)
+    nptest.assert_allclose(mesh3_material.getEmissiveFactor().flatten(), [0.1,0.1,0.5,1], atol = 0.01)
 
-    assert mesh0.getTextures() is None
     assert mesh1.getTextures() is None
-    assert  mesh2.getTextures() is None
+    assert mesh2.getTextures() is None
+    assert  mesh3.getTextures() is None
 
-    assert mesh3.getTextures() is not None
-    assert len(mesh3.getTextures()) == 1
+    assert mesh0.getTextures() is not None
+    assert len(mesh0.getTextures()) == 1
 
-    texture = mesh3.getTextures()[0]
+    texture = mesh0.getTextures()[0]
     assert len(texture.getTextureImage().getResourceContents()) == 38212
     assert len(texture.getUVs()) == 4

tesseract_python/tests/tesseract_task_composer/test_tesseract_task_composer.py

@@ -77,7 +77,6 @@ def freespace_example_progam_iiwa(manipulator_info, goal = None, composite_profi
 
 def test_task_composer_trajopt_example():
 
-    planning_task_problem = None
     output_program = None
     future = None
     task_executor = None