Configurable north direction for NED and ENU conversions

Co-authored-by: LaurieCheers <[email protected]>

ROSGeometry.md

@@ -98,3 +98,13 @@ Or, if you need to work with it in the FLU coordinate space for now, you can wri
 (Note, the As function does NOT do any coordinate conversion. It simply assumes the point is in the FLU coordinate frame already, and transfers it into an appropriate container.)
 
 And again, the same goes for converting a Quaternion message into a Unity Quaternion or `Quaternion<C>`.
+
+# Geographical Coordinates
+
+The geographical coordinate systems NED and ENU are more complicated than the rest: besides the obvious "north" direction, they also provide a convention for which direction is "forward", which is not even consistent between them - In NED, forward (i.e. yaw 0) is north, whereas in ENU forward is east. Because of this inconsistency, there's no universal convention that we could establish for which direction is north in Unity. Instead, we're forced to make a distinction between local and world rotations.
+
+To correctly convert a *world* position or rotation to or from NED or ENU coordinates, you should use the standard NED or ENU coordinate space - for example, `To<NED>()`. This conversion will respect what direction you have set as North. By default, the Unity Z axis points north, but this is a configuration option that you can change in the Robotics/ROS Settings menu.
+
+If you have a *local* rotation represented in NED or ENU coordinates, you should convert it using the coordinate space NEDLocal or ENULocal: for example, `To<NEDLocal>()`. These conversions are not appropriate for handling world rotations, because they don't respect the north direction, only what direction is forward (i.e. the Unity Z axis). In other words, with NEDLocal and ENULocal, an identity quaternion is still identity. This is not necessarily true for NED or ENU. NEDLocal is a synonym for the FRD coordinate space (north = forward, east = right, down = down) and ENULocal is a synonym for the FLU coordinate space (east = forward, north = left, up = up).
+
+If you only care about NED coordinates, you can avoid having to deal with this distinction by setting the Z axis direction to North (the default) - this will make the NED coordinate space equal to NEDLocal. Similarly, if you only care about ENU coordinates, setting the Z axis direction to East will make ENU equal to ENULocal.

com.unity.robotics.ros-tcp-connector/CHANGELOG.md

@@ -49,6 +49,8 @@ Upgrade the TestRosTcpConnector project to use Unity LTS version 2020.3.11f1
 
  - Unity service implementations can be async
 
+ - Added geographical world coordinate transformation by a Compass component
+
 ### Changed
 - Publishing a message to an unregistered topic will show an error.
 - Registering a service now requires both the request and response message type.

com.unity.robotics.ros-tcp-connector/Editor/ROSSettingsEditor.cs

@@ -3,6 +3,7 @@
 using System.IO;
 using System.Collections.Generic;
 using System.Linq;
+using Unity.Robotics.ROSTCPConnector.ROSGeometry;
 using UnityEditor.Callbacks;
 
 namespace Unity.Robotics.ROSTCPConnector.Editor
@@ -20,6 +21,7 @@ public static void OpenWindow()
 
  GameObject prefabObj;
  Unity.Robotics.ROSTCPConnector.ROSConnection prefab;
+ GeometryCompass geometryCompassPrefab;
 
  public enum RosProtocol
  {
@@ -138,6 +140,11 @@ protected virtual void OnGUI()
  {
  PrefabUtility.SavePrefabAsset(prefab.gameObject);
  }
+
+ // ROS Geometry Compass Settings
+ EditorGUILayout.Space();
+ EditorGUILayout.LabelField("Compass settings", EditorStyles.boldLabel);
+ GeometryCompass.UnityZAxisDirection = (CardinalDirection)EditorGUILayout.EnumPopup("Unity Z Axis Direction", GeometryCompass.UnityZAxisDirection);
  }
 
  void OnInspectorUpdate() { Repaint(); }

com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs

@@ -1,7 +1,5 @@
 using RosMessageTypes.Geometry;
 using System;
-using System.Collections;
-using System.Collections.Generic;
 using UnityEngine;
 
 namespace Unity.Robotics.ROSTCPConnector.ROSGeometry
@@ -37,28 +35,179 @@ public class FLU : ICoordinateSpace
  public Quaternion ConvertToRUF(Quaternion q) => new Quaternion(-q.y, q.z, q.x, -q.w);
  }
 
- public class NED : ICoordinateSpace
+ public class ENULocal : FLU { }
+
+ public class FRD : ICoordinateSpace
  {
  public Vector3 ConvertFromRUF(Vector3 v) => new Vector3(v.z, v.x, -v.y);
  public Vector3 ConvertToRUF(Vector3 v) => new Vector3(v.y, -v.z, v.x);
  public Quaternion ConvertFromRUF(Quaternion q) => new Quaternion(q.z, q.x, -q.y, -q.w);
  public Quaternion ConvertToRUF(Quaternion q) => new Quaternion(q.y, -q.z, q.x, -q.w);
  }
 
+ public class NEDLocal : FRD { }
+
+ public class NED : ICoordinateSpace
+ {
+ public Vector3 ConvertFromRUF(Vector3 v)
+ {
+ switch (GeometryCompass.UnityZAxisDirection)
+ {
+ case CardinalDirection.North:
+ return new Vector3(v.z, v.x, -v.y);
+ case CardinalDirection.East:
+ return new Vector3(-v.x, v.z, -v.y);
+ case CardinalDirection.South:
+ return new Vector3(-v.z, -v.x, -v.y);
+ case CardinalDirection.West:
+ return new Vector3(v.x, -v.z, -v.y);
+ default:
+ throw new NotSupportedException();
+ }
+ }
+
+ public Vector3 ConvertToRUF(Vector3 v)
+ {
+ switch (GeometryCompass.UnityZAxisDirection)
+ {
+ case CardinalDirection.North:
+ return new Vector3(v.y, -v.z, v.x);
+ case CardinalDirection.East:
+ return new Vector3(-v.x, -v.z, v.y);
+ case CardinalDirection.South:
+ return new Vector3(-v.y, -v.z, -v.x);
+ case CardinalDirection.West:
+ return new Vector3(v.x, -v.z, -v.y);
+ default:
+ throw new NotSupportedException();
+ }
+ }
+
+ public Quaternion ConvertFromRUF(Quaternion q)
+ {
+ switch (GeometryCompass.UnityZAxisDirection)
+ {
+ case CardinalDirection.North:
+ break;
+ case CardinalDirection.East:
+ q = GeometryCompass.k_NinetyYaw * q;
+ break;
+ case CardinalDirection.South:
+ q = GeometryCompass.k_OneEightyYaw * q;
+ break;
+ case CardinalDirection.West:
+ q = GeometryCompass.k_NegativeNinetyYaw * q;
+ break;
+ default:
+ throw new NotSupportedException();
+ }
+ return new Quaternion(q.z, q.x, -q.y, -q.w);
+ }
+
+ public Quaternion ConvertToRUF(Quaternion q)
+ {
+ var r = new Quaternion(q.y, -q.z, q.x, -q.w);
+ switch (GeometryCompass.UnityZAxisDirection)
+ {
+ case CardinalDirection.North:
+ return r;
+ case CardinalDirection.East:
+ return GeometryCompass.k_NegativeNinetyYaw * r;
+ case CardinalDirection.South:
+ return GeometryCompass.k_OneEightyYaw * r;
+ case CardinalDirection.West:
+ return GeometryCompass.k_NinetyYaw * r;
+ default:
+ throw new NotSupportedException();
+ }
+ }
+ }
+
  public class ENU : ICoordinateSpace
  {
- public Vector3 ConvertFromRUF(Vector3 v) => new Vector3(v.x, v.z, v.y);
- public Vector3 ConvertToRUF(Vector3 v) => new Vector3(v.x, v.z, v.y);
- public Quaternion ConvertFromRUF(Quaternion q) => new Quaternion(q.x, q.z, q.y, -q.w);
- public Quaternion ConvertToRUF(Quaternion q) => new Quaternion(q.x, q.z, q.y, -q.w);
+ public Vector3 ConvertFromRUF(Vector3 v)
+ {
+ switch (GeometryCompass.UnityZAxisDirection)
+ {
+ case CardinalDirection.North:
+ return new Vector3(v.x, v.z, v.y);
+ case CardinalDirection.East:
+ return new Vector3(v.z, -v.x, v.y);
+ case CardinalDirection.South:
+ return new Vector3(-v.x, -v.z, v.y);
+ case CardinalDirection.West:
+ return new Vector3(-v.z, v.x, v.y);
+ default:
+ throw new NotSupportedException();
+ }
+ }
+ public Vector3 ConvertToRUF(Vector3 v)
+ {
+ switch (GeometryCompass.UnityZAxisDirection)
+ {
+ case CardinalDirection.North:
+ return new Vector3(v.x, v.z, v.y);
+ case CardinalDirection.East:
+ return new Vector3(-v.y, v.z, v.x);
+ case CardinalDirection.South:
+ return new Vector3(-v.x, v.z, -v.y);
+ case CardinalDirection.West:
+ return new Vector3(v.y, v.z, -v.x);
+ default:
+ throw new NotSupportedException();
+ }
+ }
+
+ public Quaternion ConvertFromRUF(Quaternion q)
+ {
+ switch (GeometryCompass.UnityZAxisDirection)
+ {
+ case CardinalDirection.North:
+ q = GeometryCompass.k_NegativeNinetyYaw * q;
+ break;
+ case CardinalDirection.East:
+ break;
+ case CardinalDirection.South:
+ q = GeometryCompass.k_NinetyYaw * q;
+ break;
+ case CardinalDirection.West:
+ q = GeometryCompass.k_OneEightyYaw * q;
+ break;
+ default:
+ throw new NotSupportedException();
+ }
+
+ return new Quaternion(q.z, -q.x, q.y, -q.w);
+ }
+
+ public Quaternion ConvertToRUF(Quaternion q)
+ {
+ q = new Quaternion(-q.y, q.z, q.x, -q.w);
+ switch (GeometryCompass.UnityZAxisDirection)
+ {
+ case CardinalDirection.North:
+ return GeometryCompass.k_NinetyYaw * q;
+ case CardinalDirection.East:
+ return q;
+ case CardinalDirection.South:
+ return GeometryCompass.k_NegativeNinetyYaw * q;
+ case CardinalDirection.West:
+ return GeometryCompass.k_OneEightyYaw * q;
+ default:
+ throw new NotSupportedException();
+ }
+ }
  }
 
  public enum CoordinateSpaceSelection
  {
  RUF,
  FLU,
+ FRD,
  NED,
- ENU
+ ENU,
+ NEDLocal,
+ ENULocal,
  }
 
  public static class CoordinateSpaceExtensions
@@ -133,10 +282,16 @@ public static Vector3 From(this PointMsg self, CoordinateSpaceSelection selectio
  return self.From<RUF>();
  case CoordinateSpaceSelection.FLU:
  return self.From<FLU>();
+ case CoordinateSpaceSelection.FRD:
+ return self.From<FRD>();
  case CoordinateSpaceSelection.ENU:
  return self.From<ENU>();
  case CoordinateSpaceSelection.NED:
  return self.From<NED>();
+ case CoordinateSpaceSelection.ENULocal:
+ return self.From<ENULocal>();
+ case CoordinateSpaceSelection.NEDLocal:
+ return self.From<NEDLocal>();
  default:
  Debug.LogError("Invalid coordinate space " + selection);
  return self.From<RUF>();
@@ -151,10 +306,16 @@ public static Vector3 From(this Point32Msg self, CoordinateSpaceSelection select
  return self.From<RUF>();
  case CoordinateSpaceSelection.FLU:
  return self.From<FLU>();
+ case CoordinateSpaceSelection.FRD:
+ return self.From<FRD>();
  case CoordinateSpaceSelection.ENU:
  return self.From<ENU>();
  case CoordinateSpaceSelection.NED:
  return self.From<NED>();
+ case CoordinateSpaceSelection.ENULocal:
+ return self.From<ENULocal>();
+ case CoordinateSpaceSelection.NEDLocal:
+ return self.From<NEDLocal>();
  default:
  Debug.LogError("Invalid coordinate space " + selection);
  return self.From<RUF>();
@@ -169,10 +330,16 @@ public static Vector3 From(this Vector3Msg self, CoordinateSpaceSelection select
  return self.From<RUF>();
  case CoordinateSpaceSelection.FLU:
  return self.From<FLU>();
+ case CoordinateSpaceSelection.FRD:
+ return self.From<FRD>();
  case CoordinateSpaceSelection.ENU:
  return self.From<ENU>();
  case CoordinateSpaceSelection.NED:
  return self.From<NED>();
+ case CoordinateSpaceSelection.ENULocal:
+ return self.From<ENULocal>();
+ case CoordinateSpaceSelection.NEDLocal:
+ return self.From<NEDLocal>();
  default:
  Debug.LogError("Invalid coordinate space " + selection);
  return self.From<RUF>();
@@ -187,10 +354,16 @@ public static Quaternion From(this QuaternionMsg self, CoordinateSpaceSelection
  return self.From<RUF>();
  case CoordinateSpaceSelection.FLU:
  return self.From<FLU>();
+ case CoordinateSpaceSelection.FRD:
+ return self.From<FRD>();
  case CoordinateSpaceSelection.ENU:
  return self.From<ENU>();
  case CoordinateSpaceSelection.NED:
  return self.From<NED>();
+ case CoordinateSpaceSelection.ENULocal:
+ return self.From<ENULocal>();
+ case CoordinateSpaceSelection.NEDLocal:
+ return self.From<NEDLocal>();
  default:
  Debug.LogError("Invalid coordinate space " + selection);
  return self.From<RUF>();

com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/GeometryCompass.cs

@@ -0,0 +1,71 @@
+using System;
+using UnityEngine;
+
+namespace Unity.Robotics.ROSTCPConnector.ROSGeometry
+{
+ public enum CardinalDirection
+ {
+ North = 0,
+ East = 1,
+ South = 2,
+ West = 3,
+ }
+
+ public class GeometryCompass : ScriptableObject
+ {
+ public const string k_CompassSettingsAsset = "GeometryCompassSettings.asset";
+
+ [SerializeField]
+ CardinalDirection m_ZAxisDirection;
+
+ static GeometryCompass s_Instance;
+
+ static GeometryCompass GetOrCreateSettings()
+ {
+ if (s_Instance != null)
+ return s_Instance;
+#if UNITY_EDITOR
+ string assetPath = System.IO.Path.Combine("Assets/Resources", k_CompassSettingsAsset);
+ s_Instance = UnityEditor.AssetDatabase.LoadAssetAtPath<GeometryCompass>(assetPath);
+ if (s_Instance == null)
+ {
+ s_Instance = ScriptableObject.CreateInstance<GeometryCompass>();
+ s_Instance.m_ZAxisDirection = CardinalDirection.North;
+ if(!UnityEditor.AssetDatabase.IsValidFolder("Assets/Resources"))
+ UnityEditor.AssetDatabase.CreateFolder("Assets", "Resources");
+ UnityEditor.AssetDatabase.CreateAsset(s_Instance, assetPath);
+ UnityEditor.AssetDatabase.SaveAssets();
+ }
+#else
+ s_Instance = Resources.Load<GeometryCompass>(k_CompassSettingsAsset);
+#endif
+ return s_Instance;
+ }
+
+ public static CardinalDirection UnityZAxisDirection
+ {
+ get
+ {
+ GeometryCompass compass = GetOrCreateSettings();
+ return (compass == null) ? CardinalDirection.North : compass.m_ZAxisDirection;
+ }
+
+#if UNITY_EDITOR
+ set
+ {
+ GeometryCompass compass = GetOrCreateSettings();
+ if (compass.m_ZAxisDirection != value)
+ {
+ compass.m_ZAxisDirection = value;
+ UnityEditor.EditorUtility.SetDirty(compass);
+ UnityEditor.AssetDatabase.SaveAssets();
+ }
+ }
+#endif
+ }
+
+ public static readonly Quaternion k_NinetyYaw = Quaternion.Euler(0, 90, 0);
+ public static readonly Quaternion k_OneEightyYaw = Quaternion.Euler(0, 180, 0);
+ public static readonly Quaternion k_NegativeNinetyYaw = Quaternion.Euler(0, -90, 0);
+ }
+}