Wave Body MBARI: add MBARI buoy example for testing (#76)

- Add test model for the MBARI WEC buoy (float only)

Signed-off-by: Rhys Mainwaring <[email protected]>

gz-waves-models/models/mbari_buoy/model.config

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<model>
+  <name>MBARI Buoy</name>
+  <version>1.0</version>
+  <sdf version="1.8">model.sdf</sdf>
+
+  <author>
+    <name>Michael Anderson</name>
+    <email>[email protected]</email>
+  </author>
+  <author>
+    <name>Louise Poubel</name>
+    <email>[email protected]</email>
+  </author>
+  <author>
+    <name>Dharini Dutia</name>
+    <email>[email protected]</email>
+  </author>
+
+  <author>
+    <name>Rhys Mainwaring</name>
+    <email>[email protected]</email>
+  </author>
+
+  <description>
+    Base MBARI-WEC (Wave Energy Converter) model for ocean sensing
+    and harvesting wave energy.
+
+    This is a version of the MBARI-WEC model from https://github.com/osrf/buoy_sim.
+    It has been simplified for testing the linear-wave-body model.
+  </description>
+
+</model>

gz-waves-models/models/mbari_buoy/model.sdf

@@ -0,0 +1,71 @@
+<?xml version="1.0" ?>
+<sdf version="1.8">
+  <model name="mbari_buoy">
+    <!--
+      In the orginal model the link origin is located
+      at the bridle articulation point.
+
+      CoM location:       (0 0 2.03) m
+      Waterline location: (0 0 2.27) m
+      CoB:                (0 0 2.05) m
+
+      Vo:                 2.39 m^3
+      S:                  5.47 m^2
+      S11 = S22:          1.37 m^4
+
+      Source: Buoy_Assembly_09-27-2022
+
+      We set the origin at the waterplane, and offset from there.
+    
+      Questions:
+        The inertial data in the data sheet looks like it may be for a
+        coordinate system with y-up as the smallest diagonal element is Iyy
+        and we'd expect the inertia to be approx. symmetric about the z axis.
+    -->
+    <link name="base_link">
+      <pose>0 0 0 0 0 0</pose>
+      <inertial>
+        <pose>0 0 -0.24 0 0 0</pose>
+        <!-- <mass>1400</mass> -->
+        <mass>2450</mass>
+        <!-- <inertia>
+          <ixx>1429</ixx>
+          <ixy>6.74</ixy>
+          <ixz>4.70</ixz>
+          <iyy>670.3</iyy>
+          <iyz>30.5</iyz>
+          <izz>1476</izz>
+        </inertia> -->
+        <inertia>
+          <ixx>1429</ixx>
+          <ixy>4.70</ixy>
+          <ixz>6.74</ixz>
+          <iyy>1476</iyy>
+          <iyz>30.5</iyz>
+          <izz>670.3</izz>
+        </inertia>
+      </inertial>
+      <visual name="visual_Buoy">
+        <pose>0 0 -2.27 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>model://mbari_buoy/meshes/buoy_float.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 0.0 1</ambient>
+          <diffuse>1.0 1.0 0.0 1</diffuse>
+          <specular>1.0 1.0 0.0 1</specular>
+        </material>
+      </visual>
+      <collision name="collision">
+        <pose>0 0 0.19 0 0 0 </pose>
+        <geometry>
+          <box>
+            <size>2.34 2.34 1</size>
+          </box>
+        </geometry>
+      </collision>
+    </link>
+  </model>
+</sdf>

gz-waves-models/worlds/mbari_buoy.sdf

@@ -0,0 +1,186 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="waves">
+    <physics name="1ms" type="ignore">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+    </physics>
+
+    <plugin filename="gz-sim-physics-system"
+            name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin filename="gz-sim-sensors-system"
+            name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+      <background_color>0.8 0.8 0.8</background_color>
+    </plugin>
+    <plugin filename="gz-sim-scene-broadcaster-system"
+            name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <plugin filename="gz-sim-user-commands-system"
+            name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin filename="gz-sim-imu-system"
+            name="gz::sim::systems::Imu">
+    </plugin>
+
+    <!-- switch off gravity in physics engine when using added mass -->
+    <gravity>0 0 0</gravity>
+
+    <scene>
+      <ambient>1.0 1.0 1.0</ambient>
+      <background>0.8 0.8 0.8</background>
+      <sky></sky>
+    </scene>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.6 0.6 0.6 1</specular>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <include>
+      <uri>model://regular_waves_6s_2m</uri>
+    </include>
+
+    <include>
+      <pose>0 0 0 0 0 0</pose>
+      <uri>model://mbari_buoy</uri>
+      <name>mbari_buoy</name>
+      <plugin
+          filename="gz-waves1-linear-wave-body-system"
+          name="gz::sim::systems::LinearWaveBody">
+
+        <!-- BEM HDF5 data file -->
+        <hdf5_file>model://mbari_buoy/config/hydrodata/mbari_buoy.h5</hdf5_file>
+
+        <!-- waves parameter overrides -->
+        <waves>
+          <regular>
+            <period>6</period>
+            <height>2</height>
+            <direction>0</direction>
+            <phase>0</phase>
+          </regular>
+        </waves>
+
+        <!-- simulation parameter overrides -->
+        <simulation_parameters>
+          <gravity>9.81</gravity>
+          <fluid_density>1025</fluid_density>
+        </simulation_parameters>
+
+        <!-- geometry overrides -->
+        <geometry>
+          <center_of_waterplane>0 0 0</center_of_waterplane>
+          <center_of_buoyancy>0 0 -0.22</center_of_buoyancy>
+          <displaced_volume>2.39</displaced_volume>
+        </geometry>
+
+        <!-- 
+          Dimensioned hydrodynamics coefficients for T=6.0s (w = 1.0472 rad/s)
+        -->
+        <!-- <hydro_coeffs>
+          <scaled>0</scaled>
+          <hydrostatic>
+            <linear_restoring>
+              0.      0.      0.      0.      0.      0.
+              0.      0.      0.      0.      0.      0.
+              0.      0.  34690.6     0.      0.      0.
+              0.      0.      0.  14256.3     0.      0.
+              0.      0.      0.      0.  14256.3     0.
+              0.      0.      0.      0.      0.      0.
+            </linear_restoring>
+          </hydrostatic>
+        </hydro_coeffs> -->
+
+        <!-- enable / disable forces -->
+        <forces>
+          <gravity_on>1</gravity_on>
+          <buoyancy_on>1</buoyancy_on>
+          <hydrostatic_restoring_on>1</hydrostatic_restoring_on>
+          <radiation_damping_on>1</radiation_damping_on>
+          <radiation_added_mass_on>1</radiation_added_mass_on>
+          <excitation_on>1</excitation_on>
+          <excitation_froude_krylov_on>0</excitation_froude_krylov_on>
+          <excitation_scattering_on>0</excitation_scattering_on>
+        </forces>
+
+        <!-- enable / disable debug messages -->
+        <debug>
+          <gravity_on>0</gravity_on>
+          <buoyancy_on>0</buoyancy_on>
+          <radiation_damping_on>0</radiation_damping_on>
+          <radiation_added_mass_on>0</radiation_added_mass_on>
+          <excitation_on>0</excitation_on>
+        </debug>
+
+        <!-- publish forces -->
+        <publishers>
+          <update_rate>20</update_rate>
+
+          <gravity_on>1</gravity_on>
+          <buoyancy_on>1</buoyancy_on>
+          <hydrostatic_restoring_on>1</hydrostatic_restoring_on>
+          <radiation_damping_on>1</radiation_damping_on>
+          <radiation_added_mass_on>1</radiation_added_mass_on>
+          <excitation_on>1</excitation_on>
+          <excitation_froude_krylov_on>1</excitation_froude_krylov_on>
+          <excitation_scattering_on>1</excitation_scattering_on>
+
+          <gravity_topic>/force/gravity</gravity_topic>
+          <buoyancy_topic>/force/buoyancy</buoyancy_topic>
+          <hydrostatic_restoring_topic>/force/restoring</hydrostatic_restoring_topic>
+          <radiation_damping_topic>/force/radiation_damping</radiation_damping_topic>
+          <radiation_added_mass_topic>/force/radiation_added_mass</radiation_added_mass_topic>
+          <excitation_topic>/force/excitation</excitation_topic>
+          <excitation_froude_krylov_topic>/force/excitation_froude_krylov</excitation_froude_krylov_topic>
+          <excitation_scattering_topic>/force/excitation_scattering</excitation_scattering_topic>
+        </publishers>
+      </plugin>
+
+      <!--
+        To use this plugin for comparison, uncomment and disable all
+        forces except gravity in gz-waves1-linear-wave-body-system.
+      -->
+      <!-- <plugin
+          filename="gz-waves1-hydrodynamics-system"
+          name="gz::sim::systems::Hydrodynamics">
+        <enable>mbari_buoy::base_link::collision</enable>
+        <hydrodynamics>
+          <damping_on>1</damping_on>
+          <viscous_drag_on>1</viscous_drag_on>
+          <pressure_drag_on>1</pressure_drag_on>
+        </hydrodynamics>
+      </plugin> -->
+
+      <plugin
+        filename="gz-sim-joint-state-publisher-system"
+        name="gz::sim::systems::JointStatePublisher">
+      </plugin>
+
+      <plugin
+        filename="gz-sim-odometry-publisher-system"
+        name="gz::sim::systems::OdometryPublisher">
+        <odom_frame>odom</odom_frame>
+        <robot_base_frame>base_link</robot_base_frame>
+        <odom_publish_frequency>50</odom_publish_frequency>
+        <odom_topic>model/mbari_buoy/odometry</odom_topic>
+        <dimensions>3</dimensions>
+      </plugin>
+
+      <plugin
+        filename="gz-sim-pose-publisher-system"
+        name="gz::sim::systems::PosePublisher">
+        <publish_link_pose>true</publish_link_pose>
+        <use_pose_vector_msg>true</use_pose_vector_msg>
+        <static_publisher>true</static_publisher>
+        <static_update_frequency>1</static_update_frequency>
+      </plugin>
+
+    </include>
+
+  </world>
+</sdf>

gz_waves_bridge/launch/mbari_buoy.launch.py

@@ -0,0 +1,75 @@
+"""Launch Gazebo world with an ellipsoid buoy."""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    model_name = "mbari_buoy"
+
+    # Bridge to forward tf and joint states to ros2
+    # LaunchConfiguration("world_name"),
+    bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=[
+            "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock",
+            "/world/mbari_buoy/model/mbari_buoy/joint_state@sensor_msgs/msg/JointState[gz.msgs.Model",
+            "/model/mbari_buoy/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry",
+            "/model/mbari_buoy/pose@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V",
+            "/model/mbari_buoy/pose_static@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V",
+            "/force/gravity@geometry_msgs/msg/Wrench[gz.msgs.Wrench",
+            "/force/buoyancy@geometry_msgs/msg/Wrench[gz.msgs.Wrench",
+            "/force/restoring@geometry_msgs/msg/Wrench[gz.msgs.Wrench",
+            "/force/radiation_damping@geometry_msgs/msg/Wrench[gz.msgs.Wrench",
+            "/force/radiation_added_mass@geometry_msgs/msg/Wrench[gz.msgs.Wrench",
+            "/force/excitation@geometry_msgs/msg/Wrench[gz.msgs.Wrench",
+            "/force/excitation_froude_krylov@geometry_msgs/msg/Wrench[gz.msgs.Wrench",
+            "/force/excitation_scattering@geometry_msgs/msg/Wrench[gz.msgs.Wrench",
+        ],
+        remappings=[
+            (
+                "/world/mbari_buoy/model/mbari_buoy/joint_state",
+                "joint_states",
+            ),
+            ("/model/mbari_buoy/pose", "/tf"),
+            ("/model/mbari_buoy/pose_static", "/tf_static"),
+        ],
+        parameters=[
+            {"qos_overrides./tf_static.publisher.durability": "transient_local"}
+        ],
+        output="screen",
+    )
+
+    # body_response_publisher republishes the buoy odometry to maritime dof
+    # topics: surge, sway, heave, roll, pitch, yaw
+    # mainly for plotting - where plotting tools do not have built-in
+    # converters from quaternions to euler angles.
+    #
+    body_response_publisher = Node(
+        package="gz_waves_bridge",
+        executable="body_response_publisher",
+        arguments=[],
+        remappings=[
+            ("/odom", "/model/mbari_buoy/odometry"),
+            ("/surge", "/model/" + model_name + "/surge"),
+            ("/sway", "/model/" + model_name + "/sway"),
+            ("/heave", "/model/" + model_name + "/heave"),
+            ("/roll", "/model/" + model_name + "/roll"),
+            ("/pitch", "/model/" + model_name + "/pitch"),
+            ("/yaw", "/model/" + model_name + "/yaw"),
+        ],
+        parameters=[],
+        output="screen",
+    )
+
+    return LaunchDescription(
+        [
+            bridge,
+            body_response_publisher,
+        ]
+    )