Getting roll pitch and yaw from accelerometer, gyro and magnetometer readings
I am trying to get the roll, pitch, yaw data from my LMS9DS1 IMU sensor. My setup is this:
The IMU data is read through the I2C of my arduino Uno. The accelerometer, gyro and magnetometer data are sent to my via rosserial communication with my laptop.
On the laptop, I listen to the IMU data published from the arduino. This data is then used to create an IMU message to be sent to imu_filter_madgwick on the /imu/data_raw
and /imu/magentic_field
topics for the IMU and magnetometer messages respectively.
I expect the imu_filter_madgwick to compute the x, y, z orientation for me which I can then use to compute the pitch, yaw and roll orientations.
My problem is that when I listen to the /imu/data
topic, the orientation data shows nan for x, y, z values.
I am new to using IMU and I will really appreciate if someone can show me what I have been doing wrong, or which tool to use.
My codes: 1. IMU listener from the serial port:
`#!/usr/bin/env python
import rospy
import roslib
from std_msgs.msg import String
from std_msgs.msg import Float64
from std_msgs.msg import Bool
from sensor_msgs.msg import Imu
from sensor_msgs.msg import MagneticField
mag_msg = MagneticField()
imu_msg = Imu()
def serial_callback(data):
global imu_msg, mag_msg
acc_mag_gyr = [float(i) for i in data.data.split(',')]
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.angular_velocity.x = acc_mag_gyr[0]
imu_msg.angular_velocity.y = acc_mag_gyr[1]
imu_msg.angular_velocity.z = acc_mag_gyr[2]
imu_msg.angular_velocity_covariance[0] = -1
mag_msg = MagneticField()
mag_msg.header.stamp = rospy.Time.now()
mag_msg.magnetic_field.x = acc_mag_gyr[3]
mag_msg.magnetic_field.y = acc_mag_gyr[4]
mag_msg.magnetic_field.z = acc_mag_gyr[5]
imu_msg.linear_acceleration.x = acc_mag_gyr[6]
imu_msg.linear_acceleration.y = acc_mag_gyr[7]
imu_msg.linear_acceleration.z = acc_mag_gyr[8]
imu_msg.linear_acceleration_covariance[0] = -1
def listener():
sub = rospy.Subscriber('chatter',String,serial_callback,queue_size=1)
pub = rospy.Publisher('/imu/data_raw',Imu, queue_size=1)
pub1 = rospy.Publisher('/imu/magnetic_field',MagneticField, queue_size=1)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
pub.publish(imu_msg)
pub1.publish(mag_msg)
rate.sleep()
if __name__=='__main__':
node = rospy.init_node('my_rover',anonymous=True)
try:
listener()
except rospy.ROSInterruptException:
print('Something Went wrong')
pass
`
2 Launch file to run my package and imu_filter_madgwick packages:
<launch>
<node pkg="rosserial_python" type="serial_node.py" name="serial_node" output="screen">
<param name="port" value="/dev/ttyACM0"/>
</node>
<node pkg='serial_com' type='my_rover3.py' name='my_imu_pub' output='screen'>
</node>
<node pkg='imu_filter_madgwick' type='imu_filter_node' name='imu_filter' output='screen'>
</node>
</launch>
The results of listening to the /imu/data
topic:
header:
seq: 66
stamp:
secs: 1522424233
nsecs: 521625041
frame_id: ''
orientation:
x: nan
y: nan
z: nan
w: nan
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
x: -0.16
y: -0.43
z: 10.06
angular_velocity_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
x: 0.94
y: -0.84
z: -6.12
linear_acceleration_covariance: [-1.0, 0.0, 0.0, 0.0 ...
Now, the
/imu/data
sometimes publishes the orientation values and other times it behaves exactly as presented in the question. However, I do not know why this is so.