From fba4812bf884536a082a0b7f2a1e522dfbcfcb57 Mon Sep 17 00:00:00 2001
From: Rhys Mainwaring <rhys.mainwaring@me.com>
Date: Mon, 10 Oct 2022 08:34:24 +0100
Subject: [PATCH] Wave Body: use model:// URI to specify BEM file location
 (#72)

- Use model://<model_path_to_config> URI syntax to locate BEM data files.

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
---
 gz-waves-models/worlds/ellipsoid_buoy.sdf     | 333 +++++++++---------
 gz-waves-models/worlds/spheroid_test1a.sdf    |   3 +-
 .../linear_wave_body/LinearWaveBody.cc        |   8 +-
 3 files changed, 176 insertions(+), 168 deletions(-)

diff --git a/gz-waves-models/worlds/ellipsoid_buoy.sdf b/gz-waves-models/worlds/ellipsoid_buoy.sdf
index e335312e..ea7aaeec 100644
--- a/gz-waves-models/worlds/ellipsoid_buoy.sdf
+++ b/gz-waves-models/worlds/ellipsoid_buoy.sdf
@@ -104,181 +104,182 @@
       <plugin
           filename="gz-waves1-linear-wave-body-system"
           name="gz::sim::systems::LinearWaveBody">
-          <hdf5_file>/Users/rhys/Code/robotics/gz_waves_ws/src/asv_wave_sim/gz-waves-models/models/ellipsoid_buoy/config/hydrodata/ellipsoid_f5244.h5</hdf5_file>
 
-          <!-- waves overrides -->
-          <waves>
-            <regular>
-              <period>6</period>
-              <height>4</height>
-              <direction>0</direction>
-              <phase>0</phase>
-            </regular>
-          </waves>
+        <hdf5_file>model://ellipsoid_buoy/config/hydrodata/ellipsoid_f5244.h5</hdf5_file>
 
-          <!-- simulation parameter overrides -->
-          <simulation_parameters>
-            <gravity>9.81</gravity>
-            <fluid_density>1025</fluid_density>
-          </simulation_parameters>
+        <!-- waves overrides -->
+        <waves>
+          <regular>
+            <period>6</period>
+            <height>4</height>
+            <direction>0</direction>
+            <phase>0</phase>
+          </regular>
+        </waves>
 
-          <!-- geometry overrides -->
-          <geometry>
-            <center_of_waterplane>0 0 0</center_of_waterplane>
-            <center_of_buoyancy>0 0 -0.935</center_of_buoyancy>
-            <displaced_volume>129.1381</displaced_volume>
-          </geometry>
+        <!-- simulation parameter overrides -->
+        <simulation_parameters>
+          <gravity>9.81</gravity>
+          <fluid_density>1025</fluid_density>
+        </simulation_parameters>
+
+        <!-- geometry overrides -->
+        <geometry>
+          <center_of_waterplane>0 0 0</center_of_waterplane>
+          <center_of_buoyancy>0 0 -0.935</center_of_buoyancy>
+          <displaced_volume>129.1381</displaced_volume>
+        </geometry>
 
-          <!-- unscaled hydrodynamics coefficients (overrides) -->
-          <hydro_coeffs>
-            <scaled>0</scaled>
-            <hydrostatic>
-              <linear_restoring>
-                0.        0.        0.        0.        0.        0.
-                0.        0.        0.        0.        0.        0.
-                0.        0.   777494.4      -3.6    -402.7       0.
-                0.        0.       -3.6 6167224.2      -0.1       0.
-                0.        0.     -402.7      -0.1 6179979.4       0.
-                0.        0.        0.        0.        0.        0.
-              </linear_restoring>
-            </hydrostatic>
-            <radiation>
-              <!-- 
-                1. Added mass calculated using Capytaine and converted
-                to WEC-Sim hdf5 with bemio, T = 6.0s, w = 1.0472 rad/s
-              -->
-              <added_mass>
-                52732.7109003133   -0.0461102712478722 5.88390664387022 0.296417351644741 246367.266847758 -1.69953653780694
-                -0.0678279438415122 52944.8240664938 -1.47610970294622 -246974.480351485 -0.376605346811632 34.7833617031108
-                5.56653991122738 0.0361713254775427 197297.142256690 0.802697521049529 -87.4935912381782 -0.0504125898702509
-                0.410840691063419 -247341.339091709 5.02714890072835 1214533.79620113 2.83888902153701 -166.779120688123
-                246734.857503928 -0.346433003966073 -87.1795788996776 2.60143644204397 1215195.98272167 -2.36242713115735
-                -1.30857894347732 34.6478012553392 0.0173325214982212 -165.977540011457 -1.09781377470450 108.258384753611
-              </added_mass>
-              <!--
-                2. Clean version of 1. above. BL and TR blocks skew symmetric,
-                globally symmetrised. Mainly here to check that the warnings
-                raised by DART about lack of zeros and (skew)symmetry do
-                not have a material impact.
-              -->
-              <!-- <added_mass>
-                  52733.       0.       0.       0.  246854.      -2.
-                      0.   52945.       0. -246854.       0.      61.
-                      0.       0.  197297.       2.     -61.       0.
-                      0. -246854.       2. 1214534.       3.    -166.
-                246854.       0.     -61.       3. 1215196.      -2.
-                    -2.      61.       0.    -166.      -2.     108.
-              </added_mass> -->
-              <damping>
-                7209.55521458289 -0.00241865277674112 -0.656452733818339 0.0362151909394597 33068.9436025348 -0.277065103262815
-                -0.00574983097131929 7224.96421786452 0.0739544503252989 -33093.8859951854 -0.0481987286722465 4.65143300682749
-                -0.894991216445364 1.44416946591210 114644.248735475 -6.26262145357765 -57.3651869899508 -0.0632414440092088
-                0.0458640797833837 -33189.4228421519 -2.06423557434899 152022.421220666 0.311347231737735 -21.3664338314253
-                33165.4358604128 -0.0326285489204398 -57.6246853052043 0.264850645030705 152122.332407559 -1.27495177084548
-                -0.231015729667316 4.65422866544508 -0.00392458624416725 -21.3177927485565	-1.06001165595936 0.0259605842341352
-              </damping>
-            </radiation>
-            <excitation>
-              <combined>
-                <re>
-                  3750.27966906368
-                  1.36136318915143
-                  422045.810882563
-                  -12.1223298789796
-                  17026.4196175993
-                  -0.120671686522160
-                </re>
-                <im>
-                  154952.048410117
-                  0.0258067417631121
-                  119022.435012338
-                  -1.48920899489199
-                  712781.363146319
-                  -5.43599198405567
-                </im>
-              </combined>
-              <froude_krylov>
-                <re>
-                  9.23543827040243
-                  0.0162480147898594
-                  622401.014204239
-                  -6.38980408679980
-                  -282.389237456753
-                  -0.000103055222405902
-                </re>
-                <im>
-                  126959.968499544
-                  -0.0166309938157517
-                  -34.6068541762491
-                  0.533226736595381
-                  590993.613169733
-                  -3.78930171294708
-                </im>
-              </froude_krylov>
-              <scattering>
-                <re>
-                  3741.04423079327
-                  1.34511517436157
-                  -200355.203321676
-                  -5.73252579217983
-                  17308.8088550560
-                  -0.120568631299754
-                </re>
-                <im>
-                  27992.0799105738
-                  0.0424377355788638
-                  119057.041866514
-                  -2.02243573148737
-                  121787.749976586
-                  -1.64669027110858
-                </im>
-              </scattering>
-            </excitation>
-          </hydro_coeffs>
+        <!-- unscaled hydrodynamics coefficients (overrides) -->
+        <hydro_coeffs>
+          <scaled>0</scaled>
+          <hydrostatic>
+            <linear_restoring>
+              0.        0.        0.        0.        0.        0.
+              0.        0.        0.        0.        0.        0.
+              0.        0.   777494.4      -3.6    -402.7       0.
+              0.        0.       -3.6 6167224.2      -0.1       0.
+              0.        0.     -402.7      -0.1 6179979.4       0.
+              0.        0.        0.        0.        0.        0.
+            </linear_restoring>
+          </hydrostatic>
+          <radiation>
+            <!-- 
+              1. Added mass calculated using Capytaine and converted
+              to WEC-Sim hdf5 with bemio, T = 6.0s, w = 1.0472 rad/s
+            -->
+            <added_mass>
+              52732.7109003133   -0.0461102712478722 5.88390664387022 0.296417351644741 246367.266847758 -1.69953653780694
+              -0.0678279438415122 52944.8240664938 -1.47610970294622 -246974.480351485 -0.376605346811632 34.7833617031108
+              5.56653991122738 0.0361713254775427 197297.142256690 0.802697521049529 -87.4935912381782 -0.0504125898702509
+              0.410840691063419 -247341.339091709 5.02714890072835 1214533.79620113 2.83888902153701 -166.779120688123
+              246734.857503928 -0.346433003966073 -87.1795788996776 2.60143644204397 1215195.98272167 -2.36242713115735
+              -1.30857894347732 34.6478012553392 0.0173325214982212 -165.977540011457 -1.09781377470450 108.258384753611
+            </added_mass>
+            <!--
+              2. Clean version of 1. above. BL and TR blocks skew symmetric,
+              globally symmetrised. Mainly here to check that the warnings
+              raised by DART about lack of zeros and (skew)symmetry do
+              not have a material impact.
+            -->
+            <!-- <added_mass>
+                52733.       0.       0.       0.  246854.      -2.
+                    0.   52945.       0. -246854.       0.      61.
+                    0.       0.  197297.       2.     -61.       0.
+                    0. -246854.       2. 1214534.       3.    -166.
+              246854.       0.     -61.       3. 1215196.      -2.
+                  -2.      61.       0.    -166.      -2.     108.
+            </added_mass> -->
+            <damping>
+              7209.55521458289 -0.00241865277674112 -0.656452733818339 0.0362151909394597 33068.9436025348 -0.277065103262815
+              -0.00574983097131929 7224.96421786452 0.0739544503252989 -33093.8859951854 -0.0481987286722465 4.65143300682749
+              -0.894991216445364 1.44416946591210 114644.248735475 -6.26262145357765 -57.3651869899508 -0.0632414440092088
+              0.0458640797833837 -33189.4228421519 -2.06423557434899 152022.421220666 0.311347231737735 -21.3664338314253
+              33165.4358604128 -0.0326285489204398 -57.6246853052043 0.264850645030705 152122.332407559 -1.27495177084548
+              -0.231015729667316 4.65422866544508 -0.00392458624416725 -21.3177927485565	-1.06001165595936 0.0259605842341352
+            </damping>
+          </radiation>
+          <excitation>
+            <combined>
+              <re>
+                3750.27966906368
+                1.36136318915143
+                422045.810882563
+                -12.1223298789796
+                17026.4196175993
+                -0.120671686522160
+              </re>
+              <im>
+                154952.048410117
+                0.0258067417631121
+                119022.435012338
+                -1.48920899489199
+                712781.363146319
+                -5.43599198405567
+              </im>
+            </combined>
+            <froude_krylov>
+              <re>
+                9.23543827040243
+                0.0162480147898594
+                622401.014204239
+                -6.38980408679980
+                -282.389237456753
+                -0.000103055222405902
+              </re>
+              <im>
+                126959.968499544
+                -0.0166309938157517
+                -34.6068541762491
+                0.533226736595381
+                590993.613169733
+                -3.78930171294708
+              </im>
+            </froude_krylov>
+            <scattering>
+              <re>
+                3741.04423079327
+                1.34511517436157
+                -200355.203321676
+                -5.73252579217983
+                17308.8088550560
+                -0.120568631299754
+              </re>
+              <im>
+                27992.0799105738
+                0.0424377355788638
+                119057.041866514
+                -2.02243573148737
+                121787.749976586
+                -1.64669027110858
+              </im>
+            </scattering>
+          </excitation>
+        </hydro_coeffs>
 
-          <!-- enable / disable forces -->
-          <forces>
-            <gravity_on>1</gravity_on>
-            <buoyancy_on>1</buoyancy_on>
-            <hydrostatic_restoring_on>1</hydrostatic_restoring_on>
-            <radiation_damping_on>1</radiation_damping_on>
-            <radiation_added_mass_on>1</radiation_added_mass_on>
-            <excitation_on>1</excitation_on>
-            <excitation_froude_krylov_on>1</excitation_froude_krylov_on>
-            <excitation_scattering_on>1</excitation_scattering_on>
-          </forces>
+        <!-- enable / disable forces -->
+        <forces>
+          <gravity_on>1</gravity_on>
+          <buoyancy_on>1</buoyancy_on>
+          <hydrostatic_restoring_on>1</hydrostatic_restoring_on>
+          <radiation_damping_on>1</radiation_damping_on>
+          <radiation_added_mass_on>1</radiation_added_mass_on>
+          <excitation_on>1</excitation_on>
+          <excitation_froude_krylov_on>1</excitation_froude_krylov_on>
+          <excitation_scattering_on>1</excitation_scattering_on>
+        </forces>
 
-          <!-- enable / disable debug messages -->
-          <debug>
-            <gravity_on>0</gravity_on>
-            <buoyancy_on>0</buoyancy_on>
-            <radiation_damping_on>0</radiation_damping_on>
-            <radiation_added_mass_on>0</radiation_added_mass_on>
-            <excitation_on>0</excitation_on>
-          </debug>
+        <!-- enable / disable debug messages -->
+        <debug>
+          <gravity_on>0</gravity_on>
+          <buoyancy_on>0</buoyancy_on>
+          <radiation_damping_on>0</radiation_damping_on>
+          <radiation_added_mass_on>0</radiation_added_mass_on>
+          <excitation_on>0</excitation_on>
+        </debug>
 
-          <!-- publish forces -->
-          <publishers>
-            <update_rate>20</update_rate>
+        <!-- publish forces -->
+        <publishers>
+          <update_rate>20</update_rate>
 
-            <gravity_on>1</gravity_on>
-            <buoyancy_on>1</buoyancy_on>
-            <hydrostatic_restoring_on>1</hydrostatic_restoring_on>
-            <radiation_damping_on>1</radiation_damping_on>
-            <radiation_added_mass_on>1</radiation_added_mass_on>
-            <excitation_on>1</excitation_on>
-            <excitation_froude_krylov_on>1</excitation_froude_krylov_on>
-            <excitation_scattering_on>1</excitation_scattering_on>
+          <gravity_on>1</gravity_on>
+          <buoyancy_on>1</buoyancy_on>
+          <hydrostatic_restoring_on>1</hydrostatic_restoring_on>
+          <radiation_damping_on>1</radiation_damping_on>
+          <radiation_added_mass_on>1</radiation_added_mass_on>
+          <excitation_on>1</excitation_on>
+          <excitation_froude_krylov_on>1</excitation_froude_krylov_on>
+          <excitation_scattering_on>1</excitation_scattering_on>
 
-            <gravity_topic>/force/gravity</gravity_topic>
-            <buoyancy_topic>/force/buoyancy</buoyancy_topic>
-            <hydrostatic_restoring_topic>/force/restoring</hydrostatic_restoring_topic>
-            <radiation_damping_topic>/force/radiation_damping</radiation_damping_topic>
-            <radiation_added_mass_topic>/force/radiation_added_mass</radiation_added_mass_topic>
-            <excitation_topic>/force/excitation</excitation_topic>
-            <excitation_froude_krylov_topic>/force/excitation_froude_krylov</excitation_froude_krylov_topic>
-            <excitation_scattering_topic>/force/excitation_scattering</excitation_scattering_topic>
-          </publishers>
+          <gravity_topic>/force/gravity</gravity_topic>
+          <buoyancy_topic>/force/buoyancy</buoyancy_topic>
+          <hydrostatic_restoring_topic>/force/restoring</hydrostatic_restoring_topic>
+          <radiation_damping_topic>/force/radiation_damping</radiation_damping_topic>
+          <radiation_added_mass_topic>/force/radiation_added_mass</radiation_added_mass_topic>
+          <excitation_topic>/force/excitation</excitation_topic>
+          <excitation_froude_krylov_topic>/force/excitation_froude_krylov</excitation_froude_krylov_topic>
+          <excitation_scattering_topic>/force/excitation_scattering</excitation_scattering_topic>
+        </publishers>
 
       </plugin>
 
diff --git a/gz-waves-models/worlds/spheroid_test1a.sdf b/gz-waves-models/worlds/spheroid_test1a.sdf
index e3b8497c..e3b98ebc 100644
--- a/gz-waves-models/worlds/spheroid_test1a.sdf
+++ b/gz-waves-models/worlds/spheroid_test1a.sdf
@@ -53,7 +53,8 @@
       <plugin
           filename="gz-waves1-linear-wave-body-system"
           name="gz::sim::systems::LinearWaveBody">
-        <hdf5_file>/Users/rhys/Code/robotics/gz_waves_ws/src/asv_wave_sim/gz-waves-models/models/spheroid_test1a/config/hydrodata/spheroid_f3676.h5</hdf5_file>
+
+        <hdf5_file>models://spheroid_test1a/config/hydrodata/spheroid_f3676.h5</hdf5_file>
 
         <waves>
           <regular>
diff --git a/gz-waves/src/systems/linear_wave_body/LinearWaveBody.cc b/gz-waves/src/systems/linear_wave_body/LinearWaveBody.cc
index 7229f532..7ad82963 100644
--- a/gz-waves/src/systems/linear_wave_body/LinearWaveBody.cc
+++ b/gz-waves/src/systems/linear_wave_body/LinearWaveBody.cc
@@ -16,6 +16,7 @@
 #include "LinearWaveBody.hh"
 
 #include <gz/common/Profiler.hh>
+#include <gz/common/SystemPaths.hh>
 
 #include <gz/msgs/wrench.pb.h>
 
@@ -763,7 +764,12 @@ void LinearWaveBody::Configure(const Entity &_entity,
   // 1a. read WEC-Sim hdf5 BEM file
   if (_sdf->HasElement("hdf5_file"))
   {
-    this->dataPtr->hydroData.hdf5File = _sdf->Get<std::string>("hdf5_file");
+    std::string fileUri = _sdf->Get<std::string>("hdf5_file");
+
+    // Find the file path
+    common::SystemPaths systemPaths;
+    this->dataPtr->hydroData.hdf5File = systemPaths.FindFileURI(fileUri);
+
     this->dataPtr->ReadWECSim(_ecm);
   }
   else