From 6cb88311293c2286789d25c42b385ca77de64caa Mon Sep 17 00:00:00 2001
From: LaurieCheers <73140792+LaurieCheers-unity@users.noreply.github.com>
Date: Fri, 27 May 2022 12:00:25 -0700
Subject: [PATCH 1/2] AngularVelocityFrom and AngularVelocityTo accessors

---
 .../Runtime/ROSGeometry/CoordinateSpaces.cs   | 28 ++++++++++++++++---
 .../Runtime/ROSGeometry/ROSVector3.cs         |  2 +-
 2 files changed, 25 insertions(+), 5 deletions(-)

diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs
index 6f0a27be..706da403 100644
--- a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs
+++ b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs
@@ -256,6 +256,12 @@ public static Vector3<C> To<C>(this Vector3 self)
             return new Vector3<C>(self);
         }
 
+        public static Vector3<C> AngularVelocityTo<C>(this Vector3 self)
+            where C : ICoordinateSpace, new()
+        {
+            return Vector3<C>.FromUnityAngularVelocity(self);
+        }
+
         public static Quaternion<C> To<C>(this Quaternion self)
             where C : ICoordinateSpace, new()
         {
@@ -269,7 +275,12 @@ public static Quaternion<C> To<C>(this Quaternion self)
 
         public static Vector3 From<C>(this PointMsg self) where C : ICoordinateSpace, new()
         {
-            return new Vector3<C>((float)self.x, (float)self.y, (float)self.z).toUnity;
+            return self.As<C>().toUnity;
+        }
+
+        public static Vector3 AngularVelocityFrom<C>(this PointMsg self) where C : ICoordinateSpace, new()
+        {
+            return self.As<C>().toUnityAngularVelocity;
         }
 
         public static Vector3<C> As<C>(this Point32Msg self) where C : ICoordinateSpace, new()
@@ -279,7 +290,12 @@ public static Quaternion<C> To<C>(this Quaternion self)
 
         public static Vector3 From<C>(this Point32Msg self) where C : ICoordinateSpace, new()
         {
-            return new Vector3<C>(self.x, self.y, self.z).toUnity;
+            return self.As<C>().toUnity;
+        }
+
+        public static Vector3 AngularVelocityFrom<C>(this Point32Msg self) where C : ICoordinateSpace, new()
+        {
+            return self.As<C>().toUnityAngularVelocity;
         }
 
         public static Vector3<C> As<C>(this Vector3Msg self) where C : ICoordinateSpace, new()
@@ -289,7 +305,11 @@ public static Quaternion<C> To<C>(this Quaternion self)
 
         public static Vector3 From<C>(this Vector3Msg self) where C : ICoordinateSpace, new()
         {
-            return new Vector3<C>((float)self.x, (float)self.y, (float)self.z).toUnity;
+            return self.As<C>().toUnity;
+        }
+        public static Vector3 AngularVelocityFrom<C>(this Vector3Msg self) where C : ICoordinateSpace, new()
+        {
+            return self.As<C>().toUnityAngularVelocity;
         }
 
         public static Quaternion<C> As<C>(this QuaternionMsg self) where C : ICoordinateSpace, new()
@@ -299,7 +319,7 @@ public static Quaternion<C> To<C>(this Quaternion self)
 
         public static Quaternion From<C>(this QuaternionMsg self) where C : ICoordinateSpace, new()
         {
-            return new Quaternion<C>((float)self.x, (float)self.y, (float)self.z, (float)self.w).toUnity;
+            return self.As<C>().toUnity;
         }
 
         public static TransformMsg To<C>(this Transform transform) where C : ICoordinateSpace, new()
diff --git a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/ROSVector3.cs b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/ROSVector3.cs
index de4cb0f2..a9f69d46 100644
--- a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/ROSVector3.cs
+++ b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/ROSVector3.cs
@@ -49,7 +49,7 @@ public static explicit operator Vector3(Vector3<C> vec)
 
         public static Vector3<C> FromUnityAngularVelocity(Vector3 angularVelocityRUF)
         {
-            return new Vector3<C> { internalVector = s_CoordinateSpace.ConvertAngularVelocityFromRUF(angularVelocityRUF) };
+            return MakeInternal(s_CoordinateSpace.ConvertAngularVelocityFromRUF(angularVelocityRUF));
         }
 
         // for internal use only - this function does not convert coordinate spaces correctly

From d31784d9a5e5811b4aa2d64a15f2dc4b38bcb12c Mon Sep 17 00:00:00 2001
From: LaurieCheers <73140792+LaurieCheers-unity@users.noreply.github.com>
Date: Fri, 27 May 2022 12:17:01 -0700
Subject: [PATCH 2/2] Docs

---
 ROSGeometry.md | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/ROSGeometry.md b/ROSGeometry.md
index 52107884..322e798d 100644
--- a/ROSGeometry.md
+++ b/ROSGeometry.md
@@ -37,9 +37,11 @@ Hence, writing 3d data into a message can often be as simple as writing:
 	}
 	ros.Send("imu", msg);
 
-Unity's standard Transform class also has a `To<C>()` extension method that returns a ROS Transform message. So creating a geometry_msgs/Transform message typically looks like this:
+# Angular Velocity vectors
 
-    TransformMsg msg = myGameObject.transform.To<FLU>());
+Although angular velocity values are stored using the same Vector3 structure as other vectors, they can't be converted the same way: to correctly convert an angular velocity between a left-handed and right-handed coordinate system, the vector needs to be negated.
+
+To convert a vector that represents an angular velocity, instead of using To<FLU>() and From<FLU>(), you should use the AngularVelocityTo<FLU>() and AngularVelocityFrom<FLU>() methods.
 
 # Internal details