Update density value in tethys comm plugin. (#108)

* update density value in tethys comm plugin. Signed-off-by: Arjo Chakravarty <[email protected]> * Implement Chapulina's solutions Signed-off-by: Arjo Chakravarty <[email protected]>
osrf · Dec 8, 2021 · e62cb94 · e62cb94
1 parent 8994733
commit e62cb94
Show file tree

Hide file tree

Showing 4 changed files with 12 additions and 1 deletion.
diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf
@@ -153,6 +153,7 @@
         <command_topic>tethys/command_topic</command_topic>
         <state_topic>tethys/state_topic</state_topic>
         <debug_printout>0</debug_printout>
+        <ocean_density>1000</ocean_density>
       </plugin>
       <plugin
         filename="HydrodynamicsPlugin"

diff --git a/lrauv_ignition_plugins/proto/lrauv_state.proto b/lrauv_ignition_plugins/proto/lrauv_state.proto
@@ -93,6 +93,6 @@ message LRAUVState
   float soundSpeed_               = 28;
   float temperature_              = 29;  // Celsius
   float salinity_                 = 30;  // PSU
-  float density_                  = 31;
+  float density_                  = 31;  // Density of the surrounding water in Kg / m ^ 3
   repeated float values_          = 32;  // Size 4 (0: chlorophyll in ug / L, 1: pressure in Pa)
 }
diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.cc b/lrauv_ignition_plugins/src/TethysCommPlugin.cc
@@ -202,6 +202,10 @@ void TethysCommPlugin::Configure(
   {
     this->debugPrintout = _sdf->Get<bool>("debug_printout");
   }
+  if (_sdf->HasElement("density"))
+  {
+    this->oceanDensity = _sdf->Get<double>("ocean_density");
+  }
 
   // Initialize transport
   if (!this->node.Subscribe(this->commandTopic,
@@ -593,6 +597,9 @@ void TethysCommPlugin::PostUpdate(
   stateMsg.set_temperature_(this->latestTemperature.Celsius());
   stateMsg.add_values_(this->latestChlorophyll);
 
+  // Set Ocean Density
+  stateMsg.set_density_(this->oceanDensity);
+
   double pressure = 0.0;
   if (latlon)
   {

diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.hh b/lrauv_ignition_plugins/src/TethysCommPlugin.hh
@@ -182,6 +182,9 @@ namespace tethys
     /// Latest chlorophyll data received from sensor. NaN if not received.
     private: float latestChlorophyll{std::nanf("")};
 
+    /// Ocean Density in kg / m ^ 3
+    private: double oceanDensity{1000};
+
     /// Latest current data received from sensor. NaN if not received.
     private: ignition::math::Vector3d latestCurrent
         {std::nan(""), std::nan(""), std::nan("")};