From c60cd51337860039d856ff84cdbc9cd7d2a2089a Mon Sep 17 00:00:00 2001
From: acxz <17132214+acxz@users.noreply.github.com>
Date: Wed, 20 Jul 2022 15:05:59 -0400
Subject: [PATCH] use buoyancy plugin for the wamv

remove buoyancy code inside usv dyanmics, model wamv buoyancy as two buoyant cylinders in location of pontoons
---
 .../src/usv_gazebo_dynamics_plugin.cc         | 74 -------------------
 .../urdf/buoyancy/wamv_buoyancy_plugin.xacro  | 52 +++++++++++++
 wamv_gazebo/urdf/macros.xacro                 |  2 +
 wamv_gazebo/urdf/wamv_gazebo.xacro            |  2 +
 4 files changed, 56 insertions(+), 74 deletions(-)
 create mode 100644 wamv_gazebo/urdf/buoyancy/wamv_buoyancy_plugin.xacro

diff --git a/usv_gazebo_plugins/src/usv_gazebo_dynamics_plugin.cc b/usv_gazebo_plugins/src/usv_gazebo_dynamics_plugin.cc
index 05aae52bc..3e2df29d7 100644
--- a/usv_gazebo_plugins/src/usv_gazebo_dynamics_plugin.cc
+++ b/usv_gazebo_plugins/src/usv_gazebo_dynamics_plugin.cc
@@ -170,11 +170,6 @@ void UsvDynamicsPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
     0,                0,                0,   0,   0,   this->paramNdotR;
 }
 
-double UsvDynamicsPlugin::CircleSegment(double R, double h)
-{
-  return R*R*acos( (R-h)/R ) - (R-h)*sqrt(2*R*h-h*h);
-}
-
 //////////////////////////////////////////////////
 void UsvDynamicsPlugin::Update()
 {
@@ -287,75 +282,6 @@ void UsvDynamicsPlugin::Update()
     ignition::math::Vector3d(kForceSum(0), kForceSum(1), kForceSum(2)));
   this->link->AddRelativeTorque(
     ignition::math::Vector3d(kForceSum(3), kForceSum(4), kForceSum(5)));
-
-  // Loop over boat grid points
-  // Grid point location in boat frame - might be able to precalculate these?
-  tf2::Vector3 bpnt(0, 0, 0);
-  // Grid point location in world frame
-  tf2::Vector3 bpntW(0, 0, 0);
-  // For each hull
-  for (int ii = 0; ii < 2; ii++)
-  {
-  // Grid point in boat frame
-  bpnt.setY((ii*2.0-1.0)*this->paramBoatWidth/2.0);
-  // For each length segment
-    for (int jj = 1; jj <= this->paramLengthN; jj++)
-    {
-      bpnt.setX(((jj - 0.5) / (static_cast<float>(this->paramLengthN)) - 0.5) *
-        this->paramBoatLength);
-
-      // Transform from vessel to water/world frame
-      bpntW = xformV * bpnt;
-
-      // Debug
-      ROS_DEBUG_STREAM_THROTTLE(1.0, "[" << ii << "," << jj <<
-          "] grid points" << bpnt.x() << "," << bpnt.y() << "," << bpnt.z());
-      ROS_DEBUG_STREAM_THROTTLE(1.0, "v frame euler " << kEuler);
-      ROS_DEBUG_STREAM_THROTTLE(1.0, "in water frame" << bpntW.x() << "," <<
-          bpntW.y() << "," << bpntW.z());
-
-      // Vertical location of boat grid point in world frame
-      const float kDdz = kPose.Pos().Z() + bpntW.z();
-      ROS_DEBUG_STREAM("Z, pose: " << kPose.Pos().Z() << ", bpnt: "
-        << bpntW.z() << ", dd: " << kDdz);
-
-      // Find vertical displacement of wave field
-      // World location of grid point
-      ignition::math::Vector3d X;
-      X.X() = kPose.Pos().X() + bpntW.x();
-      X.Y() = kPose.Pos().Y() + bpntW.y();
-
-      // Compute the depth at the grid point.
-      double simTime = kTimeNow.Double();
-      // double depth = WavefieldSampler::ComputeDepthDirectly(
-      //  *waveParams, X, simTime);
-      double depth = 0.0;
-      if (waveParams)
-      {
-        depth = WavefieldSampler::ComputeDepthSimply(*waveParams, X, simTime);
-      }
-
-      // Vertical wave displacement.
-      double dz = depth + X.Z();
-
-      // Total z location of boat grid point relative to water surface
-      double  deltaZ = (this->waterLevel + dz) - kDdz;
-      deltaZ = std::max(deltaZ, 0.0);  // enforce only upward buoy force
-      deltaZ = std::min(deltaZ, this->paramHullRadius);
-      // Buoyancy force at grid point
-      const float kBuoyForce = CircleSegment(this->paramHullRadius, deltaZ) *
-        this->paramBoatLength/(static_cast<float>(this->paramLengthN)) *
-        GRAVITY * this->waterDensity;
-      ROS_DEBUG_STREAM("buoyForce: " << kBuoyForce);
-
-      // Apply force at grid point
-      // From web, Appears that position is in the link frame
-      // and force is in world frame
-      this->link->AddForceAtRelativePosition(
-        ignition::math::Vector3d(0, 0, kBuoyForce),
-        ignition::math::Vector3d(bpnt.x(), bpnt.y(), bpnt.z()));
-    }
-  }
 }
 
 GZ_REGISTER_MODEL_PLUGIN(UsvDynamicsPlugin);
diff --git a/wamv_gazebo/urdf/buoyancy/wamv_buoyancy_plugin.xacro b/wamv_gazebo/urdf/buoyancy/wamv_buoyancy_plugin.xacro
new file mode 100644
index 000000000..de48b1ea0
--- /dev/null
+++ b/wamv_gazebo/urdf/buoyancy/wamv_buoyancy_plugin.xacro
@@ -0,0 +1,52 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="usv_buoyancy_gazebo" params="name width:=2.4 length:=4.9">
+    <!--Gazebo Plugin for simulating WAM-V buoyancy-->
+    <gazebo>
+      <plugin name="usv_buoyancy_${name}_hull_left" filename="libbuoyancy_gazebo_plugin.so">
+        <!-- Wave model -->
+        <wave_model>ocean_waves</wave_model>
+        <fluid_density>997.8</fluid_density>
+        <fluid_level>0</fluid_level>
+
+        <linear_drag>0</linear_drag>
+        <angular_drag>0</angular_drag>
+
+        <buoyancy>
+          <link_name>${namespace}/base_link</link_name>
+          <!-- pose info from wamv_base.urdf.xacro/${prefix}_float-->
+          <pose>0 -1.03 0.2 0 1.57 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.213</radius>
+              <length>${length}</length>
+            </cylinder>
+          </geometry>
+        </buoyancy>
+      </plugin>
+
+      <plugin name="usv_buoyancy_${name}_hull_right" filename="libbuoyancy_gazebo_plugin.so">
+        <!-- Wave model -->
+        <wave_model>ocean_waves</wave_model>
+        <fluid_density>997.8</fluid_density>
+        <fluid_level>0</fluid_level>
+
+        <linear_drag>0</linear_drag>
+        <angular_drag>0</angular_drag>
+
+        <buoyancy>
+          <link_name>${namespace}/base_link</link_name>
+          <!-- pose info from wamv_base.urdf.xacro/${prefix}_float-->
+          <pose>0 1.03 0.2 0 1.57 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.213</radius>
+              <length>${length}</length>
+            </cylinder>
+          </geometry>
+        </buoyancy>
+      </plugin>
+
+    </gazebo>
+  </xacro:macro>
+</robot>
diff --git a/wamv_gazebo/urdf/macros.xacro b/wamv_gazebo/urdf/macros.xacro
index 775ac0e72..f6bd98190 100644
--- a/wamv_gazebo/urdf/macros.xacro
+++ b/wamv_gazebo/urdf/macros.xacro
@@ -4,6 +4,8 @@
        name="WAM-V">
   <!-- Include dynamics macros -->
   <xacro:include filename="$(find wamv_gazebo)/urdf/dynamics/wamv_gazebo_dynamics_plugin.xacro" />
+  <!-- Include buoyancy macros -->
+  <xacro:include filename="$(find wamv_gazebo)/urdf/buoyancy/wamv_buoyancy_plugin.xacro" />
 
   <!-- Include sensor macros -->
   <xacro:include filename="$(find wamv_gazebo)/urdf/components/wamv_camera.xacro" />
diff --git a/wamv_gazebo/urdf/wamv_gazebo.xacro b/wamv_gazebo/urdf/wamv_gazebo.xacro
index 496a09ee3..89426a9fc 100644
--- a/wamv_gazebo/urdf/wamv_gazebo.xacro
+++ b/wamv_gazebo/urdf/wamv_gazebo.xacro
@@ -10,5 +10,7 @@
     <xacro:include filename="${thruster_layout}"/>
     <!-- Attach hydrodynamics plugin -->
     <xacro:usv_dynamics_gazebo name="wamv_dynamics_plugin"/>
+    <!-- Attach buoyancy plugin -->
+    <xacro:usv_buoyancy_gazebo name="wamv_buoyancy_plugin"/>
   </xacro:macro>
 </robot>