Fix Goalie ID Assignment in Sensor Fusion

[sauravbanna]

Description

In Sensor Fusion, we process the data we get from the SSL Camera into a World proto with Robots on each team.

Currently, we assign an ID to be the goalie based on either the SensorFusionConfig or a default of 0.

However, we can run into the case where the assigned goalie is not present in the game, which causes a "Robot with ID not found" error.

This PR adds an extra check to make sure the assigned goalie exists in game. If not, it assigns the next lowest ID to be the goalie.

Testing Done

Tested in sim, especially in sandbox mode by removing robots. When active goalie was removed, the next lowest ID was automatically made goalie.

Resolved Issues

Length Justification and Key Files to Review

Review Checklist

It is the reviewers responsibility to also make sure every item here has been covered

• Function & Class comments: All function definitions (usually in the .h file) should have a javadoc style comment at the start of them. For examples, see the functions defined in thunderbots/software/geom. Similarly, all classes should have an associated Javadoc comment explaining the purpose of the class.
• Remove all commented out code
• Remove extra print statements: for example, those just used for testing
• Resolve all TODO's: All TODO (or similar) statements should either be completed or associated with a github issue

[sauravbanna]

TODO: add test for 0 robot default goalie behavior