ROS

Question

我需要融合gps、imu和里程计数据，所以开始测试robot_localization包

我正在为 ekf_localization_node 的输入发布有效的模拟消息 sensor_msgs/Imu 和 nav_msgs/Odometry，然后我正在为 navsat_transform_node 的输入提供里程计来自 ekf_localization_node 输出的消息和模拟 sensor_msgs/NavSatFix 消息。当我启动 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)

一些注意事项：

此处来自‘Latest world frame pose is:’的值全为零，但是他们并不总是一样的。
我已经更改了磁偏角和偏移值，但我得到了相同的结果。
也更改了启动文件中 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]

任何帮助将不胜感激！

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

ROS - 使用 navsat_transform_node 从 robot_localization 包中获取 nan 值

ROS - Getting nan values with navsat_transform_node from robot_localization package

localization

transform

nan