First Plugin Changing the Buoyancy Model

I read the implementation of the buoyancy model in Gazebo. I understand the authors' motivation for implementing it in a generic way, which includes simulating two environments and allowing motion above what would be considered the surface line. In my case, I want to define any height above a certain Z value as the surface of the water.

To achieve this, I will modify the buoyancy to be equal to the weight of the vehicle when its position is at or above Z. The reason for this modification is that my vehicle is designed to have a ballast system that initially provides positive buoyancy. During the setup, the ballast system transitions to negative buoyancy, allowing the vehicle to perform an initial dive to a launch depth. This approach minimizes the distance required to test control algorithms, thereby optimizing the testing process.

My code modification is below

///////////////////////////////////////////////
// Compute Buoyancy at level Z=0
//////////////////////////////////////////////////
math::Vector3d BuoyancyPrivate::ComputeBuoyancyForce(
  const math::Pose3d &linkWorldPose, const math::Vector3d &gravity, double volume, double mass) const
{
  double fluidDensityAtPose = this->fluidDensity;
  math::Vector3d buoyancyForce;

  if (linkWorldPose.Pos().Z() >= 0)
  {
    // Buoyancy force equals vehicle weight at Z = 0
    buoyancyForce = mass * gravity;
  }
  else
  {
    // Standard buoyancy calculation
    buoyancyForce = -fluidDensityAtPose * volume * gravity;
  }

  return buoyancyForce;
} tag

I tested my plugin by placing the vehicle at Z=1


After Starting the simulation I can see the vehicle dropping but not sinking

The vehicle at this point has neutral buoyancy so stays in the location where it lands.