From 7d95e152221455e19e81bce1a99bbdb6763ec38b Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Wed, 13 Jul 2022 17:09:28 -0400 Subject: [PATCH 1/4] require usv_gazebo_plugins to use c++17 --- usv_gazebo_plugins/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/usv_gazebo_plugins/CMakeLists.txt b/usv_gazebo_plugins/CMakeLists.txt index 3bed368d1..911f76675 100644 --- a/usv_gazebo_plugins/CMakeLists.txt +++ b/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} From f6cdd51b4defd328dd60a26fc4423fd14357f97e Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Wed, 13 Jul 2022 16:47:01 -0400 Subject: [PATCH 2/4] require wave_gazebo_plugins to use c++17 --- wave_gazebo_plugins/CMakeLists.txt | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/wave_gazebo_plugins/CMakeLists.txt b/wave_gazebo_plugins/CMakeLists.txt index 998674a3e..c9e04e0e1 100644 --- a/wave_gazebo_plugins/CMakeLists.txt +++ b/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 From 820798fff3de0075372d3eb43c9bf0c795702a30 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 3/4] 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(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(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 @@ + + + + + + + + ocean_waves + 997.8 + 0 + + 0 + 0 + + + ${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} + + + + + + + + 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"> + + 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 @@ + + From ef0c888fcd47f0a2ef6cb0f6ed78716d7b5ff8fd Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Sat, 23 Jul 2022 00:57:40 -0400 Subject: [PATCH 4/4] add mesh geometry for buoyancy calculation --- .../usv_gazebo_plugins/polyhedron_volume.hh | 9 +++ .../usv_gazebo_plugins/shape_volume.hh | 25 +++++++- usv_gazebo_plugins/src/polyhedron_volume.cc | 33 ++++++++++ usv_gazebo_plugins/src/shape_volume.cc | 63 ++++++++++++++++++- .../urdf/buoyancy/wamv_buoyancy_plugin.xacro | 33 +--------- 5 files changed, 131 insertions(+), 32 deletions(-) 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} - - - - -