Add imaging sonar to simulation and integrate ogrid

[Kevin Allen]

After merging #242, we noticed we'll need a simulated imaging sonar to test out navigation stack now that it checks for way point validity.