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
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.