Skip to content

Update unity-ros coordinate transformation #174

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 3 commits into from
Sep 8, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion ROSGeometry.md
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ Unity's standard Transform class also has a `To<C>()` 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<C>` and `Quaternion<C>`. 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<C>` and `Quaternion<C>`. 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<FLU> to a Vector3<RUF>.)

Expand Down
2 changes: 2 additions & 0 deletions com.unity.robotics.ros-tcp-connector/CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,14 @@ public interface CoordinateSpace : ICoordinateSpace
{
}

//RUF is the Unity coordinate space, so no conversion needed
/// <summary>
/// RUF is the Unity coordinate space, so no conversion needed
/// <list type="bullet">
/// <item><term>X axis: </term><description>Right and South</description></item>
/// <item><term>Y axis: </term><description>Up</description></item>
/// <item><term>Z axis: </term><description>Forward and East</description></item>
/// </list>
/// </summary>
public class RUF : ICoordinateSpace
{
public Vector3 ConvertFromRUF(Vector3 v) => v;
Expand All @@ -29,6 +36,14 @@ public class RUF : ICoordinateSpace
public Quaternion ConvertToRUF(Quaternion q) => q;
}

/// <summary>
/// ROS standard forward, left, up (FLU) coordinate (REP-103)
/// <list type="bullet">
/// <item><term>X axis: </term><description>Forward and East</description></item>
/// <item><term>Y axis: </term><description>Left and North</description></item>
/// <item><term>Z axis: </term><description>Up</description></item>
/// </list>
/// </summary>
public class FLU : ICoordinateSpace
{
public Vector3 ConvertFromRUF(Vector3 v) => new Vector3(v.z, -v.x, v.y);
Expand All @@ -37,21 +52,31 @@ public class FLU : ICoordinateSpace
public Quaternion ConvertToRUF(Quaternion q) => new Quaternion(-q.y, q.z, q.x, -q.w);
}

/// <summary>
/// Local north, east, down (NED) coordinates for outdoor systems, such as airplane and submarine (REP-103)
/// <list type="bullet">
/// <item><term>X axis: </term><description>North</description></item>
/// <item><term>Y axis: </term><description>East</description></item>
/// <item><term>Z axis: </term><description>Down</description></item>
/// </list>
/// </summary>
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);
}
/// <summary>
/// Local east, north, up (ENU) coordinates for short-range Cartesian representations of geographic locations (REP-103)
/// <list type="bullet">
/// <item><term>X axis: </term><description>East</description></item>
/// <item><term>Y axis: </term><description>North</description></item>
/// <item><term>Z axis: </term><description>Up</description></item>
/// </list>
/// </summary>
public class ENU : FLU { }

public enum CoordinateSpaceSelection
{
Expand Down