Fix X500 kinetic energy threshold

[nkoenig]

Resolves issue #954.

I tested each robot type using the steps described in issue #954:

1. Start simulation with a cave world.
2. rostopic pub -1 X1/cmd_vel geometry_msgs/Twist -- '[0,0,5]' '[0,0,0]'
3. Wait for the robot to reach the grass area above the cave entrance
4. rostopic pub -1 X1/cmd_vel geometry_msgs/Twist -- '[0,0,0]' '[0,0,0]'

I found an issue only with the X500 robot. The minimum factor that would trigger a collision was 5.2. Based on this, I updated the X500 to have a factor of 5.

The update to the if statement is just an extra safety check to make sure that the maps have been initialized.

Signed-off-by: Nate Koenig nate@openrobotics.org

[acschang]

Reproduced the issue on final_event and then could not reproduce the issue on this branch.

LGTM.

[iche033]

replaced by #973