Errors in Kinetic / ARM #55

Hi, I'm really impressed with this code but have run into a couple of issues, I think might be related.

First, I constantly get this error:

[ERROR] [1543867337.867718293]: [CREATE] Unknown mode detected: 252743663270254058981690501133436787895498668826663733637000776013412360950214277709398187746400460009372844193492090595425297940066201946348352200523363198873163318461413287195509944170593093187536689539940275561930708793405606929731851927692420771088109218725054971849345505388159995781185415942242304.000000

I've modified the code to spit out the mode value, this is the very large number.

Additionally, I can't visualize odometry in rviz because of a similar problem. Some of the covariance numbers are just huge:

pose: pose: position: x: 0.000722417084035 y: -7.40265249988e-07 z: 0.0 orientation: x: 0.0 y: 0.0 z: -0.00102470564418 w: 0.999999474989 covariance: [0.000361207639798522, -7.402609298878815e-07, 0.0, 0.0, 0.0, -0.0010246954625472426, -7.402609867313004e-07, 4.551321985957202e-09, 0.0, 0.0, 0.0, 1.0500214557396248e-05, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, -0.0010246954625472426, 1.0500214557396248e-05, 0.0, 0.0, 0.0, 0.026162702590227127]

I've forked the repo and I'd do my best to make changes / fixes if you can point me in the right direction. Thanks!