height position control in gazebo

edit

Gazebo has a position_3d plugin that will give you the robot's height. You can use this plugin like so:

<gazebo>
  <controller:gazebo_ros_p3d name="p3d_base_controller" plugin="libgazebo_ros_p3d.so">
    <alwaysOn>true</alwaysOn>
    <updateRate>100.0</updateRate>
    <bodyName>base_link</bodyName>
    <topicName>base_pose_ground_truth</topicName>
    <gaussianNoise>0</gaussianNoise>
    <frameName>map</frameName>
    <xyzOffsets>0 0 0</xyzOffsets> 
    <rpyOffsets>0 0 0</rpyOffsets>
    <interface:position name="p3d_base_position"/>
  </controller:gazebo_ros_p3d>
</gazebo>

This plugin will publish a sensor_msgs/Odometry message that you can extract height from.