Add mesh geometry to BuoyancyPlugin

usv_gazebo_plugins/CMakeLists.txt

@@ -19,8 +19,8 @@ catkin_package(
   CATKIN_DEPENDS message_runtime gazebo_dev roscpp wave_gazebo_plugins
 )
 
-# Plugins require c++11
-set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
+# Plugins require c++17
+set(CMAKE_CXX_FLAGS "-std=c++17 ${CMAKE_CXX_FLAGS}")
 
 include_directories( include
   ${catkin_INCLUDE_DIRS}

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);
+}

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);

wamv_gazebo/urdf/buoyancy/wamv_buoyancy_plugin.xacro

@@ -0,0 +1,25 @@
+<?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}" 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>
+          <geometry>
+            <!-- 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>
+    </gazebo>
+  </xacro:macro>
+</robot>

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" />

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>

wave_gazebo_plugins/CMakeLists.txt

@@ -2,21 +2,13 @@ cmake_minimum_required(VERSION 2.8.3)
 project(wave_gazebo_plugins)
 
 ###############################################################################
-# Compile as C++11, supported in ROS Kinetic and newer
 
-#set(CMAKE_CXX_STANDARD 11)
-#set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
-# For this package set as C++14
 include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX14)
-    set(CMAKE_CXX_FLAGS "-std=c++14")
-elseif(COMPILER_SUPPORTS_CXX0X)
-    set(CMAKE_CXX_FLAGS "-std=c++0x")
+CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17)
+if(COMPILER_SUPPORTS_CXX17)
+    add_compile_options(-std=c++17)
 else()
-    message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
+    message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++17 support. Please use a different C++ compiler.")
 endif()
 
 # Set policy for CMake 3.1+. Use OLD policy to let FindBoost.cmake, dependency