HOWTO: Creating a Plugin Model
The are two distinct type of models in Gazebo:
- Static models have their code in the main Gazebo distribution, and are statically linked into the server. Generally speaking, such models will be added by the lead developers.
- Plugin models are shared objects that are loaded at runtime (like loadable modules in the Linux kernel). They are the recommended method for all new, experimental or third party models.
Plugin models have serveral advantanges over their static counterparts:
- They are easier to build (no mucking about with autotools, or digging about in the server internals).
- The allow for quicker debugging through a much faster code/compile/test cycle. Unlike static models, one does not need to rebuild the Gazebo server, only the model itself.
- Code can be maintained in a separate source repository. This is particularly useful for third-party developers, and those users with funky one-off models that belong with the rest of their code, data, papers, etc.
Developers are advised to read Coding Standards and Conventions before creating new models.
Plugin Model Example
Sample code for a very basic plugin model in provided in theexamples
directory; for a default install, this will be:
/usr/local/src/gazebo/examples/plugins/ExampleModel/
Copy the files from this directory and rename Makefile.example
to Makefile
. Try building the example:
$ make
This produces a plugin model names ExampleModel.so
. You can test the model using the included world file:
$ gazebo ./example.world
The world file model
tag has an additional attribute specifying the path to the plugin, i.e.,:
<model:ExampleModel> <plugin>ExampleModel.so</plugin> ... </model:ExampleModel>
Note that the same plugin can be specified by more than one model.
Note that the server will search for plugin models according to the following algorithm. Given a request to load the plugin <foo>:
- If <foo> is an absolute path, then try to load it, and exit with an error if that fails. If <foo> is a relative path, go to (2).
- If the environment variable GAZEBOPATH is set, it is interpreted as a colon-separated list of directores; try to load <foo> from each directory in turn. If <foo> is found, great. If not (or if GAZEBOPATH is not set), go to (3).
- Try to load <foo> by interpreting it as relative to the directory in which the config file resides. If that doesn't work, go to (4).
- Try to load <foo> from <prefix>/lib/gazebo/plugins, where <prefix> is the installation prefix determined at configure time. The default prefix is /usr/local.
Writing a simple driver
To make a model, create a new class that inherits the Gazebo Model class. This base class defines the standard API that all models must support. The basic elements of the new model class are as follows.
- Load(...)
- One or more bodies: every model must have at least one instance of class Body to manage physical interactions (position, velocity, forces and torques).
- One or more geoms: geoms are basic geometric objects that have both physical and visual properties (mass, size, shape, color, texture, etc). Geoms come in many flavours, but all are derived from the Geom class. Geoms are attached to bodies.
- One or more joints: if the model has more than one body, these bodies are attached to each other via joints. Thus, for example, a simple robot may consist of five bodies connected by four joints:
- One body for the robot chassis.
- Four bodies representing the robot's four wheels.
- Four joints attaching the wheels to the chassis. Joints come in several varieties, representing axles, linear actuators, etc., but most joints have
motors
that can be used to articulate the model. The various flavours of joints are derived all from the Joint class.
The Load() method has two arguments:
- file : a pointer to the worldfile object.
- node : a pointer to the worldfile node for this model.
The node
argument is used to access the worldfile settings for this particular model; one can read user-defined model dimensions, motor settings, update rates and so on using the methods provided in the WorldFileNode class.
- Init(...)
this->position = gz_position_alloc(); if (gz_position_create(this->position, this->world->gz_server, this->GetId(), "ExampleModel", (int) this, (int) this->parent) != 0) return -1;
Models may support more than one interface at the same time.
- Fini(...)
gz_position_destroy( this->position ); gz_position_free( this->position ); this->position = NULL;
- Update(...)
if (this->world->GetSimTime() - this->updateTime > this->updatePeriod) { this->updateTime = this->world->GetSimTime(); // Get commands from the external interface this->GetPositionCmd(); // Set joint motor speeds and/or torques ... // Update the interface this->PutPositionData(); }
Thus, for example, if the device being simulated has a communication frequency of 10Hz, we should only check for new commands (and update outgoing data) once every 100ms. Failure to limit the update rate in this manner will result in a very slow simulation, and/or a poor fidelity simulation. Note also the use of the World::GetSimTime() function; this is the elapsed simulation time, which may be very different from the elapsed real time. Always use the simulation time when updating models.
- Other Examples
- Pioneer2AT : this model simulates an ActivMedia P2AT robot with skid-steering, odometry and sonar sensors.
- SonyVID30 : this model simulates a Sony pan-tilt-zoom unit; the model permits control of the pan, tilt, and zoom values, and provides image data from the camera sensor.
Gazebo also makes heavy use of both OpenGL and the ODE (Open Dynamics Engine); the documentation for these projects provides valuable insight into the many important concepts:
- OpenGL Programming Guide and Reference Manual.
- Open Dynamics Engine Documentation.
Registration and instantiation
In order to use a plugin model, the Gazebo server needs to know two things:
- The model name (as it will appear in the world file).
- The model factory function (used to create an instance of the model class).
The ModelFactory.hh header provides a macro that will automatically handle model registration and instantiation. Use this macro at the top of the model file:
GZ_REGISTER_PLUGIN("ExampleModel", ExampleModel)
The first argument gives the model name as it appears in the world file; the second argument gives the class name.
Building the shared library
The example model includes a Makefile for building shared objects. To manually build a shared object, try:
$ g++ -Wall -g3 -c ExampleModel.cc $ g++ -shared -nostartfiles -o ExampleModel.so ExampleModel.o
While the above method will probably work, it is recommended that you use pkg-config to get the compile flags; i.e.,
$ g++ -Wall -g3 `pkg-config --cflags gazebo` -c ExampleModel.cc $ g++ -shared -nostartfiles -o ExampleModel.so ExampleModel.o
- Todo:
- Add example of building plugin using libtool.