robot_localization Ouputs NaNs when IMU/Odom messages not NaNing, works in RobotPoseEKF
Hi,
I have a standard configuration for robot_localization to give me a odom->base_footprint transform. We have been using this sensor data and transformations with robot_pose_ekf for over a year now with no issues. When migrating to robot_localization, randomly, after some inconsistent period of time, the entire /odometry/filtered message becomes filled with nans. This happens with no changes in the odom or imu messages (not giving out nans, nothing out of the ordinary here). It has occurred before adding lidar_odometry (odom0).
We get errors about TF_NAN_INPUT and TF_DENORMALIZED_QUATERNION from using the output /odometry/filtered (because their filled with nans when we populate the TF messages) to update TF outside of robot_localization.
I have included example imu and odom messages, all fields of them are being actively published without any NaNs. I also added the normal robot_localization output and what our nan message looks like, with the robot_localization_config.yaml file.
Any thoughts?
odom (odom1) message from wheel encoders
header:
seq: 176323
stamp:
secs: 1507078251
nsecs: 535428047
frame_id: odom
child_frame_id: base_footprint
pose:
pose:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
covariance: [1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0]
twist:
twist:
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
covariance: [1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]
imu message from imu
header:
seq: 157520
stamp:
secs: 1507078338
nsecs: 195523977
frame_id: imu_link
orientation:
x: 0.0101925589928
y: -0.00273427809162
z: -0.501709473646
w: 0.864971814291
orientation_covariance: [1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06]
angular_velocity:
x: 0.0
y: 0.0
z: 0.0
angular_velocity_covariance: [1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06]
linear_acceleration:
x: -0.0453446374339
y: 0.177417214661
z: 9.64665574154
linear_acceleration_covariance: [1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06, 1e-06]
normal robot_localization output
header:
seq: 78377
stamp:
secs: 1507078377
nsecs: 253389359
frame_id: odom
child_frame_id: base_footprint
pose:
pose:
position:
x: -0.000973945901217
y: 0.126273200099
z: -1.61762382101e-17
orientation:
x: -0.000105876156922
y: -0.000161010011301
z: 3.51162535183e-06
w: 0.999999981427
covariance: [9.997287159166042e-07, 5.664801003518761e-29, -2.1928212327851335e-23, 1.995211302619292e-21, 1.9952112977467756e-21, 1.9952112987625185e-21, 5.66480204376903e-29, 9.997287159166042e-07, -1.0024367487167898e-23, 4.133819569988897e-21, 4.133819559834025e-21, 4.133819561950336e-21, -2.192821232213108e-23, -1 ...
Did you check the imu & odom inputs on the exact time when robot_localization outputs NaN values? I recommend to use bag files to analyze the messages.
Also the covariances are really small (arithmetic errors?), how do you get that accurate odometry?
There are no NaNs anywhere in any of the inputs to robot_localization, I have checked with multiple bag files. Nothing on the inputs are out of the ordinary as far as I can tell in values, normalization, rates, frames, delay, etc. Which covariances are you refering to? EKF, odom, or IMU?
All covariances. IMU and ODOM are both 0.000001, that is about micrometer accuracy. And the output of robot_localization is even lower. In your initial_estimate_covariance I even see multiple 1e-9.
They are accurate, those numbers could use some tuning but I just used some defaults for those to get robot_localization off the ground. These are the same values that have worked with robot pose ekf for over a year now, however. Would robot_localization create NaNs here where robot pose ekf didnt?
Can you post a bag file that includes this happening? The only time I have ever seen NaN values is through sensor data or ill-conditioned covariance matrices.
Also, what is publishing the transform to the IMU frame?
I don't have a bag file off hand but from the ill-conditioned covariances clicks well. I'm starting to run it today with updated covariances (IMU only on main diagonal, odom not 1e-6) and I'll update back if the issue resumes