Merge pull request #16 from nilsbore/master

Added optional 3d obstacle avoidance to scitos_2d_navigation. We've been running for a week and seems to work. The downsampling of the cloud takes quite a lot of cpu (about 30% of one cpu) but I can't think of any faster way to do it (approximate voxel grid atm, ideas?). This is dependent on merged pull request https://github.com/strands-project/scitos_common/pull/26 since we need good estimate of chest camera height and angle with respect to floor. Also made the costmap update at 3 hz instead of 5 to make sure it would always complete on time. Needs more testing with people around. As mentioned, with previous usage this won't make any difference.
Update README.md
Update README.md
Update README.md
Plugged a memory leak. (history_ never deleted.)
Changed the raytrace range to be longer