fixed Unity to ROS Quaternion!

Author: micahpearlman

Runtime/Scripts/ROS/MessageTypes/Geometry/ZOROSGeometryMessages.cs

@@ -123,10 +123,11 @@ public UnityEngine.Quaternion ToUnityQuaternion() {
         /// </summary>
         /// <param name="q"></param>
         public void FromUnityQuaternionToROS(UnityEngine.Quaternion q) {
-            this.x = (double)-q.x;
-            this.y = (double)-q.z;
+            this.x = (double)-q.z;
+            this.y = (double)q.x;
             this.z = (double)-q.y;
             this.w = (double)q.w;
+
         }
 
         [Newtonsoft.Json.JsonIgnore]
@@ -245,8 +246,8 @@ public void FromLocalUnityTransform(UnityEngine.Transform transform) {
         /// </summary>
         /// <param name="transform"></param>
         public void FromGlobalUnityTransformToROS(UnityEngine.Transform transform) {
-            translation.UnityVector3 = transform.position;
-            rotation.UnityQuaternion = transform.rotation;
+            translation.FromUnityVector3ToROS(transform.position);
+            rotation.FromUnityQuaternionToROS(transform.rotation);
         }
 
         [Newtonsoft.Json.JsonIgnore]

Runtime/Scripts/ROS/Unity/Publishers/ZOROSTransformPublisher.cs

@@ -34,12 +34,10 @@ public string FrameID {
         protected override void ZOStart() {
             base.ZOStart();
             // if the child frame id is not set then set it to be the name of this game object.
-            if (ChildFrameID.Length == 0) {
+            if (string.IsNullOrEmpty(ChildFrameID) == true) {
                 ChildFrameID = this.gameObject.name;
             }
 
-            // TODO: autoset frameId by looking up the hierarch for a parent ZOROSTransformPublisher
-            // if no parent found then set to "map"
         }
         protected override void ZOUpdateHzSynchronized() {
             _transformMessage.header.Update();
@@ -53,12 +51,14 @@ protected override void ZOUpdateHzSynchronized() {
 
         protected override void ZOOnValidate() {
             base.ZOOnValidate();
-            if (ChildFrameID == "") {
+            if (string.IsNullOrEmpty(ChildFrameID) == true) {
                 ChildFrameID = Name;
             }
-            if (FrameID == "") {
+            if (string.IsNullOrEmpty(FrameID) == true) {
                 if (transform.parent) {
                     FrameID = transform.parent.name;
+                } else {
+                    FrameID = ZOROSUnityManager.Instance.WorldFrameId;
                 }
             }
             if (UpdateRateHz == 0) {

Samples~/ZeroSimSamples/Robots/Spot/spot_rigged.prefab

@@ -196,40 +196,6 @@ HingeJoint:
   m_EnablePreprocessing: 1
   m_MassScale: 1
   m_ConnectedMassScale: 1
---- !u!138 &662680477
-FixedJoint:
-  m_ObjectHideFlags: 0
-  m_CorrespondingSourceObject: {fileID: 0}
-  m_PrefabInstance: {fileID: 0}
-  m_PrefabAsset: {fileID: 0}
-  m_GameObject: {fileID: 705516937584561444}
-  m_ConnectedBody: {fileID: 1690162257}
-  m_ConnectedArticulationBody: {fileID: 0}
-  m_BreakForce: Infinity
-  m_BreakTorque: Infinity
-  m_EnableCollision: 0
-  m_EnablePreprocessing: 1
-  m_MassScale: 1
-  m_ConnectedMassScale: 1
---- !u!114 &662680478
-MonoBehaviour:
-  m_ObjectHideFlags: 0
-  m_CorrespondingSourceObject: {fileID: 0}
-  m_PrefabInstance: {fileID: 0}
-  m_PrefabAsset: {fileID: 0}
-  m_GameObject: {fileID: 705516937584561444}
-  m_Enabled: 1
-  m_EditorHideFlags: 0
-  m_Script: {fileID: 11500000, guid: d54edc081e20dfbb3b27d3f855559c74, type: 3}
-  m_Name: 
-  m_EditorClassIdentifier: 
-  _updateRateHz: 0
-  _debug: 0
-  _currentUpdateHz: 0
-  _currentFixedUpdateHz: 0
-  _fixedJoint: {fileID: 662680477}
-  _connectedBody: {fileID: 1690162257}
-  _name: joint.fixed_from_body_to_base_llnk
 --- !u!114 &662680489
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -250,29 +216,13 @@ MonoBehaviour:
   _name: ros.publisher.transform_body
   _frameId: map
   _childFrameId: body
---- !u!54 &1690162257
-Rigidbody:
-  m_ObjectHideFlags: 0
-  m_CorrespondingSourceObject: {fileID: 0}
-  m_PrefabInstance: {fileID: 0}
-  m_PrefabAsset: {fileID: 0}
-  m_GameObject: {fileID: 1730632979493296597}
-  serializedVersion: 2
-  m_Mass: 1
-  m_Drag: 0
-  m_AngularDrag: 0.05
-  m_UseGravity: 1
-  m_IsKinematic: 0
-  m_Interpolate: 0
-  m_Constraints: 0
-  m_CollisionDetection: 0
---- !u!114 &1690162256
+--- !u!114 &662680477
 MonoBehaviour:
   m_ObjectHideFlags: 0
   m_CorrespondingSourceObject: {fileID: 0}
   m_PrefabInstance: {fileID: 0}
   m_PrefabAsset: {fileID: 0}
-  m_GameObject: {fileID: 1730632979493296597}
+  m_GameObject: {fileID: 705516937584561444}
   m_Enabled: 1
   m_EditorHideFlags: 0
   m_Script: {fileID: 11500000, guid: b28630d63f8f5fb68a139c2eebaae78f, type: 3}
@@ -283,7 +233,7 @@ MonoBehaviour:
   _currentUpdateHz: 0
   _currentFixedUpdateHz: 0
   _ROSTopic: /odometry
-  _name: publisher.odometry_base_link
+  _name: publisher.odometry_body
   _applyNoise: 0
   _a1: 0.05
   _a2: 15