diff --git a/Runtime/Scripts/ROS/Unity/Publishers/ZOROSIMUPublisher.cs b/Runtime/Scripts/ROS/Unity/Publishers/ZOROSIMUPublisher.cs index ae6318e..b2422b0 100644 --- a/Runtime/Scripts/ROS/Unity/Publishers/ZOROSIMUPublisher.cs +++ b/Runtime/Scripts/ROS/Unity/Publishers/ZOROSIMUPublisher.cs @@ -29,7 +29,7 @@ public ZOIMU IMUSensor { } public enum CoordinateSystemEnum { - RightHanded_XBackward_YLeft_ZUp, + RightHanded_XBackward_YLeft_ZDown, RightHanded_XRight_YDown_ZForward, // RealSense D435i Unity_LeftHanded_XRight_YUp_ZForward, // Unity Standard ROS_RightHanded_XForward_YLeft_ZUp, // ROS Standard @@ -128,7 +128,7 @@ private Task OnPublishImuDelegate(ZOIMU lidar, string name, Vector3 linearAccel, _imuMessage.angular_velocity.UnityVector3 = angularVelocity; _imuMessage.orientation.UnityQuaternion = orientation; - } else if (CoordinateSystem == CoordinateSystemEnum.RightHanded_XBackward_YLeft_ZUp) { + } else if (CoordinateSystem == CoordinateSystemEnum.RightHanded_XBackward_YLeft_ZDown) { // (x, y, z, w) -> (-x, -z, y, -w). _imuMessage.orientation.x = -orientation.z; _imuMessage.orientation.y = -orientation.x; @@ -137,11 +137,11 @@ private Task OnPublishImuDelegate(ZOIMU lidar, string name, Vector3 linearAccel, _imuMessage.linear_acceleration.x = -linearAccel.z + -gravity.z; _imuMessage.linear_acceleration.y = -linearAccel.x + -gravity.x; - _imuMessage.linear_acceleration.z = linearAccel.y + gravity.y; + _imuMessage.linear_acceleration.z = -linearAccel.y - gravity.y; _imuMessage.angular_velocity.x = -angularVelocity.z; _imuMessage.angular_velocity.y = -angularVelocity.x; - _imuMessage.angular_velocity.z = angularVelocity.y; + _imuMessage.angular_velocity.z = -angularVelocity.y; } else if (CoordinateSystem == CoordinateSystemEnum.RightHanded_XRight_YDown_ZForward) { // aka RealSense Quaternion flippedYOrientation = Quaternion.Euler(Vector3.Scale(orientation.eulerAngles, Vector3.up * -1)); _imuMessage.orientation.x = flippedYOrientation.x;