ROS-从Robot_localization软件包中使用NAVSAT_TRANSFORM_NODE获取NAN值

Question

我需要融合GPS，IMU和探测数据，因此我开始测试robot_localization软件包。

我正在发布有效的模拟消息sensor_msgs/imu，以及nav_msgs/odometry for ekf_localization_node的输入，然后我将使用ekf_localization_node和odemetry message的odometry message馈送navsat_transform_node的输入。启动navsat_transform_node时，我将获得以下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，Odometry和GPS消息的坐标帧，但收到了相同的错误。

/ODOMETIRE/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]

任何帮助将不胜感激！

Answer 1

如果有人有相同的问题，则完整的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