NaNs sometimes appear in tf for hokuyo_link

[Stefan Kohlbrecher]

Running the simulator with our software I sometimes get the following:

#!

Error:   TF_NAN_INPUT: Ignoring transform for child_frame_id "hokuyo_link" from authority "unknown_publisher" because of a nan value in the transform (-0.044600 0.000000 0.088000) (nan nan nan nan)
         at line 240 in /tmp/buildd/ros-indigo-tf2-0.5.7-0trusty-20141230-0040/src/buffer_core.cpp
Error:   TF_NAN_INPUT: Ignoring transform for child_frame_id "hokuyo_link" from authority "unknown_publisher" because of a nan value in the transform (-0.044600 0.000000 0.088000) (nan nan nan nan)
         at line 240 in /tmp/buildd/ros-indigo-tf2-0.5.7-0trusty-20141230-0040/src/buffer_core.cpp

i.e. the orientation quaternion consists of four NaNs. This only happens very rarely, but wanted to have this tracked anyways.

[DouglasS]

Thanks @Stefan_Kohlbrecher. Off the top of my head my guess would be that this is the result of a numerically unstable conversion between two different representations of orientation (we are most likely not using quaternions internally for those poses) but it'll take some time to dig in to it.

[DouglasS]

@Stefan_Kohlbrecher I've dug in to our conversion code for switching between a rigid body transform and a quaternion, and we seem to handle the classic cases of numerical instability correctly. I am unable to create a failing unit test for the common case of instability, which is when the trace of the 4x4 matrix is approaching zero (with traces on the order of magnitude of approx. 1e-323). I haven't given up trying to identify some more corner cases we could be missing but at this point I am leaning towards ROSJava doing something silly when we pass the data in to the publisher.

[Stefan Kohlbrecher]

Thanks for the update. ROSJava occassionally garbling data sounds like a undesirable explanation given the complete API uses that, but probably not impossible.

[DouglasS]

@Stefan_Kohlbrecher I cannot reproduce this locally. I'm not sure if it's because I'm using new models and maybe the old ones weren't generated properly, but all TF's and robot description output seems to work on my end in testing.

[DouglasS]

@Stefan_Kohlbrecher I just reproduced this. It happens if you launch rviz before Simulation Construction Set has completely loaded. Not sure if it's something we should look at on our end or not, but I'll keep it in mind.

[Stefan Kohlbrecher]

@dljsjr Thanks for further looking into it. I'm pretty sure I saw this occassionally while SCS and our onboard software were running, will have another look once the new release is out. This isn't really critical as it only leads to a warning (and a single lost LIDAR scan), so even if it still happens we can probably ignore till past DRC Finals ;)

[DouglasS]

@Stefan_Kohlbrecher Hey Stefan, @btshrewsbury was able to track down the issue and it turns out that there were two things in the SCS stack publishing the transform for the hokuyo_link, which would occasionally cause hiccups. We're testing a fix for it now, and should be able to push a new Java build soon. We'll let you know and you can run the ihmc_sim distribution update script to pull it in.