Adds support for ocean currents

src/systems/hydrodynamics/Hydrodynamics.cc

@@ -20,6 +20,8 @@
 
 #include <ignition/plugin/Register.hh>
 
+#include "ignition/msgs/vector3d.pb.h"
+
 #include "ignition/gazebo/components/AngularVelocity.hh"
 #include "ignition/gazebo/components/LinearVelocity.hh"
 #include "ignition/gazebo/components/Pose.hh"
@@ -29,6 +31,8 @@
 #include "ignition/gazebo/System.hh"
 #include "ignition/gazebo/Util.hh"
 
+#include "ignition/transport/Node.hh"
+
 #include "Hydrodynamics.hh"
 
 using namespace ignition;
@@ -96,6 +100,12 @@ class ignition::gazebo::systems::HydrodynamicsPrivateData
   /// \brief Water density [kg/m^3].
   public: double waterDensity;
 
+  /// \brief The ignition transport node
+  public: transport::Node node;
+
+  /// \brief Ocean current experienced by this body
+  public: math::Vector3d currentVector {0, 0, 0};
+
   /// \brief Added mass of vehicle;
   /// See: https://en.wikipedia.org/wiki/Added_mass
   Eigen::MatrixXd Ma;
@@ -104,9 +114,23 @@ class ignition::gazebo::systems::HydrodynamicsPrivateData
   public: Eigen::VectorXd prevState;
 
   /// Link entity
-  public: ignition::gazebo::Entity linkEntity;
+  public: Entity linkEntity;
+
+  /// Ocean current callback
+  public: void UpdateCurrent(const msgs::Vector3d &_msg);
+
+  /// Mutex
+  public: std::mutex mtx;
 };
 
+/////////////////////////////////////////////////
+void HydrodynamicsPrivateData::UpdateCurrent(const msgs::Vector3d &_msg)
+{
+  math::Vector3d newCurrVec {_msg.x(), _msg.y(), _msg.z()};
+  std::lock_guard<std::mutex> lock(this->mtx);
+  this->currentVector = newCurrVec;
+}
+
 /////////////////////////////////////////////////
 void AddAngularVelocityComponent(
   const ignition::gazebo::Entity &_entity,
@@ -202,6 +226,29 @@ void Hydrodynamics::Configure(
 
   // Create model object, to access convenient functions
   auto model = ignition::gazebo::Model(_entity);
+
+  std::string ns {""};
+  if (_sdf->HasElement("namespace"))
+  {
+    ns = _sdf->Get<std::string>("namespace");
+  }
+  auto currentTopic = [=]() ->std::string
+  {
+    if(ns == "")
+    {
+      return "/ocean_current";
+    }
+    else
+    {
+      return ignition::transport::TopicUtils::AsValidTopic(
+        "/model/" + ns + "/ocean_current");
+    }
+  }();
+  this->dataPtr->node.Subscribe(
+    currentTopic,
+    &HydrodynamicsPrivateData::UpdateCurrent,
+    this->dataPtr.get());
+
   if(!_sdf->HasElement("link_name"))
   {
     ignerr << "You musk specify a <link_name> for the hydrodynamic"
@@ -266,11 +313,18 @@ void Hydrodynamics::PreUpdate(
     return;
   }
 
+  // Get current vector
+  math::Vector3d currentVector;
+  {
+    std::lock_guard lock(this->dataPtr->mtx);
+    currentVector = this->dataPtr->currentVector;
+  }
   // Transform state to local frame
   auto pose = baseLink.WorldPose(_ecm);
   // Since we are transforming angular and linear velocity we only care about
   // rotation
-  auto localLinearVelocity = pose->Rot().Inverse() * linearVelocity->Data();
+  auto localLinearVelocity = pose->Rot().Inverse() *
+    (linearVelocity->Data() - currentVector);
   auto localRotationalVelocity = pose->Rot().Inverse() * *rotationalVelocity;
 
   state(0) = localLinearVelocity.X();

src/systems/hydrodynamics/Hydrodynamics.hh

@@ -66,6 +66,12 @@ namespace systems
   ///     Defaults to 998kgm^-3. [kgm^-3]
   ///   <link_name> - The link of the model that is being subject to
   ///     hydrodynamic forces. [Required]
+  ///   <namespace> - This allows the robot to have an individual namespace for
+  ///     current. This is useful when you have multiple vehicles in different
+  ///     locations and you wish to control the currents of each vehicle
+  ///     separately. If no namespace is given then the plugin listens on
+  ///     the `/ocean_current` topic for a `Vector3d` message. Otherwise it
+  ///     listens on `/model/{namespace name}/ocean_current`.[String, Optional]
   ///
   /// # Example
   /// An example configuration is provided in the examples folder. The example
@@ -86,6 +92,14 @@ namespace systems
   /// -m ignition.msgs.Double -p 'data: -31'
   /// ```
   /// The vehicle should move in a circle.
+  /// ## Ocean Currents
+  /// When underwater, vehicles are often subject to ocean currents. The
+  /// hydrodynamics plugin allows simulation of such currents. We can add
+  /// a current simply by publishing the following:
+  /// ```
+  /// ign topic -t /ocean_current -m ignition.msgs.Vector3d -p 'x: 1, y:0, z:0'
+  /// ```
+  /// You should observe your vehicle slowly drift to the side.
   class Hydrodynamics:
     public ignition::gazebo::System,
     public ignition::gazebo::ISystemConfigure,

tutorials/underwater_vehicles.md

@@ -14,7 +14,7 @@ The behaviour of a moving body through water is different from the behaviour of
 a ground based vehicle. In particular bodies moving underwater experience much
 more forces derived from drag, buoyancy and lift. The way these forces act on
 a body can be seen in the following diagram:
-![force diagram](https://raw.githubusercontent.com/ignitionrobotics/ign-gazebo/main/tutorials/files/underwater/MBARI%20forces.png)
+![force diagram](https://raw.githubusercontent.com/ignitionrobotics/ign-gazebo/ign-gazebo5/tutorials/files/underwater/MBARI%20forces.png)
 
 # Setting up the buoyancy plugin
 The buoyancy plugin in ignition uses the collision mesh to calculate the volume
@@ -177,3 +177,13 @@ ign topic -t /model/tethys/joint/propeller_joint/cmd_pos \
 -m ignition.msgs.Double -p 'data: -31'
 ```
 The vehicle should move in a circle.
+
+# Ocean Currents
+
+When underwater, vehicles are often subject to ocean currents. The hydrodynamics
+plugin allows simulation of such currents. We can add a current simply by
+publishing the following:
+```
+ign topic -t /ocean_current -m ignition.msgs.Vector3d -p 'x: 1, y:0, z:0'
+```
+You should observe your vehicle slowly drift to the side.