0

I have created ROS package and integrated with gazebo simulation. So also have IMU ROS driver and can check the ros topic from both side( ros package and gazebo ) correspond accordingly . So with ros2 topic list i can check the topic list. For /robot/imu list i would like to check the IMU outputs angles around x, y & z axis in robot coordinates. The IMU ROS driver msg file is :

std_msgs/Header header
int8 mode
bool gnss_fix
bool imu_error
bool magnetometer_pressure_error
bool gnss_error
bool gnss_heading_ins
bool gnss_compass
sensor_msgs/Imu imu

And the output of ros2 topic echo /robot/imu is

header:
  stamp:
    sec: 1224
    nanosec: 600000000
  frame_id: robot/body_imu/imu_sensor
orientation:
  x: 0.0
  y: 0.0
  z: 0.18261382665583006
  w: 0.9831847183078644
orientation_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
angular_velocity:
  x: 0.0
  y: 0.0
  z: 1.8199688171336792e-10
angular_velocity_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
linear_acceleration:
  x: 0.0
  y: 0.0
  z: 9.800000000000002
linear_acceleration_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
---

As can see IMU orientation is in quaternion . So my question is how to check the IMU outputs angles around x, y & z axis in robot coordinates?

Thanks

TylerH
  • 20,799
  • 66
  • 75
  • 101
Bob9710
  • 205
  • 3
  • 15

1 Answers1

1

You can get Euler angles using this code

import math

def quaternion2euler(q0, q1, q2, q3):
# q0: scalar part

angles = {"roll": 0, "pitch": 0, "yaw": 0}
denom_r = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3
numerator_r = 2 * (q0 * q1 + q2 * q3)
roll = math.atan2(numerator_r, denom_r)
angles["roll"] = roll

pitch = math.asin(2 * (q0 * q2 - q1 * q3))
angles["pitch"] = pitch

numerator_y = 2 * (q0 * q3 + q1 * q2)
denom_y = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3
yaw = math.atan2(numerator_y, denom_y)
angles["yaw"] = yaw

return angles

You can read more on Wikipedia.

neak
  • 69
  • 4