Getting roll pitch and yaw from accelerometer, gyro and magnetometer readings

asked 2018-03-30 11:09:52 -0600

elcymon gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

Comments

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.

elcymon gravatar imageelcymon ( 2018-04-17 05:22:11 -0600 )edit