P2 - Convert GPS Waypoints to Navigation Goals

[erickmu1]

Description:

• IGVC will provide GPS waypoints so Caffeine can navigate the course
• We need to convert those into nav goals so the Navigation Stack can use them to navigate

Goals:

• [x] Convert GPS waypoint to a Nav Goal

Acceptance Criteria:

• [x] Nav Goal is located at the same spot the GPS waypoint refers to
• [x] Can navigate to waypoint specified by the GPS

Notes:

• The UTM frame should provide the conversion GPS to map

[Marc-Morcos]

The code to do what we need is in place but there are a few bugs

1. The coordinates in static_waypoints.json are on the course, but they are not the correct coordinates for the competition, just random points for testing.
2. navigate_waypoints.py has an issue on line 118 where lookup_transform expects our frame not to start with a /, but our actual frame does start with a slash, (the "frame" variable doesnt have a slash at the front either for some reason)

[1999michael]

GPS -> odom/map etc... works, just that GPS isn't accurate enough that our points may end up off course. Need to think about how to get better GPS coordinates or how to compensate

[1999michael]

Why it may not be accurate enough:

1. GPS point recorded isn't good (could average over many trials)
2. GPS isn't very accurate (noise too large)
3. Not enough distance traveled for Filtering to work nicely (though I don't think we have Filtering)

What we can do for testing (only):

1. Reduce GPS noise and verify GPS noise is the cause

[Marc-Morcos]

It seems that the gps noise was already 0 to begin with. there is a gps drift of 1.5 m so i disabled that and made new waypoints. The robot navigates these waypoints nicely (except the one going over the ramp but thats a different issue). For thoroughness, i reenabled the drift in constants.xacro and it still navigated pretty well, within a small (and somehow consistent?) distance of where I put the actual points (which caused one of the points to glitch into a wall). Also load waypoints crashes on startup most of the time for me, although maybe thats just my computer.