Template OMPL Interface #2

Instead of the hard-wired response returned by the #1 version, integrate with a minimal implementation of an OMPL optimizer. The optimizer will, again, only return straight paths, solely for the purpose of testing integration. Related to roboskel/quadcopter_ompl_planner#1