search

tu-darmstadt-ros-pkg / hector_gazebo

hector_gazebo provides packages related to the simulation of robots using gazebo (gazebo plugins, world files etc.).
http://www.ros.org/wiki/hector_gazebo
184 stars 157 forks source link

hector_gazebo_ros_imu sometimes publishes NaN angular_velocity #20

Closed jim-rothrock closed 9 years ago

jim-rothrock commented 9 years ago

I'm using the indigo-devel branch. When my simulated robot comes to a stop, hector_gazebo_imu very rarely publishes an angular_velocity with x, y, and z components that are NaN. It happens when delta.w (gazebo_ros_imu.cpp, line 264) is (apparently) slightly greater than 1. I added some debugging code that prints the following when rate.x, rate.y, or rate.z is NaN:

acos(delta.w) = nan, delta.x = -0.0000000032, delta.y = -0.0000000121, delta.z = 0.0000000110, delta.w = 1.0000000000, dt = 0.0100000000

delta.w must be slightly greater than 1, at a precision greater than what I used to print the message, because assigning the above values to delta does not result in NaNs. If delta.w is set to 1.00000000001, however, rate is set to (NaN, NaN, NaN).

I'm working around this by clamping delta.w to the range [-1, 1] immediately before the acos(delta.w) line.

meyerj commented 9 years ago

You are absolutely right. I had exactly the same problem yesterday in another piece of code that uses the acos function.

Fixed in 9425150aed908b5b54200b94a9145e71a0f4cbde. Thanks for reporting!