Navigate to un-occupied points in map

[Christopher-P]

Brittany wrote the code to only nav to points that are not inside the cost map. We need to implement this feature (found in testCode) and create a single method (service not pub/sub) for all systems in RAS to call the navigation service.