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,17 +100,36 @@ 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;
+  public: Eigen::MatrixXd Ma;
 
   /// \brief Previous state.
   public: Eigen::VectorXd prevState;
 
-  /// Link entity
-  public: ignition::gazebo::Entity linkEntity;
+  /// \brief Link entity
+  public: Entity linkEntity;
+
+  /// \brief Ocean current callback
+  public: void UpdateCurrent(const msgs::Vector3d &_msg);
+
+  /// \brief Mutex
+  public: std::mutex mtx;
 };
 
+/////////////////////////////////////////////////
+void HydrodynamicsPrivateData::UpdateCurrent(const msgs::Vector3d &_msg)
+{
+  std::lock_guard<std::mutex> lock(this->mtx);
+  this->currentVector = ignition::msgs::Convert(_msg);
+}
+
 /////////////////////////////////////////////////
 void AddAngularVelocityComponent(
   const ignition::gazebo::Entity &_entity,
@@ -180,7 +203,9 @@ void Hydrodynamics::Configure(
   ignition::gazebo::EventManager &/*_eventMgr*/
 )
 {
-  this->dataPtr->waterDensity     = SdfParamDouble(_sdf, "waterDensity", 998);
+  this->dataPtr->waterDensity     = SdfParamDouble(_sdf, "waterDensity",
+                                      SdfParamDouble(_sdf, "water_density", 998)
+                                    );
   this->dataPtr->paramXdotU       = SdfParamDouble(_sdf, "xDotU"       , 5);
   this->dataPtr->paramYdotV       = SdfParamDouble(_sdf, "yDotV"       , 5);
   this->dataPtr->paramZdotW       = SdfParamDouble(_sdf, "zDotW"       , 0.1);
@@ -202,20 +227,40 @@ void Hydrodynamics::Configure(
 
   // Create model object, to access convenient functions
   auto model = ignition::gazebo::Model(_entity);
-  if(!_sdf->HasElement("link_name"))
+
+  std::string ns {""};
+  std::string currentTopic {"/ocean_current"};
+  if (_sdf->HasElement("namespace"))
+  {
+    ns = _sdf->Get<std::string>("namespace");
+    currentTopic = 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"
       << " plugin to act upon";
     return;
   }
   auto linkName = _sdf->Get<std::string>("link_name");
   this->dataPtr->linkEntity = model.LinkByName(_ecm, linkName);
-  if(!_ecm.HasEntity(this->dataPtr->linkEntity))
+  if (!_ecm.HasEntity(this->dataPtr->linkEntity))
   {
     ignerr << "Link name" << linkName << "does not exist";
     return;
   }
 
+  if(_sdf->HasElement("default_current"))
+  {
+    this->dataPtr->currentVector = _sdf->Get<math::Vector3d>("default_current");
+  }
+
   this->dataPtr->prevState = Eigen::VectorXd::Zero(6);
 
   AddWorldPose(this->dataPtr->linkEntity, _ecm);
@@ -266,11 +311,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

@@ -43,29 +43,39 @@ namespace systems
   /// The exact description of these parameters can be found on p. 37 of
   /// Fossen's book. They are used to calculate added mass, linear and quadratic
   /// drag and coriolis force.
-  ///   <xDotU> - Added mass in x direction [kg]
-  ///   <yDotV> - Added mass in y direction [kg]
-  ///   <zDotW> - Added mass in z direction [kg]
-  ///   <kDotP> - Added mass in roll direction [kgm^2]
-  ///   <mDotQ> - Added mass in pitch direction [kgm^2]
-  ///   <nDotR> - Added mass in yaw direction [kgm^2]
-  ///   <xUU>   - Stability derivative, 2nd order, x component [kg/m]
-  ///   <xU>    - Stability derivative, 1st order, x component [kg]
-  ///   <yVV>   - Stability derivative, 2nd order, y component [kg/m]
-  ///   <yV>    - Stability derivative, 1st order, y component [kg]
-  ///   <zWW>   - Stability derivative, 2nd order, z component [kg/m]
-  ///   <zW>    - Stability derivative, 1st order, z component [kg]
-  ///   <kPP>   - Stability derivative, 2nd order, roll component [kg/m^2]
-  ///   <kP>    - Stability derivative, 1st order, roll component [kg/m]
-  ///   <mQQ>   - Stability derivative, 2nd order, pitch component [kg/m^2]
-  ///   <mQ>    - Stability derivative, 1st order, pitch component [kg/m]
-  ///   <nRR>   - Stability derivative, 2nd order, yaw component [kg/m^2]
-  ///   <nR>    - Stability derivative, 1st order, yaw component [kg/m]
+  ///   * <xDotU> - Added mass in x direction [kg]
+  ///   * <yDotV> - Added mass in y direction [kg]
+  ///   * <zDotW> - Added mass in z direction [kg]
+  ///   * <kDotP> - Added mass in roll direction [kgm^2]
+  ///   * <mDotQ> - Added mass in pitch direction [kgm^2]
+  ///   * <nDotR> - Added mass in yaw direction [kgm^2]
+  ///   * <xUU>   - Stability derivative, 2nd order, x component [kg/m]
+  ///   * <xU>    - Stability derivative, 1st order, x component [kg]
+  ///   * <yVV>   - Stability derivative, 2nd order, y component [kg/m]
+  ///   * <yV>    - Stability derivative, 1st order, y component [kg]
+  ///   * <zWW>   - Stability derivative, 2nd order, z component [kg/m]
+  ///   * <zW>    - Stability derivative, 1st order, z component [kg]
+  ///   * <kPP>   - Stability derivative, 2nd order, roll component [kg/m^2]
+  ///   * <kP>    - Stability derivative, 1st order, roll component [kg/m]
+  ///   * <mQQ>   - Stability derivative, 2nd order, pitch component [kg/m^2]
+  ///   * <mQ>    - Stability derivative, 1st order, pitch component [kg/m]
+  ///   * <nRR>   - Stability derivative, 2nd order, yaw component [kg/m^2]
+  ///   * <nR>    - Stability derivative, 1st order, yaw component [kg/m]
   /// Additionally the system also supports the following parameters:
-  ///   <waterDensity> - The density of the fluid its moving in.
+  ///   * <waterDensity> - The density of the fluid its moving in.
+  ///     Defaults to 998kgm^-3. [kgm^-3, deprecated]
+  ///   * <water_density> - The density of the fluid its moving in.
   ///     Defaults to 998kgm^-3. [kgm^-3]
-  ///   <link_name> - The link of the model that is being subject to
+  ///   * <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 set 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]
+  ///   <default_current> - A generic current.
+  ///      [vector3d m/s, optional, default = [0,0,0]m/s]
   ///
   /// # Example
   /// An example configuration is provided in the examples folder. The example
@@ -86,6 +96,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.md.in

@@ -29,6 +29,7 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively.
 * \subpage videorecorder "Video Recorder": Record videos from the 3D render window.
 * \subpage collada_world_exporter "Collada World Exporter": Export an entire
 world to a single Collada mesh.
+* \subpage underwater_vehicles "Underwater Vehicles": Understand how to simulate underwater vehicles.
 
 **Migration from Gazebo classic**
 

tutorials/underwater_vehicles.md

@@ -1,3 +1,4 @@
+\page underwater_vehicles Underwater vehicles
 # Simulating Autnomous Underwater Vehicles
 
 Ignition now supports basic simulation of underwater vehicles.
@@ -14,7 +15,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 +178,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.