Testing position action server
catkinized transverse_mercator library
Added simple 2D vector math library
Separated planner from action server