Pushed Theta Astar implementation
Added support for clearance