From b589a797c55ec8e58522c1259c6d0497c3d45898 Mon Sep 17 00:00:00 2001
From: Lou Amadio <lou@polyhobbyist.com>
Date: Mon, 25 Sep 2023 15:38:07 -0700
Subject: [PATCH] Finish handling complex dae

---
 src/GeometryMesh.ts                    | 13 ++--
 src/Render.ts                          | 82 +++++++++++++++++++++++++-
 test/testdata/basic_with_stl_mesh.urdf |  2 +-
 test/testdata/r2.urdf                  | 12 ++--
 4 files changed, 90 insertions(+), 19 deletions(-)

diff --git a/src/GeometryMesh.ts b/src/GeometryMesh.ts
index 0c3f473..c5013a2 100644
--- a/src/GeometryMesh.ts
+++ b/src/GeometryMesh.ts
@@ -22,16 +22,13 @@ export class Mesh implements IGeometry {
         // Get a pointer to the mesh
         if (meshes.length > 0 && this.transform != undefined) {
             this.meshes = meshes;
+            this.meshes[0].parent = this.transform;
 
             // find the top level bone in skeletons
             if (skeletons != undefined && skeletons.length > 0) {
                 let rootBone = skeletons[0].bones.find(b => b.getParent() == undefined);
                 if (rootBone != undefined) {
-                    let t = rootBone.getTransformNode();
-                    if (t != undefined) {
-                        t.parent = this.transform;
-                        //t.addRotation(0, 0, Math.PI).addRotation(Math.PI/2, 0, 0);
-                    }
+                    rootBone.returnToRest();
                 }
             } else {
 
@@ -39,7 +36,7 @@ export class Mesh implements IGeometry {
                     if (this.transform != undefined) {
                         m.addRotation(0, 0, Math.PI).addRotation(Math.PI/2, 0, 0);
                         m.parent = this.transform;
-                        m.scaling = this.scale;
+                        
                         if (this.material != undefined && this.material.material != undefined) {
                             m.material = this.material.material;
                         }
@@ -52,12 +49,14 @@ export class Mesh implements IGeometry {
 
     public create(scene: BABYLON.Scene, mat : Material | undefined) : void {
         this.transform = new BABYLON.TransformNode("mesh_mesh", scene);
+        this.transform.scaling = this.scale;
+
         this.material = mat;
 
         if (this.uri.startsWith("file://"))
         {
             // Handle relative paths
-            var filePath = this.uri.substring(7);
+            var filePath = this.uri.substring(7); 
             if (!filePath.startsWith("/")) {
                 filePath = path.join(__dirname, filePath);
             }
diff --git a/src/Render.ts b/src/Render.ts
index 993d845..1639edc 100644
--- a/src/Render.ts
+++ b/src/Render.ts
@@ -217,12 +217,88 @@ export async function RenderMain() {
     <link name="base_link">
       <visual>
         <geometry>
-          <mesh filename="https://raw.githubusercontent.com/polyhobbyist/babylon-collada-loader/main/test/testdata/Chassis.dae" scale="1 1 1" />
+          <mesh filename="https://raw.githubusercontent.com/polyhobbyist/bb_description/main/meshes/dome.stl" scale="0.001 0.001 0.001" />
         </geometry>
       </visual>
     </link>
-     </robot>
-  `;
+  
+  
+    <link name="camera_link">
+      <visual>
+        <geometry>
+          <box size="0.01 0.01 0.001"/>
+        </geometry>
+      </visual>
+    </link>
+  
+    <joint name="dome_to_camera_joint" type="fixed">
+      <parent link="base_link"/>
+      <child link="camera_link"/>
+      <origin xyz="0 0.12 0.08" rpy="2.093 0 0"/>
+    </joint>
+  
+    <link name="front_depth_link">
+      <visual>
+        <geometry>
+          <box size="0.01 0.01 0.001"/>
+        </geometry>
+        <material name="red"/>
+      </visual>
+    </link>
+    
+    <joint name="dome_to_front_depth_joint" type="fixed">
+      <parent link="base_link"/>
+      <child link="front_depth_link"/>
+      <origin xyz="0 0.11 0.09" rpy="2.093 0 0"/>
+    </joint>
+  
+    <link name="left_depth_link">
+      <visual>
+        <geometry>
+          <box size="0.01 0.01 0.001"/>
+        </geometry>
+        <material name="red"/>
+      </visual>
+    </link>
+    
+    <joint name="dome_to_left_depth_joint" type="fixed">
+      <parent link="base_link"/>
+      <child link="left_depth_link"/>
+      <origin xyz="0.11693 -0.067 0.05" rpy="1.047198 0.959931 0.436332"/>
+    </joint>
+  
+    <link name="right_depth_link">
+      <visual>
+        <geometry>
+          <box size="0.01 0.01 0.001"/>
+        </geometry>
+        <material name="red"/>
+      </visual>
+    </link>
+    
+    <joint name="dome_to_right_depth_joint" type="fixed">
+      <parent link="base_link"/>
+      <child link="right_depth_link"/>
+      <origin xyz="-0.11693 -0.067 0.05" rpy="1.047198 -0.959931 -0.436332"/>
+    </joint>
+  
+  
+    <link name="depth_link">
+      <visual>
+        <geometry>
+          <sphere radius="0.001"/>
+        </geometry>
+      </visual>
+    </link>
+  
+    <joint name="dome_to_depth_joint" type="fixed">
+      <parent link="base_link"/>
+      <child link="depth_link"/>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+    </joint>
+  
+  </robot>
+    `;
   
   const canvas = document.getElementById("renderCanvas") as HTMLCanvasElement; // Get the canvas element
   const engine = new BABYLON.Engine(canvas, true); // Generate the BABYLON 3D engine
diff --git a/test/testdata/basic_with_stl_mesh.urdf b/test/testdata/basic_with_stl_mesh.urdf
index 16c07f2..d54d1a7 100644
--- a/test/testdata/basic_with_stl_mesh.urdf
+++ b/test/testdata/basic_with_stl_mesh.urdf
@@ -3,7 +3,7 @@
   <link name="base_link">
     <visual>
       <geometry>
-        <mesh filename="file://../test/testdata/xyz_cube.stl" />
+        <mesh filename="https://raw.githubusercontent.com/polyhobbyist/babylon_ros/ea736e27ccd4df44096bc5c05bacaf5171c6b86a/test/testdata/xyz_cybe.stl" />
       </geometry>
     </visual>
   </link>
diff --git a/test/testdata/r2.urdf b/test/testdata/r2.urdf
index bec4c46..85d016d 100644
--- a/test/testdata/r2.urdf
+++ b/test/testdata/r2.urdf
@@ -169,8 +169,7 @@
     <visual>
       <origin rpy="0.0 0 0" xyz="0 0 0"/>
       <geometry>
-        <!-- mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/ -->
-          <cylinder length="0.1" radius="0.035"/>
+        <mesh filename="https://raw.githubusercontent.com/ros/urdf_tutorial/ros1/meshes/l_finger.dae"/>
       </geometry>
     </visual>
   </link>
@@ -184,8 +183,7 @@
     <visual>
       <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
       <geometry>
-        <!-- mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/ -->
-          <cylinder length="0.1" radius="0.035"/>
+        <mesh filename="https://raw.githubusercontent.com/ros/urdf_tutorial/ros1/meshes/l_finger_tip.dae"/>
       </geometry>
     </visual>
   </link>
@@ -199,8 +197,7 @@
     <visual>
       <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
       <geometry>
-        <!-- mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/ -->
-          <cylinder length="0.1" radius="0.035"/>
+        <mesh filename="https://raw.githubusercontent.com/ros/urdf_tutorial/ros1/meshes/l_finger.dae"/>
       </geometry>
     </visual>
   </link>
@@ -214,8 +211,7 @@
     <visual>
       <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
       <geometry>
-        <!-- mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/ -->
-          <cylinder length="0.1" radius="0.035"/>
+        <mesh filename="https://raw.githubusercontent.com/ros/urdf_tutorial/ros1/meshes/l_finger_tip.dae"/>
       </geometry>
     </visual>
   </link>