Case study: migrating the ArduPilot ModelPlugin from Gazebo classic to Ignition Gazebo
A variety of changes are required when migrating a plugin from Gazebo classic to Ignition Gazebo. In this tutorial we offer as a case study the migration of one particular
ModelPlugin, ardupilot_gazebo. We hope that this example provides useful tips to others who are migrating their existing plugins from classic to Ignition.
The complete, migrated version of the
ardupilot_gazebo plugin covered in this tutorial can be found in this fork.
ardupilot_gazebo plugin is used with Gazebo to assist with simulating unmanned aerial vehicles (UAVs, aka drones). For more information on how to use it, check the ArduPilot documentation.
As context to understand what we're migrating, here's a system diagram for how the ArduPilot Gazebo plugin works is used:
UAV icon credit: By Julian Herzog, CC BY 4.0, https://commons.wikimedia.org/w/index.php?curid=60965475
For each UAV model in simulation, there is one instance of ArduPilotPlugin loaded into the simulation process. That plugin uses internal simulation APIs to retrieve the UAV's current state, which it sends to an external ArduPilot process via a custom UDP protocol (it's called Flight Dynamics Model, or FDM). The ArduPilot process in turn makes the vehicle state available via the MAVLink protocol to other processes, such as QGroundControl (QGC). The user can issue commands in QGC like "take off" or "goto waypoint", which are sent via MAVLink to ArduPilot, which computes motor commands and sends them to the plugin, which passes them onto the vehicle via internal simulation APIs.
To be clear, this structure is pre-existing and widely used in UAV simulation. Our contribution in this tutorial is port the plugin from Gazebo to Ignition, preserving the rest of the setup.
Structure of the migration
Migration of this plugin involves modifications to multiple parts of the associated code:
- The plugin header file,
- The plugin source file,
- The plugin's CMake build recipe,
- The custom model in which the plugin is used
We'll take them each in turn in the following sections.
Plugin header file (ArduPilotPlugin.hh)
The old code includes these Gazebo classic headers:
In the new code, we still need
<sdf/sdf.hh>, because the underlying SDFormat library is used by both classic and Ignition. But in place of the
<gazebo/...> headers, we'll pull in one from Ignition:
In the old code, the plugin class
ArduPilotPlugin is declared in the
In the new code we declare the class in the
In the old code, the plugin class inherits from
In the new code, we use multiple inheritance to declare that our plugin will act as a system (in the entity-component-system, or ECS, pattern used by Ignition), and further which interfaces of a system it will use (we also update the symbol visibility macro):
With this declaration we're indicating that our plugin will supply implementation of the
In the old code, the
Load() method is called once for each instance of the plugin that is loaded, allowing for startup configuration, like pulling parameters out of the plugin's SDF configuration:
In the new code, we use
Configure() for the same purpose (if a different signature):
Similarly, the old code provides
OnUpdate(), which is called once per time step while simulation is running:
In the new code, this method is replaced by two methods,
As the names suggest, the former is called before each time step, while the latter is called after each time step. Note the subtle difference in signature:
PreUpdate() takes a non-
const reference to the
PostUpdate() takes a
const reference to it. We'll make any changes to the state of simulation (e.g., setting torques on joints) in
PreUpdate() and we'll read out results from simulation (e.g., getting the pose of a link) in
The remaining changes in the header are just bookkeeping, to allow us to have access to the right objects with the right types in other class methods. These three helpers:
Plugin source file (ArduPilotPlugin.cc)
The old code includes these Gazebo-related headers:
Like we did in
ArduPilotPlugin.hh, we'll keep
<sdf/sdf.hh>. The others are replaced with Ignition equivalents, and where possible we narrow the inclusion to exactly what we need. We start by enumerating those components (part of the ECS pattern used by Ignition) that we're using:
To better understand the ECS pattern as it is used in Ignition, it's helpful to learn about the EntityComponentManager (ECM), which is responsible for managing the ECS graph. A great resource to understand the logic under the hood of the ECM is the
SdfEntityCreator class (header, source). This class is responsible for mapping the content of an SDF file to the entities and components that form the graph handled by the ECM. For example, if you wonder which components can be accessed by default from the plugin, this class is the best entry point.
Next we include the parts of
ign-gazebo itself that we're using:
We need a few things from
Because we'll be subscribing to data published by a sensor, we need a header from
And we keep the SDFormat header:
Now let's get into the class member declarations. The
PID class has moved from
In the old code we store a
physics::JointPtr for each propeller joint we're controlling:
In the new code we store an
In the old code we store an
event::ConnectionPtr to manage periodic calls to the
There's no equivalent class member in the new code. Instead we declared our intent to have this class's update methods called via its inheritance.
In the old code we store a
physics::ModelPtr for the model we're acting on:
In the new code we instead store references to the model, the entity underlying the model, and the entity underyling one of the links in the model:
The old code uses a custom time class:
while the new code uses
In this plugin we need to read data from an IMU sensor attached to the UAV. In the old code we store a pointer to the sensor:
In the new code, instead of accessing the sensor object directly we must subscribe to a topic published by the sensor (you might be tempted to try retrieving the sensor data via components attached to the IMU entity, but that won't work because the logic to produce the data lives in the IMU system and its output can only be consumed via subscription). So we need a few more variables to track the state of subscription, data receipt via subscription, and so on:
We also need a callback function that will be invoked upon receipt of newly published data from the IMU sensor. The callback just latches the latest message in a mutex-controlled fashion:
Throughout the code, we replace the following output streams from the old code:
with their Ignition equivalents:
Plugin interface: Configure()
In the old code, we store the model pointer and name:
In the new code, we store the entity, model, and name a bit differently:
Also in the new code we need to make sure of the existence of the specific components that we need. In our case, we're going to access the
WorldLinearVelocity components of the entity representing one of the UAV model's links. The data in those components will be periodically updated by the physics system. But the physics system will not necessarily create the components, so before accessing them later in our code, we need to ensure that the components exist:
We'll see this pattern elsewhere in the new code: check for a component's existence, create it if necessary, then proceed with using it.
We also clone the
const sdf::Element that we've passed so that we can call non-
const methods on it:
In the old code we retrieve a pointer to each joint that we're controlling:
In the new code we retrieve the entity that represents the joint:
The accessor methods for members in the
PID class have changed. The old code uses a
Get prefix, e.g.:
In the new code, the
Get prefix is gone:
The old code does a bunch of lookups to get a pointer to the IMU sensor. In the new code, we just store the name of the sensors from the user-supplied SDF configuration:
and we do the equivalent lookup later, in
PreUpdate(), which we'll cover next.
Plugin interface: OnUpdate() -> PreUpdate() + PostUpdate()
The old code does the following each time step in its
As mentioned above, in the new code we're splitting that work into two halves: the "write" part should happen in
PreUpdate() and the "read" part should happen in
PreUpdate() we receive new commands from the external ArduPilot process and write the resulting forces to propeller joints in simulation:
PostUpdate() we read the latest state (e.g., IMU sensor data, UAV pose and velocity) from simulation and send it out to ArduPilot:
Note the differences in both methods with regard to time-handling: (i) the current simulation time is passed in as part of an
ignition::gazebo::UpdateInfo object; and (ii) we operate on time values using
One-time initialization in PreUpdate(): subscribing to sensor data
Though it's not part of the regular update loop, we subscribe to the IMU sensor data in
PreUpdate() because the information that we need for that subscription isn't available when we're in
That one-time subscription logic looks like this, starting with determination of the right topic name and ending with registering our previously defined
imuCb() method as the callback to receive new IMU data:
Writing to simulation
Based on commands received from ArduPilot, new forces are applied to the propeller joints in
ApplyMotorForces(), using the joints' current velocities as feedback. In the old code that's done by calling
SetForce() on each joint
In the new code, for each joint
i we read from the
JointVelocity component attached to the corresponding entity, and we write to the
JointForceCmd component attached to the same entity (creating it first in case it doesn't yet exist):
A similar pattern is used for the case of setting a velocity on a joint; instead of calling
SetVelocity() on the joint, we write to the
JointVelocityCmd component of the joint entity.
Reading from simulation
To prepare the data that will be sent to ArduPilot, in
SendState() we need to read some information from simulation, specifically: linear acceleration and angular velocity from the IMU, and the UAV's pose and linear velocity in the world frame.
In the old code, we get the IMU data by calling methods on the sensor object and copying the result into the packet that we're going to send to ArduPilot:
In the new code, as previously mentioned, these data are accessed by subscribing to the sensor via ign-transport. In that subscription we registered a callback that just copies the latest IMU message to
imuMsg and sets the flag
imuMsgMutex to exclude concurrent access to those variables. So we access the latest IMU sensor by copying and reading from that message:
In the old code, we access the UAV's pose linear velocity in the world frame by calling
WorldLinearVelocity(), respectively, on the model object:
In the new code we instead read from the
WorldLinearVelocity components attached to the entity representing one of the UAV model's links:
Registering the plugin
In the old code we register our plugin via the macro
Compared to the code changes, the updates in the CMake configuration are pretty minor and primarily result from the fact that the formerly monolithic Gazebo project is now a set of Ignition libraries.
In the old code we retrieve all the required build configuration by finding the Gazebo package:
In the new code we explicitly reference each Ignition package that we use:
In the old code we need only refer to the build configuration retrieved from the Gazebo package:
Whereas in the new code we refer to build configuration from each Ignition package:
The old UAV is defined in two parts: (i) the
iris_with_standoffs model, which defines the vehicle structure; and (ii) the
iris_with_ardupilot model, which includes the extends the
iris_with_standoffs model by adding plugins needed to fly it.
Because model inclusion is not (yet?) supported, the new model just combines the additional plugin configuration from
iris_with_standoffs. Along the way a few changes are made, as follows.
<script> tag for visual material is not (yet?) supported, so in the model, instances of
<script> are just commented out (which leaves the UAV visually untextured, but functional).
In the old model, loading an instance of the LiftDrag plugin for each half of each propeller looks like this:
In the new model, we do this instead:
In the old model, it's possible to read joint state and apply joint forces automatically. In the new model, we must instantiate the
JointStatePublisher plugin once for the entire model and the
ApplyJointForce plugin once for each propeller joint:
You should be able to apply the same general changes covered in this tutorial to your Gazebo plugins to migrate them to Ignition.
Check out these instructions if you'd like to learn more about using ardupilot_gazebo with Ignition.