Skip to content

Motorboat Simulation

Summary

This simulation node wraps around a Gazebo Harmonic (a general purpose physics simulator) simulation of a motorboat traveling in a free plane of water. The source files are in src/motorboat_simulation/, which includes the motorboat gazebo file (motorboat_model.sdf), the custom plugins, and the autopilot_transform ros package.

How to Run It

To run the simulation alone, run:

cd /home/ws/src/motorboat_sim
gz sim motorboat_model.sdf

To run the ros node, run:

cd /home/ws/src/launch
ros2 launch motorboat_simulation.launch.py

How it Works

The gazebo simulation for the motorboat is contained in motorboat_model.sdf. These sdf files are essentially JSON written differently, for which the full sdf specification can be found here: sdformat spec. The file is a combination of basic data, the shape, mass, initial position, and inertia of all the elements plus the joints that connect them, and plugins, some of which are built-in to gazebo and some of which we wrote ourselves.

The file starts with various scenery/light/camera/physics parameters, most of which are fairly standard. Some notable built-in plugins here are gz-sim-user-commands-system, which allows us to send gazebo commands (which are sent from the ros node) to control the simulation. The gz-sim-navsat-system gives the positional data that the autopilot expects. Below, the model my_ship is the boat, which is where the real meat of the program is. Here is where every piece of the boat is defined, with all the plugins that allow our boat to move!

First, the boat's parts are mostly life-scale, with the measurements being taken from the most recent boats on hand and the masses being estimated. Notably, the boat's hull is split into four pieces because of the buoyancy plugin, which only acts at the center of mass because it's terrible. The inertia tensors are calculated manually, either from a formula or solidworks. A description of the inertia tensors can be found here and in the sdformat specification.

Here are the notable built-in plugins:

  • gz-sim-joint-position-controller-system - This is what controls the rudder
  • gz-sim-odometry-publisher-system - Publishes position and velocity messages
  • gz-sim-thruster-system - This is what controls the thruster
  • gz-sim-hydrodynamics-system - Description below in the How to Develop section

Here are our custom plugins for the motorboat:

  • libRudderDynamics.so - This is our custom LiftDrag plugin. The built-in one is terrible. At some point, this should probably be renamed. In this case, clmax and cdmax refer to the equations of coefficient of lift and coefficient of drag. These coefficients, when plotted vs. angle of attack, form vaguely sinusoidal curves, so find the coefficient of lift vs alpha (aoa) curve for whatever shape/material you're using, then put the max in. For more details, look up more about lift/drag forces.

Additionally, these plugins are used for the sailboat (currently in progress):

  • libFoilDynamics.so - This is essentially the same as libRudderDynamics.so, but it has wind applied to it. At some point, the physics should be altered in this plugin to simulate a real foil.
  • libSailLimits.so - Allows us to control the max sail angle in the simulation.
  • libWindArrow.so - Displays an arrow in the gazebo simulation above the sailboat to show the direction of the wind.

How to Develop

The first thing I'd like to mention is to avoid using AI. I know it's easier, but LLMs are very bad with gazebosim and the ros gazebo bridge since the documentation is terrible and there are so few examples online that use Gazebo Harmonic.

To develop on the ros nodes, you need to understand ros and python, for which there are tutorials elsewhere in this documentation. To develop on the gazebo sim, you need to understand gazebo harmonic and the format of sdf files. The current simulations are good examples, but the documentation can be found here: Gazebo Harmonic Tutorials. The list of built in plugins can be found here: Gazebo 8.10.0 Systems Documentation. Some of the plugins there have terrible documentation, so you may need to look elsewhere for examples. Notably, the hydrodynamics plugin has a great explanation here: Gazebo Hydrodynamics Plugin. If you want to know what the inputs to the hydrodynamics plugin mean, I'd recommend checking out Fossen, Thor I. Guidance and Control of Ocean Vehicles. United Kingdom: Wiley, 1994 as mentioned in the documentation. You can find it in Newman Library or the Ocean Engineering lounge (allegedly).

All gazebo plugins are written in C++, so you will need to be able to at least read C++ to work with them. Additionally, at some point when using the built in plugins, you will need to check the source code. It can be found here.

As to our custom plugins, they are written in C++ and take the following form:

plugin_name
├── CMakeLists.txt
├── include
│   └── PluginName.hh
├── package.xml
└── src
    └── PluginName.cc

To rebuild the plugin, you can simply enter the build folder in /home/ws/build and run make. If you altered the CMakeLists.txt, run cmake .. in the build folder first.

To create a custom plugin, the documenation can be found here: Gazebo plugin documentation. Most plugins you will need to make will run at PreUpdate time.

Let's examine RudderDynamics to get an idea of what is necessary in a plugin:

#include "../include/RudderDynamics.hh"
#include <cmath>

namespace rudder_dynamics
{

/////////////////////////////////////////////////
RudderDynamics::RudderDynamics()
: rho_(1000.1),
  cp_(0, 0, 0),
  forward_(1, 0, 0),
  upward_(0, 0, 1),
  area_(1.0),
  clmax_(1.5),
  cdmax_(1.0)
{
  RCLCPP_INFO(rclcpp::get_logger("RudderDynamics"), "RudderDynamics plugin constructed");
}

/////////////////////////////////////////////////
RudderDynamics::~RudderDynamics() = default;

/////////////////////////////////////////////////
void RudderDynamics::Configure(const gz::sim::Entity &_entity,
                             const std::shared_ptr<const sdf::Element> &_sdf,
                             gz::sim::EntityComponentManager &_ecm,
                             gz::sim::EventManager &)
{
  if (_info.paused)
    return;

Every plugin is not required to have its own namespace, it's just the convention we use. The RCLCPP_INFO(rclcpp::get_logger("RudderDynamics"), is how to send logs to the console. The if (_info.paused) is very important, as your plugin will keep running when the simulation is paused if it's not there, which will probably break it!

// Get world linear velocity
auto optVel = links_[0].WorldLinearVelocity(_ecm);
if (!optVel)
return;
gz::math::Vector3d vel = *optVel;

// Get world pose
auto optPose = links_[0].WorldPose(_ecm);
if (!optPose)
return;
gz::math::Pose3d pose = *optPose;

Gazebo has its own math plugin, which can be found at the Gazebo math github. The documentation can be found here: Gazebo math documentation. A cool steam game called Gazebo Simulator can be found here.

/////////////////////////////////////////////////
GZ_ADD_PLUGIN(rudder_dynamics::RudderDynamics,
              gz::sim::System,
              rudder_dynamics::RudderDynamics::ISystemConfigure,
              rudder_dynamics::RudderDynamics::ISystemPreUpdate)

GZ_ADD_PLUGIN_ALIAS(rudder_dynamics::RudderDynamics, "rudder_dynamics::RudderDynamics")

The specific methods you put into GZ_ADD_PLUGIN depend on which methods you implemented. The .hh files are mostly self explanatory, if you understand C++, you should be alright with those. As for the CMakeLists.txt and package.xml, you'll need some required packages for gazebo, and the package.xml has to be there to run cmake ...

TL;DR: How to create a custom plugin: 1. Create folder with src, and include folders 2. Create header and source file and create CMakeLists.txt and package.xml 3. colcon build --symlink-install in /home/ws 4. Fix all the errors becuse you're bad at programming 5. Add the path to the build folder to GZ_SIM_SYSTEM_PLUGIN_PATH in .bashrc, which means you need to edit postCreateCommand.sh 6. Add the plugin to your sdf!