0

I need to fuse gps, imu and odometry data, so I started to test robot_localization package.

I’m publishing valid mock messages sensor_msgs/Imu, and nav_msgs/Odometry for the inputs of ekf_localization_node, then I’m feeding the input of navsat_transform_node with the odometry message from the output of ekf_localization_node and a mock sensor_msgs/NavSatFix message. When I launch navsat_transform_node I’m getting the following nan values:

process[navsat_transform_node-1]: started with pid [29575]
[ WARN] [1431390696.211039510]: MSG to TF: Quaternion Not Properly Normalized
[ INFO] [1431390696.295605608]: Corrected for magnetic declination of 0.183000, user-specified offset of 1.000000, and fixed offset of 1.570796. Transform heading factor is now nan
[ INFO] [1431390696.295816136]: Latest world frame pose is: 
Position: (0.000000, 0.000000, 0.000000)
Orientation: (0.000000, -0.000000, 0.000000)
[ INFO] [1431390696.295972831]: World frame->utm transform is 
Position: (nan, nan, nan)
Orientation: (nan, -nan, nan)

Some notes:

  • Here the values from ‘Latest world frame pose is:’ are all zeros, but they’re not always the same.
  • I’ve changed the magnetic declination and offset values, but I get the same results.
  • Have also changed the coordinates frames from imu, odometry and gps messages at the launch files, but received the same error.

The output odometry message from /odometry/gps topic is:

pose: 
  pose: 
    position: 
      x: nan
      y: nan
      z: nan
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance: [nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan]

Any help will be appreciated!

Charles P.
  • 1,947
  • 16
  • 13
  • If you are working with C++ under linux, you can add `feenableexcept(FE_INVALID | FE_OVERFLOW);` at the beginning of your code. This will cause the program to throw an exception at the line where the first NaN occurs, so the source of the NaNs can easily be tracked down with the debugger (see also [here](http://stackoverflow.com/a/2949452/2095383)) – luator May 12 '15 at 06:54
  • Thanks for your answer. I've found the error! Some mock values from the IMU were missing: imu_msg.orientation.x = 1.0; imu_msg.orientation.y = 0.0; imu_msg.orientation.z = 0.0; imu_msg.orientation.w = 0.0; Now everything working fine. Regards! – Charles P. May 14 '15 at 00:31

1 Answers1

0

If anyone has the same issue, a full IMU mock message is like the following:

#!/usr/bin/env python

import sys
import roslib
import rospy
import math
import numpy as np

from geometry_msgs.msg import Twist, Point
from sensor_msgs.msg import Imu
from std_msgs.msg import Int64
from tf.transformations import quaternion_about_axis


def imu_publisher():
    vel_x = 3.0
    vel_y = 0.0
    vel_theta = 15

    imu_pub = rospy.Publisher('/imu_data',Imu)

    rospy.init_node("butiaros_imu_publisher",anonymous=True)
    rospy.loginfo ("Topic = /imu_data")

    x = 0.0
    y = 0.0
    theta = 0.0

    current_time = rospy.get_rostime() #en segundos
    last_time = current_time

    rate = rospy.Rate(1) #1 hz (1 seg)
    i = 0
    while not rospy.is_shutdown():
        #rospy.loginfo ("Making Odometry message...")
        #ROS: Imu
        seq = i
        imu_msg = Imu() 

        imu_msg.header.seq = seq
        imu_msg.header.stamp = current_time
        imu_msg.header.frame_id = "base_link"

        # new
        imu_msg.orientation.x = 1.0;
        imu_msg.orientation.y = 0.0;
        imu_msg.orientation.z = 0.0;
        imu_msg.orientation.w = 0.0;  


        # imu_msg.orientation e imu_msg.orientation_covariance
        imu_msg.orientation_covariance[0] = -1

        # imu_msg.linear_acceleration (m/s2)
        imu_msg.linear_acceleration.x = 0.0 
        imu_msg.linear_acceleration.y = 1.0
        imu_msg.linear_acceleration.z = 2.0
        p_cov = np.array([0.0]*9).reshape(3,3)
        p_cov[0,0] = 0.001
        p_cov[1,1] = 0.001
        p_cov[2,2] = 0.001
        imu_msg.linear_acceleration_covariance = tuple(p_cov.ravel().tolist())

        # imu_msg.angular_velocity (rad/sec)
        imu_msg.angular_velocity.x = 0.0
        imu_msg.angular_velocity.y = 1.0
        imu_msg.angular_velocity.z = 2.0
        p_cov2 = np.array([0.0]*9).reshape(3,3)
        p_cov2[0,0] = 0.001
        p_cov2[1,1] = 0.001
        p_cov2[2,2] = 0.001
        imu_msg.angular_velocity_covariance = tuple(p_cov2.ravel().tolist())

        #rospy.loginfo ("Sending Odometry message...")
        imu_pub.publish(imu_msg)

        i = i + 1
        last_time = current_time
        current_time = rospy.get_rostime() # in seconds
        rate.sleep()
        #end_while
    #end_def

if __name__ == '__main__':
    try:
        imu_publisher()
    except rospy.ROSInterruptException:
        pass
Charles P.
  • 1,947
  • 16
  • 13