From d352016c2b68724ff26d433ab42e5990ac92f987 Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Thu, 30 Sep 2021 09:59:10 -0700 Subject: [PATCH] Revert "Update unity-ros coordinate transformation (#174)" (#195) This reverts commit c26c4cf15c05bd051d4955c3bc5e49f8633336f8. --- ROSGeometry.md | 2 +- .../CHANGELOG.md | 2 - .../Runtime/ROSGeometry/CoordinateSpaces.cs | 49 +++++-------------- 3 files changed, 13 insertions(+), 40 deletions(-) diff --git a/ROSGeometry.md b/ROSGeometry.md index cfbfd77..768667b 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. 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. +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. 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 3e75c87..fe61de8 100644 --- a/com.unity.robotics.ros-tcp-connector/CHANGELOG.md +++ b/com.unity.robotics.ros-tcp-connector/CHANGELOG.md @@ -58,8 +58,6 @@ 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 f873c9b..57e53c8 100644 --- a/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs +++ b/com.unity.robotics.ros-tcp-connector/Runtime/ROSGeometry/CoordinateSpaces.cs @@ -20,14 +20,7 @@ namespace Unity.Robotics.ROSTCPConnector.ROSGeometry { } - /// - /// RUF is the Unity coordinate space, so no conversion needed - /// - /// X axis: Right and South - /// Y axis: Up - /// Z axis: Forward and East - /// - /// + //RUF is the Unity coordinate space, so no conversion needed public class RUF : ICoordinateSpace { public Vector3 ConvertFromRUF(Vector3 v) => v; @@ -36,14 +29,6 @@ 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); @@ -52,31 +37,21 @@ 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.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) => 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); } - /// - /// 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 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 enum CoordinateSpaceSelection {