Prototype for path planning service. Ability to include multiple boundaries (including internal obstacles). Cleanup of package.xml