incoming wave height service (#152)

osrf · Sep 7, 2023 · d846f5d · d846f5d
1 parent 0b34366
commit d846f5d
Show file tree

Hide file tree

Showing 15 changed files with 994 additions and 21 deletions.
diff --git a/buoy_description/models/mbari_wec_ros/model.sdf b/buoy_description/models/mbari_wec_ros/model.sdf
@@ -53,6 +53,29 @@
       <publish_rate>10</publish_rate>
     </plugin>
 
+    <plugin filename="LatentData" name="buoy_gazebo::LatentDataPublisher">
+      <namespace>/</namespace>
+      <node_name>latent_data</node_name>
+      <ros2_topic>latent_data</ros2_topic>
+      <publish_rate>10</publish_rate>
+    </plugin>
+
+    <plugin filename="IncWaveHeight" name="buoy_gazebo::IncWaveHeight">
+      <namespace>/</namespace>
+      <node_name>inc_wave_service</node_name>
+      <points use_buoy_origin="true">
+        <xy>-1.0 -1.0</xy>
+        <xy>0.0 -1.0</xy>
+        <xy>1.0 -1.0</xy>
+        <xy>-1.0 0.0</xy>
+        <xy>0.0 0.0</xy>
+        <xy>1.0 0.0</xy>
+        <xy>-1.0 1.0</xy>
+        <xy>0.0 1.0</xy>
+        <xy>1.0 1.0</xy>
+      </points>
+    </plugin>
+
     <plugin
       filename="gz-sim-joint-state-publisher-system"
       name="gz::sim::systems::JointStatePublisher">

diff --git a/buoy_gazebo/src/IncidentWaves/CMakeLists.txt b/buoy_gazebo/src/IncidentWaves/CMakeLists.txt
@@ -6,10 +6,10 @@ find_package(Boost 1.71.0 COMPONENTS system iostreams filesystem)
 
 gz_add_plugin(IncidentWaves
   SOURCES
-  IncidentWaves.cpp
+    IncidentWaves.cpp
+  PUBLIC_LINK_LIBS
+    FreeSurfaceHydrodynamics
   INCLUDE_DIRS
     ..
+    ../..
 )
-
-target_link_libraries(IncidentWaves PUBLIC FreeSurfaceHydrodynamics)
-
diff --git a/buoy_gazebo/src/IncidentWaves/IncWaveState.hpp b/buoy_gazebo/src/IncidentWaves/IncWaveState.hpp
@@ -20,14 +20,16 @@
 #include <gz/sim/components/Factory.hh>
 #include <gz/sim/components/Component.hh>
 #include <gz/sim/config.hh>
+
 #include <FreeSurfaceHydrodynamics/LinearIncidentWave.hpp>
 
+
 namespace buoy_gazebo
 {
-/// \brief Structure that holds shared ptr to incident wave object(s), and other data
+/// \brief Structure that holds incident wave object(s), and other data
 struct IncWaveState
 {
-  std::shared_ptr<LinearIncidentWave> Inc;
+  LinearIncidentWave Inc;
   double x;
   double y;
 };

diff --git a/buoy_gazebo/src/IncidentWaves/IncidentWaves.cpp b/buoy_gazebo/src/IncidentWaves/IncidentWaves.cpp
@@ -47,7 +47,7 @@ class IncidentWavesPrivate
   /// \brief Model interface
   gz::sim::Model model{gz::sim::kNullEntity};
 
-  std::shared_ptr<LinearIncidentWave> Inc = std::make_shared<LinearIncidentWave>();
+  LinearIncidentWave Inc{};
 
   gz::sim::Entity IncWaveEntity;
   buoy_gazebo::IncWaveState inc_wave_state;
@@ -82,7 +82,7 @@ void IncidentWaves::Configure(
   }
 
   double seed = SdfParamDouble(_sdf, "IncWaveSeed", 0.0);
-  this->dataPtr->Inc->SetSeed(seed);
+  this->dataPtr->Inc.SetSeed(seed);
 
   auto linkName = _sdf->Get<std::string>("LinkName");
 
@@ -96,15 +96,15 @@ void IncidentWaves::Configure(
     double A = SdfParamDouble(_sdf, "A", 0.0);
     double T = SdfParamDouble(_sdf, "T", 14.0);
     double phase = SdfParamDouble(_sdf, "Phase", 0.0);
-    this->dataPtr->Inc->SetToMonoChromatic(A, T, phase, beta);
+    this->dataPtr->Inc.SetToMonoChromatic(A, T, phase, beta);
   }
 
   if (!SpectrumType.compare("Bretschneider")) {
     gzdbg << "SpectrumType " << SpectrumType << std::endl;
     double Hs = SdfParamDouble(_sdf, "Hs", 0.0);
     double Tp = SdfParamDouble(_sdf, "Tp", 14.0);
     gzdbg << "Hs = " << Hs << "  Tp = " << Tp << std::endl;
-    this->dataPtr->Inc->SetToBretschneiderSpectrum(Hs, Tp, beta);
+    this->dataPtr->Inc.SetToBretschneiderSpectrum(Hs, Tp, beta);
   }
 
   if (!SpectrumType.compare("Custom")) {
@@ -124,7 +124,7 @@ void IncidentWaves::Configure(
     }
 
     if (freq.size() > 2) {  // \TODO(anyone):  Add more checks on validity of spectrum
-      this->dataPtr->Inc->SetToCustomSpectrum(freq, S, beta);
+      this->dataPtr->Inc.SetToCustomSpectrum(freq, S, beta);
     } else {
       gzwarn << "Ill-formed custom wave-spectrum specification, no waves added" << std::endl;
     }
@@ -153,8 +153,7 @@ void IncidentWaves::PreUpdate(
   }
   auto SimTime = std::chrono::duration<double>(_info.simTime).count();
 
-
-//  buoy_gazebo::IncWaveState inc_wave_state;
+  // buoy_gazebo::IncWaveState inc_wave_state;
   if (_ecm.EntityHasComponentType(
       this->dataPtr->IncWaveEntity,
       buoy_gazebo::components::IncWaveState().TypeId()))
@@ -165,7 +164,7 @@ void IncidentWaves::PreUpdate(
     this->dataPtr->inc_wave_state = buoy_gazebo::IncWaveState(inc_wave_state_comp->Data());
   }
   double deta_dx{0.0}, deta_dy{0.0};
-  double eta = this->dataPtr->Inc->eta(
+  double eta = this->dataPtr->Inc.eta(
     this->dataPtr->inc_wave_state.x,
     this->dataPtr->inc_wave_state.y,
     SimTime, &deta_dx, &deta_dy);

diff --git a/buoy_gazebo/src/LatentData/CMakeLists.txt b/buoy_gazebo/src/LatentData/CMakeLists.txt
@@ -0,0 +1,6 @@
+file(GLOB dirs LIST_DIRECTORIES true *)
+foreach(dir ${dirs})
+  if(IS_DIRECTORY ${dir})
+    add_subdirectory(${dir})
+  endif()
+endforeach()
diff --git a/buoy_gazebo/src/LatentData/IncWaveHeight/CMakeLists.txt b/buoy_gazebo/src/LatentData/IncWaveHeight/CMakeLists.txt
@@ -0,0 +1,16 @@
+find_package(FreeSurfaceHydrodynamics REQUIRED)
+set(BOOST_USE_STATIC_LIBS OFF)
+set(BOOST_USE_MULTITHREADED ON)
+set(BOOST_USE_STATIC_RUNTIME OFF)
+find_package(Boost 1.71.0 COMPONENTS system iostreams filesystem)
+
+gz_add_plugin(IncWaveHeight
+  SOURCES
+    IncWaveHeight.cpp
+  PUBLIC_LINK_LIBS
+    FreeSurfaceHydrodynamics
+  INCLUDE_DIRS
+    ../..
+    ..
+  ROS
+)