-nan, nan, nan in output of NAVSAT TRANSFORM
I am having an issue with the NAVSAT TRANSFORM node. The output odometry/gps has nan for position and covariance as shown below.
header:
seq: 181
stamp:
secs: 1532908689
nsecs: 471820116
frame_id: odom
child_frame_id: ''
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]
twist:
twist:
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
If I look at the nodes initializing, as soon as the first GPS message comes in the NAVSAT transform sets the transform heading factor to -nan, and I suspect that is where my problem starts. But I am not sure. I have checked the ODOM, IMU, and GPSFIX messages and none of them have nan in them.
[ INFO] [1532908652.683481047]: Datum (latitude, longitude, altitude) is (37.272746, -121.818616, 46.585000)
[ INFO] [1532908652.683769224]: Datum UTM coordinate is (604740.259728, 4125783.673495)
[ INFO] [1532908652.783517516]: Initial odometry pose is Origin: (0 0 0)
Rotation (RPY): (0, -0, 0)
[ INFO] [1532908652.783951393]: Corrected for magnetic declination of 0.244346 and user-specified offset of 0.010000. Transform heading factor is now -nan
[ INFO] [1532908652.784093424]: Transform world frame pose is: Origin: (0 0 0)
Rotation (RPY): (0, -0, 0)
[ INFO] [1532908652.784163532]: World frame->utm transform is Origin: (nan nan nan)
Rotation (RPY): (-nan, nan, -nan)
Any thoughts on what is causing this?
Here is additional back ground as well as the config information as well as samples of the messages in ODOM, GPS, and IMU..
This is on ROS Kinetic with Ubuntu 16.04
The ODOM message comes directly from the initial publisher. I have not yet passed it through the localization node. I building step by step and currently using simulated (fake IMU data) so no need for localization node yet.
Here is the NAVSAT TRANSFORM launch.
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
<param name="magnetic_declination_radians" value="0.244346"/>
<param name="yaw_offset" value="0.01"/>
<remap from="/imu/data" to="/imu_data" />
<remap from="/gps/fix" to="/tcpfix" />
<remap from="/odometry/filtered" to="/odom" />
</node>
Here is GPS message
header:
seq: 3889
stamp:
secs: 1532909977
nsecs: 461611032
frame_id: gps
status:
status: 2
service: 1
latitude: 37.2727510883
longitude: -121.81862461
altitude: 47.194
position_covariance: [0.000144, 0.0 ...