Rebase suggestions for updates to trilinear interpolation

[tfoote]

This is a replacement for #142 to build on top of #127 instead of on top of #83

[mabelzhang]

Hmm, I see how this would simplify the search, but I think it makes the algorithm less robust. As you said, 5 meters is a magic number. It is very specific to the one specific data set we have. Right now, we don't have a way of calculating the resolution, and we know that resolution will depend on where you are in the data. If we upstream with this specific resolution, it's not going to work in the general case. The current code works in a more general case, whatever the granularity the data. For that, I think the complexity is worth it. We're not running into performance issues to need to simplify it.

[mabelzhang]

I tried testing this on the Test case 1 in #127, the interpolation_prism.csv, with the following changes in buoyant_tethys.sdf, and I wasn't able to run it. When I unpause the simulation, I got a seg fault. Are you able to reproduce it? The seg fault doesn't happen in #127.

Uncomment this lat long, comment out all others:

      <!-- For simple_test.csv and interpolation_test.csv -->
      <latitude_deg>0</latitude_deg>
      <longitude_deg>0</longitude_deg>

Uncomment this csv file, comment out all others:

    <plugin
      filename="ScienceSensorsSystem"
      name="tethys::ScienceSensorsSystem">
      <data_path>interpolation_prism.csv</data_path>
    </plugin>

Modify the robot position:

    <include>
      <pose>4 -5 0 0 0 0</pose>
      <uri>tethys_equipped</uri>
    </include>

[arjo129]

Since we have switched to a new and faster science data lookup architecture, I am going to close this PR.