add mesh geometry for buoyancy calculation

usv_gazebo_plugins/include/usv_gazebo_plugins/polyhedron_volume.hh

@@ -86,6 +86,11 @@ namespace buoyancy
                                            double l,
                                            int n);
 
+    /// \brief Generate a mesh polyhedron centered at origin
+    /// @param filename: filename of mesh
+    /// @return Polyhedron object
+    public: static Polyhedron makeMesh(std::string filename);
+
     /// \brief Compute full volume and center of buoyancy of the polyhedron
     /// @return Volume object with volume and centroid
     public: Volume ComputeFullVolume();
@@ -99,6 +104,10 @@ namespace buoyancy
                                    const ignition::math::Quaterniond &q,
                                    Plane &plane);
 
+    /// \brief Get Object vertices
+    /// @return Vertices of the polyhedron
+    public: std::vector<ignition::math::Vector3d> GetVertices();
+
     /// \brief Computes volume and centroid of tetrahedron
     /// tetrahedron formed by triangle + arbitrary point
     /// @param v1: point on triangle

usv_gazebo_plugins/include/usv_gazebo_plugins/shape_volume.hh

@@ -33,7 +33,8 @@ namespace buoyancy
     None,
     Box,
     Sphere,
-    Cylinder
+    Cylinder,
+    Mesh
   };
 
   /// \brief Parent shape object for volume objects
@@ -143,6 +144,28 @@ namespace buoyancy
     double r;
   };
 
+  /// \brief Mesh shape volume
+  struct MeshVolume : public ShapeVolume
+  {
+    /// \brief Default constructor
+    /// @param filename: filename of the mesh
+    explicit MeshVolume(std::string filename);
+
+    /// \brief Display string for mesh shape
+    std::string Display() override;
+
+    // Documentation inherited.
+    Volume CalculateVolume(const ignition::math::Pose3d& pose,
+                           double fluidLevel) override;
+
+    /// \brief Filename of mesh
+    std::string filename;
+
+    private:
+    /// \brief Polyhedron defining a mesh
+    Polyhedron polyhedron;
+  };
+
   /// \brief Custom exception for parsing errors
   struct ParseException : public std::exception
   {

usv_gazebo_plugins/src/polyhedron_volume.cc

@@ -17,6 +17,8 @@
 
 #include "usv_gazebo_plugins/polyhedron_volume.hh"
 
+#include <gazebo/common/MeshManager.hh>
+
 using namespace buoyancy;
 using Face = Polyhedron::Face;
 
@@ -124,6 +126,31 @@ Polyhedron Polyhedron::makeCylinder(double r, double l, int n)
   return cylinder;
 }
 
+Polyhedron Polyhedron::makeMesh(std::string filename)
+{
+    buoyancy::Polyhedron mesh;
+
+    // retrieve the subMesh from the mesh filename
+    auto subMesh =
+        gazebo::common::MeshManager::Instance()->Load(filename)->GetSubMesh(0);
+
+    // copy the submesh's vertices to the polyhedron mesh's "vertices" array
+    for (unsigned int v = 0; v < subMesh->GetVertexCount(); ++v)
+    {
+      mesh.vertices.emplace_back(subMesh->Vertex(v));
+    }
+
+    // copy the submesh's indices to the polyhedron mesh's "faces" array
+    for (unsigned int i = 0; i < subMesh->GetIndexCount(); i+=3)
+    {
+      mesh.faces.emplace_back(Face(subMesh->GetIndex(i),
+                                   subMesh->GetIndex(i+1),
+                                   subMesh->GetIndex(i+2)));
+    }
+
+    return mesh;
+}
+
 //////////////////////////////////////////////////////
 Volume Polyhedron::tetrahedronVolume(const ignition::math::Vector3d &v1,
     const ignition::math::Vector3d &v2, const ignition::math::Vector3d &v3,
@@ -287,3 +314,9 @@ Volume Polyhedron::SubmergedVolume(const ignition::math::Vector3d &x,
       0 : output.centroid.Z();
   return output;
 }
+
+//////////////////////////////////////////////////////
+std::vector<ignition::math::Vector3d> Polyhedron::GetVertices()
+{
+    return vertices;
+}

usv_gazebo_plugins/src/shape_volume.cc

@@ -90,10 +90,16 @@ ShapeVolumePtr ShapeVolume::makeShape(const sdf::ElementPtr sdf)
       throw ParseException("cylinder", "missing <radius> or <length> element");
     }
   }
+  else if (sdf->HasElement("mesh"))
+  {
+      auto meshElem = sdf->GetElement("mesh");
+      auto filename = meshElem->GetAttribute("filename")->GetAsString();
+      shape = dynamic_cast<ShapeVolume*>(new MeshVolume(filename));
+  }
   else
   {
     throw ParseException(
-        "geometry", "missing <box>, <cylinder> or <sphere> element");
+        "geometry", "missing <box>, <cylinder>, <sphere>, or <mesh> element");
   }
 
   return std::unique_ptr<ShapeVolume>(shape);
@@ -112,6 +118,8 @@ std::string ShapeVolume::Display()
       return "Cylinder";
     case ShapeType::Sphere:
       return "Sphere";
+    case ShapeType::Mesh:
+      return "Mesh";
   }
   return "None";
 }
@@ -227,3 +235,56 @@ Volume SphereVolume::CalculateVolume(const ignition::math::Pose3d &pose,
   }
   return output;
 }
+
+/////////////////////////////////////////////////
+MeshVolume::MeshVolume(std::string filename)
+    : polyhedron(Polyhedron::makeMesh(filename))
+{
+  type = ShapeType::Mesh;
+  volume = polyhedron.ComputeFullVolume().volume;
+
+  ignition::math::Vector3d min;
+  ignition::math::Vector3d max;
+
+  min.X(FLT_MAX);
+  min.Y(FLT_MAX);
+  min.Z(FLT_MAX);
+
+  max.X(-FLT_MAX);
+  max.Y(-FLT_MAX);
+  max.Z(-FLT_MAX);
+
+  for (const auto& vertex : polyhedron.GetVertices())
+  {
+    min.X(std::min(min.X(), vertex.X()));
+    min.Y(std::min(min.Y(), vertex.Y()));
+    min.Z(std::min(min.Z(), vertex.Z()));
+
+    max.X(std::max(max.X(), vertex.X()));
+    max.Y(std::max(max.Y(), vertex.Y()));
+    max.Z(std::max(max.Z(), vertex.Z()));
+  }
+
+  auto x = max.X() - min.X();
+  auto y = max.Y() - min.Y();
+  auto z = max.Z() - min.Z();
+
+  averageLength = (x + y + z) / 3;
+}
+
+/////////////////////////////////////////////////
+std::string MeshVolume::Display()
+{
+  std::stringstream ss;
+  ss << ShapeVolume::Display() << ":" << filename;
+  return ss.str();
+}
+
+/////////////////////////////////////////////////
+Volume MeshVolume::CalculateVolume(const ignition::math::Pose3d &pose,
+                                   double fluidLevel)
+{
+  Plane waterSurface;
+  waterSurface.offset = fluidLevel;
+  return this->polyhedron.SubmergedVolume(pose.Pos(), pose.Rot(), waterSurface);
+}

wamv_gazebo/urdf/buoyancy/wamv_buoyancy_plugin.xacro

@@ -3,7 +3,7 @@
   <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">
+      <plugin name="usv_buoyancy_${name}" filename="libbuoyancy_gazebo_plugin.so">
         <!-- Wave model -->
         <wave_model>ocean_waves</wave_model>
         <fluid_density>997.8</fluid_density>
@@ -14,39 +14,12 @@
 
         <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>
+            <!-- mesh from wamv_base.urdf.xacro/${namespace}/dummy_link -->
+            <mesh filename="package://wamv_description/models/WAM-V-Base/mesh/WAM-V-Base.dae"/>
           </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>