diff --git a/ROSGeometry.md b/ROSGeometry.md index 768667b..cfbfd77 100644 --- a/ROSGeometry.md +++ b/ROSGeometry.md @@ -43,7 +43,7 @@ Unity's standard Transform class also has a `To()` extension method that retu # Internal details -Some more detail about what's going on here: The core of the ROSGeometry package is the two generic structs, `Vector3` and `Quaternion`. The type parameter C here indicates the coordinate frame you're working in - either FLU, or RUF, or perhaps one of the more exotic frames such as NED (north, east, down) or ENU (east, north, up), used in aviation. +Some more detail about what's going on here: The core of the ROSGeometry package is the two generic structs, `Vector3` and `Quaternion`. The type parameter C here indicates the coordinate frame you're working in - either FLU, or RUF, or perhaps one of the more exotic frames such as NED (north, east, down) or ENU (east, north, up), used in aviation. In conversions between RUF and geographical coordinate systems, such as NED and ENU, the east direction is equivalent to the z-axis (forward) in RUF. These are fully-fledged Vector3 and Quaternion classes, so if you want, you can work with them directly to perform geometric operations in an arbitrary coordinate space. (Note, it's a compile time error to add a Vector3 to a Vector3.) diff --git a/com.unity.robotics.ros-tcp-connector/CHANGELOG.md b/com.unity.robotics.ros-tcp-connector/CHANGELOG.md index fe61de8..3e75c87 100644 --- a/com.unity.robotics.ros-tcp-connector/CHANGELOG.md +++ b/com.unity.robotics.ros-tcp-connector/CHANGELOG.md @@ -58,6 +58,8 @@ Add badges to main README ### Changed +Update the transformation of coordinate spaces using Unity's coordinate as right, up, forward (RUF) and south, up, east (SUE). + ### Deprecated ### Removed 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 57e53c8..f873c9b 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs @@ -20,7 +20,14 @@ namespace Unity.Robotics.ROSTCPConnector.ROSGeometry { } - //RUF is the Unity coordinate space, so no conversion needed + /// + /// RUF is the Unity coordinate space, so no conversion needed + /// + /// X axis: Right and South + /// Y axis: Up + /// Z axis: Forward and East + /// + /// public class RUF : ICoordinateSpace { public Vector3 ConvertFromRUF(Vector3 v) => v; @@ -29,6 +36,14 @@ namespace Unity.Robotics.ROSTCPConnector.ROSGeometry public Quaternion ConvertToRUF(Quaternion q) => q; } + /// + /// ROS standard forward, left, up (FLU) coordinate (REP-103) + /// + /// X axis: Forward and East + /// Y axis: Left and North + /// Z axis: Up + /// + /// public class FLU : ICoordinateSpace { public Vector3 ConvertFromRUF(Vector3 v) => new Vector3(v.z, -v.x, v.y); @@ -37,21 +52,31 @@ namespace Unity.Robotics.ROSTCPConnector.ROSGeometry public Quaternion ConvertToRUF(Quaternion q) => new Quaternion(-q.y, q.z, q.x, -q.w); } + /// + /// Local north, east, down (NED) coordinates for outdoor systems, such as airplane and submarine (REP-103) + /// + /// X axis: North + /// Y axis: East + /// Z axis: Down + /// + /// public class NED : 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 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 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); - } + /// + /// Local east, north, up (ENU) coordinates for short-range Cartesian representations of geographic locations (REP-103) + /// + /// X axis: East + /// Y axis: North + /// Z axis: Up + /// + /// + public class ENU : FLU { } public enum CoordinateSpaceSelection {