Basic Path Planner test

[kev-the-dev]

To verify the feasibility of lqrrt and future path planners, we should have some simple paths to throw at a generic path planner. My idea for the design:

• generate bag file(s) (using real data, simulated data, or hard coded generated data) with odometry and an occupancy grid and store a known achievable end goal (position + orientation) in the global frame. optionally, also store a reasonable distance for a path to reach this goal
• In a rostest, play the bag along with our gnc stack. Command a move to the known end goal and subscribe to the path planner. Verify that a path is produced which ends at the end goal in a reasonable distance and doesn't collide with the ogrid

[kev-the-dev]

Marshall is going to be assigned to this once he joins uf-mil