Publish URDF to robot_description.

Author: micahpearlman

Editor/CustomEditors/ZOHingeJointEditor.cs

@@ -13,17 +13,6 @@ public class ZOHingeJointEditor : UnityEditor.Editor {
         public override void OnInspectorGUI() {
             DrawPropertiesExcluding(serializedObject, _dontIncludeMe);
             serializedObject.ApplyModifiedProperties();
-
-            // DrawDefaultInspector();
-
-            // ZO.Physics.ZOHingeJoint hingeJoint = (ZO.Physics.ZOHingeJoint)target;
-
-            // UnityEngine.HingeJoint unityHinge = hingeJoint.UnityHinge;
-
-            // serializedObject.Update();
-            // SerializedProperty serializedProperty = serializedObject.FindProperty("_hinge");
-            // EditorGUILayout.PropertyField(serializedProperty);
-            // serializedObject.ApplyModifiedProperties();
         }
     }
 

Runtime/Scripts/Document/ZOSimDocumentRoot.cs

@@ -4,11 +4,8 @@
 using System.Collections.Generic;
 using UnityEngine;
 using Newtonsoft.Json.Linq;
-using Newtonsoft.Json;
-using ZO.ROS.Publisher;
-using ZO.ROS.Controllers;
-using ZO.ROS.Unity.Service;
-using ZO.Math;
+using ZO.ROS;
+using ZO.ROS.Unity;
 using ZO.ImportExport;
 namespace ZO.Document {
 
@@ -26,25 +23,21 @@ namespace ZO.Document {
     ///             ZOSimOccurrence:
     /// ```
     /// 
-    /// It is primarily responsible for serialization of the Unity structure to ZoSim JSON document and 
-    /// deserialization from ZoSim JSON document to a Unity structure.
     /// </summary>
     [ExecuteAlways]
-    public class ZOSimDocumentRoot : MonoBehaviour {
+    public class ZOSimDocumentRoot : ZOROSUnityGameObjectBase {
 
 
+        public bool _publishRobotDescription = false;
+
         /// <summary>
-        /// The ZOSim JSON document path.
+        /// Publish URDF robot description to ROS parameter server /robot_description
         /// </summary>
         /// <value></value>
-        private string _zoSimDocumentFilePath;
-        public string ZOSimDocumentFilePath {
-            get { return _zoSimDocumentFilePath; }
-            set { _zoSimDocumentFilePath = value; }
+        public bool PublishRobotDescription {
+            get => _publishRobotDescription;
+            set => _publishRobotDescription = value;
         }
-        
-
-
 
 
         /// <summary>
@@ -55,8 +48,16 @@ public string ZOSimDocumentFilePath {
         /// <value></value>
         public string Name {
 
-            get => gameObject.name;
-            set => gameObject.name = value;
+            get {
+                if (string.IsNullOrEmpty(_name)) {
+                    _name = gameObject.name;
+                }
+                return _name;
+            }
+            set {
+                gameObject.name = value;
+                _name = value;
+            }
         }
 
 
@@ -69,14 +70,34 @@ public string Name {
         /// <returns>ZOSimOccurrence</returns>
         public ZOSimOccurrence GetOccurrence(string occurrenceName) {
             ZOSimOccurrence[] children = this.transform.GetComponentsInChildren<ZOSimOccurrence>(true);
-            foreach(ZOSimOccurrence child in children) {
+            foreach (ZOSimOccurrence child in children) {
                 if (child.Name == occurrenceName) {
                     return child;
                 }
             }
             return null;
         }
 
+        public override void OnROSBridgeConnected(ZOROSUnityManager rosUnityManager) {
+            Debug.Log("INFO: ZOSimDocumentRoot::OnROSBridgeConnected");
+
+            if (PublishRobotDescription == true) {
+                ExecuteInUpdate = () => {
+                    Debug.Log("INFO: ZOSimDocumentRoot publishing robot description.");
+                    ZOExportURDF exportURDF = new ZOExportURDF();
+                    XDocument urdfXML = exportURDF.BuildURDF(this);
+                    string urdfString = "<?xml version='1.0' encoding='utf-8'?>\n" + urdfXML.ToString();
+                    ZOROSAPI.SetParam("robot_description", urdfString, Name);
+
+                };
+
+            }
+
+        }
+
+        public override void OnROSBridgeDisconnected(ZOROSUnityManager rosUnityManager) {
+            Debug.Log("INFO: ZOSimDocumentRoot::OnROSBridgeDisconnected");
+        }
 
 
 

Runtime/Scripts/ROS/Unity/ZOROSUnityGameObjectBase.cs

@@ -133,5 +133,6 @@ protected override void ZOOnDestroy() {
         #endregion // ZOROSUnityInterface
 
 
+
     }
 }

Runtime/Scripts/Util/GameObject/ZOGameObjectBase.cs

@@ -37,7 +37,7 @@ public float UpdateTimeSeconds {
                 if (UpdateRateHz == 0.0f) {
                     return 0.0f; // avoid divide by zero
                 }
-                
+
                 return 1.0f / UpdateRateHz;
             }
         }
@@ -67,6 +67,23 @@ public double NextFixedUpdateTime {
 
         private static float _nextUpdateTimeOffset = 0.0f;
 
+        #region Threading
+        // See: https://stackoverflow.com/questions/41330771/use-unity-api-from-another-thread-or-call-a-function-in-the-main-thread
+
+        private List<System.Action> _actionQueuesUpdateFunc = new List<System.Action>();
+        private List<System.Action> _actionCopiedQueueUpdateFunc = new List<System.Action>();
+
+
+        public System.Action ExecuteInUpdate {
+            set {
+                _actionQueuesUpdateFunc.Add(value);
+            }
+        }
+
+
+        #endregion // Threading
+
+
 #if UNITY_EDITOR
 
         private ZOFrequencyCounter _updateHzCounter = new ZOFrequencyCounter(ZOFrequencyCounter.TimeSourceType.RENDER_UPDATE);
@@ -105,7 +122,7 @@ protected virtual void ZOOnEnable() { }
         /// <summary>
         /// OnValidate function for Zero Sim Obects
         /// </summary>
-        protected virtual void ZOOnValidate() {}
+        protected virtual void ZOOnValidate() { }
 
 
         /// <summary>
@@ -195,6 +212,18 @@ private void OnGUI() {
         /// IMPORTANT: do not redefine this method in child classes, use ZOUpdate() instead.
         /// </summary>
         void Update() {
+
+            // run queued actions
+            _actionCopiedQueueUpdateFunc.Clear();
+            lock (_actionQueuesUpdateFunc) {
+                _actionCopiedQueueUpdateFunc.AddRange(_actionQueuesUpdateFunc);
+                _actionQueuesUpdateFunc.Clear();
+            }
+
+            foreach (System.Action action in _actionCopiedQueueUpdateFunc) {
+                action.Invoke();
+            }
+
             ZOUpdate();
 
             if (Time.time >= _nextUpdateTime) {

Samples~/ZeroSimSamples/Scenes/URDF_test.unity

@@ -3497,7 +3497,7 @@ Transform:
   m_GameObject: {fileID: 1357432953}
   m_LocalRotation: {x: -0, y: -0, z: -0, w: 1}
   m_LocalPosition: {x: 0, y: 0, z: 0}
-  m_LocalScale: {x: 0.01, y: 0.01, z: 0.01}
+  m_LocalScale: {x: 0.1, y: 0.1, z: 0.1}
   m_Children: []
   m_Father: {fileID: 2050636604}
   m_RootOrder: 0