added primitive scaling

Author: micahpearlman

Runtime/Scripts/Util/Extensions/ZOROSConversionExtensions.cs

@@ -1,4 +1,5 @@
 using UnityEngine;
+using System.Collections.Generic;
 
 namespace ZO.Util.Extensions {
     public static class ZOROSConversionExtensions {
@@ -36,6 +37,29 @@ public static string ToXMLString(this Vector3 v) {
             return $"{v.x} {v.y} {v.z}";
         }
 
+        /// <summary>
+        /// Converts URDF Xml string vector "1 2 3" to Vector3
+        /// </summary>
+        /// <param name="s">URDF XML string in format "1.0 2.0 3.0" 3 numbers separated by spaces</param>
+        /// <returns></returns>
+        public static Vector3 FromURDFStringToVector3(this string s) {
+            string[] splits = s.Split(' ');
+            List<float> numbers = new List<float>();
+            foreach (string snum in splits) {
+                if (float.TryParse(snum, out float v)) {
+                    numbers.Add(v);
+                }
+            }
+
+            if (numbers.Count == 3) {
+                return new Vector3(numbers[0], numbers[1], numbers[2]);
+            }
+
+            Debug.LogWarning($"Could not parse string: {s}");
+
+            return Vector3.zero;
+        } 
+
 
     }
 

Runtime/Scripts/Util/ImportExport/ZOImportURDF.cs

@@ -61,28 +61,47 @@ public static ZOSimDocumentRoot Import(XmlDocument xmlDocument) {
                 XmlNode[] xmlVisuals = xmlLink.GetChildrenByName("visual");
 
                 foreach (XmlNode xmlVisual in xmlVisuals) {
+                    
+                    
                     XmlNode[] xmlGeometries = xmlVisual.GetChildrenByName("geometry");
 
                     foreach (XmlNode xmlGeom in xmlGeometries) {
-                        XmlNode xmlBox = xmlGeom.GetChildByName("box");
+                        
                         GameObject visualGeo = null;
+
+                        XmlNode xmlBox = xmlGeom.GetChildByName("box");
                         if (xmlBox != null) {
                             visualGeo = GameObject.CreatePrimitive(PrimitiveType.Cube);
+                            Vector3 size = xmlBox.Attributes["size"].Value.FromURDFStringToVector3();
+                            
+                            visualGeo.transform.localScale = size.Ros2UnityScale();
                         }
+                        
                         XmlNode xmlCylinder = xmlGeom.GetChildByName("cylinder");
                         if (xmlCylinder != null) {
                             visualGeo = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
+                            float radius = float.Parse(xmlCylinder.Attributes["radius"].Value);
+                            float length = float.Parse(xmlCylinder.Attributes["length"].Value);
+                            visualGeo.transform.localScale = new Vector3(radius * 2.0f, length * 0.5f, radius * 2.0f);
                         }
+                        
                         XmlNode xmlSphere = xmlGeom.GetChildByName("sphere");
                         if (xmlSphere != null) {
                             visualGeo = GameObject.CreatePrimitive(PrimitiveType.Sphere);
+                            float radius = float.Parse(xmlSphere.Attributes["radius"].Value);
+                            visualGeo.transform.localScale = new Vector3(radius * 2.0f, radius * 2.0f, radius * 2.0f);
                         }
+
                         XmlNode xmlMesh = xmlGeom.GetChildByName("mesh");
                         if (xmlMesh != null) {
                             // TODO
                         }
                         if (visualGeo != null) {
                             visualGeo.transform.SetParent(goVisualsEmpty.transform);
+                            string visualName = xmlVisual.Attributes["name"].Value;
+                            if (visualName != null) {
+                                visualGeo.name = visualName;
+                            }
                         }
 
                     }