Skip to content

Commit 3715e7c

Browse files
micahpearlmanmicahpearlman
committed
added different coordinate systems for IMU publisher.
1 parent b2898ec commit 3715e7c

File tree

3 files changed

+91
-14
lines changed

3 files changed

+91
-14
lines changed

‎Runtime/Scripts/ROS/Unity/Publishers/ZOROSIMUPublisher.cs‎

Lines changed: 71 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,27 @@ public ZOIMU IMUSensor {
2828
set => _imuSensor = value;
2929
}
3030

31+
public enum CoordinateSystemEnum {
32+
RightHanded_XBackward_YLeft_ZUp,
33+
RightHanded_XRight_YDown_ZForward, // RealSense D435i
34+
Unity_LeftHanded_XRight_YUp_ZForward, // Unity Standard
35+
ROS_RightHanded_XForward_YLeft_ZUp, // ROS Standard
36+
};
37+
38+
public CoordinateSystemEnum _coordinateSystem = CoordinateSystemEnum.ROS_RightHanded_XForward_YLeft_ZUp;
39+
/// <summary>
40+
/// The coordinate system for the published IMU data.
41+
/// RightHanded_XBackward_YLeft_ZUp: ? Some IMU's want this reference frame
42+
/// RightHanded_XRight_YDown_ZForward: RealSense D435i IMU prefers this reference frame.
43+
/// Unity_LeftHanded_XRight_YUp_ZForward: Unity reference frame
44+
/// ROS_RightHanded_XForward_YLeft_ZUp: ROS standard reference frame.
45+
/// </summary>
46+
/// <value></value>
47+
public CoordinateSystemEnum CoordinateSystem {
48+
get { return _coordinateSystem; }
49+
set { _coordinateSystem = value; }
50+
}
51+
3152

3253
private ZOROSTransformPublisher _transformPublisher = null;
3354
public ZOROSTransformPublisher TransformPublisher {
@@ -80,18 +101,56 @@ public override void OnROSBridgeDisconnected(ZOROSUnityManager rosUnityManager)
80101
private Task OnPublishImuDelegate(ZOIMU lidar, string name, Vector3 linearAccel, Vector3 angularVelocity, Quaternion orientation) {
81102
_imuMessage.header.Update();
82103
_imuMessage.header.frame_id = TransformPublisher.ChildFrameID;
83-
_imuMessage.linear_acceleration.UnityVector3 = linearAccel;
84-
_imuMessage.angular_velocity.UnityVector3 = angularVelocity;
85-
_imuMessage.orientation.UnityQuaternion = orientation;
86-
87-
// if (_referenceFrame == ReferenceFrame.RightHanded_XBackward_YLeft_ZUp) {
88-
// // (x, y, z, w) -> (-x, -z, y, -w).
89-
// publishedOrientation = new Quaternion(-transform.rotation.z, -transform.rotation.x, transform.rotation.y, -transform.rotation.w);
90-
// // if (m_zReverse)
91-
// // rot = new Quaternion (-rot.x, rot.y, -rot.z, rot.w);
92-
// publishedAcceleration = new Vector3(-_acceleration.z, -_acceleration.x, _acceleration.y);
93-
// publishedAngularVelocity = new Vector3(-_angularVelocity.z, -_angularVelocity.x, _angularVelocity.y);
94-
// }
104+
105+
if (_coordinateSystem == CoordinateSystemEnum.ROS_RightHanded_XForward_YLeft_ZUp) {
106+
_imuMessage.linear_acceleration.UnityVector3 = linearAccel;
107+
_imuMessage.angular_velocity.UnityVector3 = angularVelocity;
108+
_imuMessage.orientation.UnityQuaternion = orientation;
109+
110+
} else if (CoordinateSystem == CoordinateSystemEnum.RightHanded_XBackward_YLeft_ZUp) {
111+
// (x, y, z, w) -> (-x, -z, y, -w).
112+
_imuMessage.orientation.x = -orientation.z;
113+
_imuMessage.orientation.y = -orientation.x;
114+
_imuMessage.orientation.z = orientation.y;
115+
_imuMessage.orientation.w = -orientation.w;
116+
117+
_imuMessage.linear_acceleration.x = -linearAccel.z;
118+
_imuMessage.linear_acceleration.y = -linearAccel.x;
119+
_imuMessage.linear_acceleration.z = linearAccel.y;
120+
121+
_imuMessage.angular_velocity.x = -angularVelocity.z;
122+
_imuMessage.angular_velocity.y = -angularVelocity.x;
123+
_imuMessage.angular_velocity.z = angularVelocity.y;
124+
} else if (CoordinateSystem == CoordinateSystemEnum.RightHanded_XRight_YDown_ZForward) {
125+
Quaternion flippedYOrientation = Quaternion.Euler(Vector3.Scale(orientation.eulerAngles, Vector3.up * -1));
126+
_imuMessage.orientation.x = flippedYOrientation.x;
127+
_imuMessage.orientation.y = flippedYOrientation.y;
128+
_imuMessage.orientation.z = flippedYOrientation.z;
129+
_imuMessage.orientation.w = -flippedYOrientation.w; // negate left to right handed coordinate system
130+
131+
_imuMessage.linear_acceleration.x = linearAccel.x;
132+
_imuMessage.linear_acceleration.y = -linearAccel.y;
133+
_imuMessage.linear_acceleration.z = linearAccel.z;
134+
135+
_imuMessage.angular_velocity.x = angularVelocity.x;
136+
_imuMessage.angular_velocity.y = -angularVelocity.y;
137+
_imuMessage.angular_velocity.z = angularVelocity.z;
138+
139+
} else if (CoordinateSystem == CoordinateSystemEnum.Unity_LeftHanded_XRight_YUp_ZForward) {
140+
_imuMessage.orientation.x = orientation.x;
141+
_imuMessage.orientation.y = orientation.y;
142+
_imuMessage.orientation.z = orientation.z;
143+
_imuMessage.orientation.w = orientation.w;
144+
145+
_imuMessage.linear_acceleration.x = linearAccel.x;
146+
_imuMessage.linear_acceleration.y = linearAccel.y;
147+
_imuMessage.linear_acceleration.z = linearAccel.z;
148+
149+
_imuMessage.angular_velocity.x = angularVelocity.x;
150+
_imuMessage.angular_velocity.y = angularVelocity.y;
151+
_imuMessage.angular_velocity.z = angularVelocity.z;
152+
153+
}
95154

96155

97156
ROSBridgeConnection.Publish(_imuMessage, ROSTopic, Name);

‎Samples~/ZeroSimSamples/Scenes/ROSIMUPublish_test.unity‎

Lines changed: 19 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2081,6 +2081,7 @@ GameObject:
20812081
- component: {fileID: 1753787005}
20822082
- component: {fileID: 1753787004}
20832083
- component: {fileID: 1753787003}
2084+
- component: {fileID: 1753787008}
20842085
m_Layer: 0
20852086
m_Name: IMU
20862087
m_TagString: Untagged
@@ -2107,6 +2108,7 @@ MonoBehaviour:
21072108
_ROSTopic: imu
21082109
_name: ros.publisher.imu_IMU
21092110
_imuSensor: {fileID: 1753787004}
2111+
_coordinateSystem: 1
21102112
--- !u!114 &1753787004
21112113
MonoBehaviour:
21122114
m_ObjectHideFlags: 0
@@ -2119,7 +2121,7 @@ MonoBehaviour:
21192121
m_Script: {fileID: 11500000, guid: 5c60fd8f844d425c388b59c3631f6062, type: 3}
21202122
m_Name:
21212123
m_EditorClassIdentifier:
2122-
_updateRateHz: 100
2124+
_updateRateHz: 10
21232125
_debug: 0
21242126
_currentUpdateHz: 0
21252127
_currentFixedUpdateHz: 0
@@ -2190,6 +2192,21 @@ Transform:
21902192
m_Father: {fileID: 0}
21912193
m_RootOrder: 7
21922194
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
2195+
--- !u!138 &1753787008
2196+
FixedJoint:
2197+
m_ObjectHideFlags: 0
2198+
m_CorrespondingSourceObject: {fileID: 0}
2199+
m_PrefabInstance: {fileID: 0}
2200+
m_PrefabAsset: {fileID: 0}
2201+
m_GameObject: {fileID: 1753787002}
2202+
m_ConnectedBody: {fileID: 0}
2203+
m_ConnectedArticulationBody: {fileID: 0}
2204+
m_BreakForce: Infinity
2205+
m_BreakTorque: Infinity
2206+
m_EnableCollision: 0
2207+
m_EnablePreprocessing: 1
2208+
m_MassScale: 1
2209+
m_ConnectedMassScale: 1
21932210
--- !u!1 &1764802517
21942211
GameObject:
21952212
m_ObjectHideFlags: 0
@@ -2551,6 +2568,7 @@ MonoBehaviour:
25512568
_currentUpdateHz: 0
25522569
_currentFixedUpdateHz: 0
25532570
_camera: {fileID: 2143639010}
2571+
_isMonochrome: 0
25542572
_width: 640
25552573
_height: 480
25562574
_postProcessMaterial: {fileID: 2100000, guid: d2f03b5f033866ec6b3b16cabc66b331,

‎package.json‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
{
22
"name": "com.fsstudio.zerosim",
3-
"version": "0.0.13",
3+
"version": "0.0.14",
44
"displayName": "ZeroSim",
55
"description": "ZeroSim ROS robotic simulator in Unity.",
66
"unity": "2020.1",

0 commit comments

Comments
 (0)