Refactor Path Planner

Path Planning code is definitely a mess.

We need to refactor it to be consistent with ODCL API. 

i.e.: planner class gets created that takes some arguments (like mission details, parameters, etc.) and then can be run to generate plans. 