Extension of ROS navigation stack 2D to 3D

by Ray Tan for BumbleBee Autonomous Underwater Vehicle (BBAUV)

The proposal strives to extend the navigation stack from 2D to 3D, as well as fixing some bugs and issues related to the navigation stack package.