localization - ROS - Getting nan values with navsat_transform_node from robot_localization package -
i need fuse gps, imu , odometry data, started test robot_localization package.
i’m publishing valid mock messages sensor_msgs/imu, , nav_msgs/odometry inputs of ekf_localization_node, i’m feeding input of navsat_transform_node odometry message output of ekf_localization_node , mock sensor_msgs/navsatfix message. when launch navsat_transform_node i’m getting following nan values:
process[navsat_transform_node-1]: started pid [29575] [ warn] [1431390696.211039510]: msg tf: quaternion not normalized [ info] [1431390696.295605608]: corrected magnetic declination of 0.183000, user-specified offset of 1.000000, , fixed offset of 1.570796. transform heading factor 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 position: (nan, nan, nan) orientation: (nan, -nan, nan) some notes:
- here values ‘latest world frame pose is:’ zeros, they’re not same.
- i’ve changed magnetic declination , offset values, same results.
- have changed coordinates frames imu, odometry , gps messages @ launch files, received same error.
the output odometry message /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 appreciated!
if has same issue, full imu mock message following:
#!/usr/bin/env python import sys import roslib import rospy import math import numpy np geometry_msgs.msg import twist, point sensor_msgs.msg import imu std_msgs.msg import int64 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) = 0 while not rospy.is_shutdown(): #rospy.loginfo ("making odometry message...") #ros: imu seq = 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) = + 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
Comments
Post a Comment