fixed odometry pose to match TF

Author: micahpearlman

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

@@ -424,29 +424,29 @@ public PoseMessage(PointMessage position, QuaternionMessage orientation) {
         /// Does Unity to ROS coordinate system transform.
         /// </summary>
         /// <param name="transform"></param>
-        public void FromLocalUnityTransform(UnityEngine.Transform transform) {
+        public void FromLocalUnityTransformToROS(UnityEngine.Transform transform) {
             this.position.UnityVector3 = transform.localPosition;
             this.orientation.UnityQuaternion = transform.localRotation;
         }
 
         [Newtonsoft.Json.JsonIgnore]
         public UnityEngine.Transform LocalUnityTransform {
-            set { FromLocalUnityTransform(value); }
+            set { FromLocalUnityTransformToROS(value); }
         }
 
         /// <summary>
         /// Convert global Unity Transform to ROS Pose. 
         /// Does Unity to ROS coordinate system transform.
         /// </summary>
         /// <param name="transform"></param>
-        public void FromGlobalUnityTransform(UnityEngine.Transform transform) {
+        public void FromGlobalUnityTransformToROS(UnityEngine.Transform transform) {
             this.position.UnityVector3 = transform.position;
             this.orientation.UnityQuaternion = transform.rotation;
         }
 
         [Newtonsoft.Json.JsonIgnore]
         public UnityEngine.Transform GlobalUnityTransform {
-            set { FromGlobalUnityTransform(value); }
+            set { FromGlobalUnityTransformToROS(value); }
         }
 
     }

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

@@ -69,14 +69,15 @@ protected override void ZOFixedUpdateHzSynchronized() {
                 _transformStampedMessage.header.Update();
                 _transformStampedMessage.header.frame_id = ZOROSUnityManager.Instance.WorldFrameId; // connect to the world
                 _transformStampedMessage.child_frame_id = "odom";
-                _transformStampedMessage.UnityLocalTransform = Rigidbody.transform;
+                _transformStampedMessage.FromLocalUnityTransformToROS(Rigidbody.transform);
                 ZOROSUnityManager.Instance.BroadcastTransform(_transformStampedMessage);
 
 
                 _currentOdometryMessage.Update(); // update times stamps
 
                 // BUGBUG: not super clear on where the pose should be?
-                _currentOdometryMessage.pose.pose.GlobalUnityTransform = Rigidbody.transform;
+                // _currentOdometryMessage.pose.pose.GlobalUnityTransform = Rigidbody.transform;
+                _currentOdometryMessage.pose.pose.FromLocalUnityTransformToROS(Rigidbody.transform);
 
                 // get velocity in /odom frame
                 Vector3 linear = Rigidbody.velocity;