Make state estimation work better in case the robot is not touching with all feet

[peci1]

I did some improvements to the state estimation node that result in estimating the body orientation more or less correctly even if the robot is not standing on all feet.

There are two cases:

1. IMU provides orientation and use_orientation_from_imu param is set to true. In this case, orientation of the body is determined by the feet in case 3 or 4 are touching, and otherwise it uses the Z axis from IMU. If two feet are touching, they are used to define roll of the base_footprint plane. In case 1 or zero legs are touching, the whole orientation is just taken from IMU and aligned to the robot base_link projection along the IMU Z vector.
2. IMU does not provide orientation data. In this case, the Z vector is copied over from the base_link frame, and otherwise, the behavior is the same as in case 1.

I automatically enabled reading the orientation from IMU when running in Gazebo, as Gazebo provides orientation. What more, this orientation is not affected by any kind of noise.

Behavior with this PR (rviz set to base_footprint fixed frame):

https://user-images.githubusercontent.com/182533/112215091-3de21180-8c20-11eb-8b40-1192dacb9066.mp4

Original behavior:

https://user-images.githubusercontent.com/182533/112215146-4f2b1e00-8c20-11eb-80db-53cd9bed31f4.mp4

I hope I understood the base_footprint frame meaning well. I imagine it as the frame that would be the result of a top-down projection of the robot to a flat ground plane.

Of course, I'm not sure if this wouldn't break some code relying on the properties of the worse performing state estimation. But I think it could be worth considering.

[grassjelly]

Hey @peci1 . Been finding ways on how to estimate the body's pose on instances when there's < 4 feet in contact with the ground. This solves this problem. Thought the sampling of point to fill in missing axes with an IMU was brilliant. Thanks for the PR.