Localization ROS-从robot_本地化包中通过navsat_transform_节点获取nan值

我需要融合gps、imu和里程计数据，所以我开始测试robot_定位软件包

我为ekf_本地化_节点的输入发布有效的模拟消息sensor_msgs/Imu和nav_msgs/Odometry，然后向navsat_transform_节点的输入提供ekf_本地化_节点输出的里程计消息和模拟sensor_msgs/NavSatFix消息。 当我启动navsat_transform_节点时，我得到以下nan值：

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)

一些注意事项：

• 在这里，“最新世界帧姿势”的值为：，均为零，但 它们并不总是一样的
• 我改变了磁偏角和偏移量的值，但得到了相同的结果
• 还更改了imu、里程计和gps信息在发射文件中的坐标帧，但收到了相同的错误

来自/odometry/gps主题的输出里程计信息为：

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]

---

任何帮助都将不胜感激

如果任何人有相同的问题，完整的IMU模拟消息如下所示：

#!/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

---

如果你在Linux下使用C++，你可以添加<代码> FEAABABLE（FEYUBALL FEYOFFULL）；代码>在代码的开头。这将导致程序在第一个NaN出现的行抛出异常，因此可以使用调试器轻松地跟踪NaN的源（另请参见），谢谢您的回答。我发现了错误！缺少来自IMU的一些模拟值：IMU_msg.orientation.x=1.0；imu_msg.orientation.y=0.0；imu_msg.orientation.z=0.0；imu_msg.orientation.w=0.0；现在一切正常。当做