Create benchmark goals for autonomy testing

## Description

Need to create benchmark goals in both map frame and lat/long in the Gazebo Mars world, and also an empty world with simple obstacles like cubes, ramps, etc.