diff --git a/usv_gazebo_plugins/include/usv_gazebo_plugins/polyhedron_volume.hh b/usv_gazebo_plugins/include/usv_gazebo_plugins/polyhedron_volume.hh index bceb7cb9b..dddff57ea 100644 --- a/usv_gazebo_plugins/include/usv_gazebo_plugins/polyhedron_volume.hh +++ b/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 GetVertices(); + /// \brief Computes volume and centroid of tetrahedron /// tetrahedron formed by triangle + arbitrary point /// @param v1: point on triangle diff --git a/usv_gazebo_plugins/include/usv_gazebo_plugins/shape_volume.hh b/usv_gazebo_plugins/include/usv_gazebo_plugins/shape_volume.hh index 7ac9f8a14..d8922a460 100644 --- a/usv_gazebo_plugins/include/usv_gazebo_plugins/shape_volume.hh +++ b/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 { diff --git a/usv_gazebo_plugins/src/polyhedron_volume.cc b/usv_gazebo_plugins/src/polyhedron_volume.cc index a14c4eea0..6705374dc 100644 --- a/usv_gazebo_plugins/src/polyhedron_volume.cc +++ b/usv_gazebo_plugins/src/polyhedron_volume.cc @@ -17,6 +17,8 @@ #include "usv_gazebo_plugins/polyhedron_volume.hh" +#include + 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 Polyhedron::GetVertices() +{ + return vertices; +} diff --git a/usv_gazebo_plugins/src/shape_volume.cc b/usv_gazebo_plugins/src/shape_volume.cc index 635ff6d04..a4c44f71a 100644 --- a/usv_gazebo_plugins/src/shape_volume.cc +++ b/usv_gazebo_plugins/src/shape_volume.cc @@ -90,10 +90,16 @@ ShapeVolumePtr ShapeVolume::makeShape(const sdf::ElementPtr sdf) throw ParseException("cylinder", "missing or element"); } } + else if (sdf->HasElement("mesh")) + { + auto meshElem = sdf->GetElement("mesh"); + auto filename = meshElem->GetAttribute("filename")->GetAsString(); + shape = dynamic_cast(new MeshVolume(filename)); + } else { throw ParseException( - "geometry", "missing , or element"); + "geometry", "missing , , , or element"); } return std::unique_ptr(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); +} diff --git a/wamv_gazebo/urdf/buoyancy/wamv_buoyancy_plugin.xacro b/wamv_gazebo/urdf/buoyancy/wamv_buoyancy_plugin.xacro index de48b1ea0..89d2b1fcd 100644 --- a/wamv_gazebo/urdf/buoyancy/wamv_buoyancy_plugin.xacro +++ b/wamv_gazebo/urdf/buoyancy/wamv_buoyancy_plugin.xacro @@ -3,7 +3,7 @@ - + ocean_waves 997.8 @@ -14,39 +14,12 @@ ${namespace}/base_link - - 0 -1.03 0.2 0 1.57 0 - - 0.213 - ${length} - + + - - - - ocean_waves - 997.8 - 0 - - 0 - 0 - - - ${namespace}/base_link - - 0 1.03 0.2 0 1.57 0 - - - 0.213 - ${length} - - - - -