Lesson 3: Robot Plugins

Previous Lesson

Lesson Purpose:
By the end of this lesson you should be able to:

Make a plugin for your rover that can move its wheels

Assumed Prerequisite Knowledge:

Lessons 1 and 2 completed, with their homeworks done.
Basic/Intermediate Knowledge of C++ (Recommended Tutorials: Be familiar with all concepts up until Advanced)
Knowledge of ROS nodes, particularly publishers and subscribers (Tutorials: 1.1, 1-16 (excluding the python tutorials))

Lesson 3: Robot Plugin

So now we've got this hunk-o-simulated robot. But it really doesn't do anything. It's a nice visualization of our robot, but any CAD software will do that for us, so it really doesn't help us that we would CAD up our robot, and then do the extra work of making a URDF just to see it sit there and do nothing with it.

1. Hello GazeboWorld
Robots in the real world usually have some bit of code that runs on them and makes them work - so we want our robot in our simulated world to also be running some code. In Gazebo that is known as a Model Plugin.

So let's get to work and make a plugin.

Go to the plugin folder of our tutorial package, and create a file: rover.cc
And create our pragma once - the bit of code that prevents this from being compiled over and over again. This is just going to be a class definition after all, so we only need one instance of it to be compiled.

Now we need to include our ROS and Gazebo headers:

Now, before the #endif we are going to define our plugin class. Insert the following code:

What we are doing is:
First: wrapping ourselves in the gazebo namespace. This is just to make things a little prettier, so we aren't constantly having to specify the namespace of gazebo.

Then we are defining a class called RoverPlugin which inherits from the gazebo class ModelPlugin. Right now we have no code in this class but an empty constructor.

Then after our class definition we add in the gazebo macro: GZ_REGISTER_MODEL_PLUGIN, which handles all of the gazebo side of registering our plugin.

The ModelPlugin class we are inheriting from is a pure virtual class though, in particular there is a single function we have to overload, which is its Load(...) function, which is called whenever the plugin is loaded. So let's overload that.

In the public: section of our class add the following function:

Here we override the ModelPlugin's Load function, which has as an input a pointer to the physics model of our robot, and a pointer to the SDF (which is sorta like the interpreted URDF).

All we do in the function right now, is we make sure that the ROS master node has been launched, if not our plugin is going to fail anyways so we might as well terminate nicely.

If the ROS core node has been launched then we are just going to send out a little Hello World message.

Our plugin is now ready to compile and attach to a model. In whole it should look like:

Now let's compile that bad boy and attach him to a model, so we can actually see it at work.

2. Compiling the Plugin and Attaching It
Our CMakeLists.txt is already set up to be able to compile plugins, so all we need to do is point it at the right plugin. To do this, around where you see the wheely_boi plugin being compiled, add the following lines:

Now head on out to your workspace folder and make sure it all still compiles with the command:
$ catkin_make
If everything was set up right, it should compile.

Now we've got to attach it to our model. Open up your rover.xacro and add (within the <robot> tag):

Now for the moment of truth. Let's try and launch our rover with it's new plugin.

First compile our updated xacro:
$ rosrun xacro xacro rover.xacro > rover.urdf

Then in another terminal, source your workspace
And then launch gazebo:
$ roslaunch gazebo_ros empty_world.launch

Now spawn your model:
$ rosrun gazebo_ros spawn_model -file rover.urdf -urdf -model Fred

In the terminal where you launched gazebo, you should then see a message saying "Rover Plugin Loaded". If you don't see that, make sure that both the terminal where you launch gazebo and the terminal where you spawn the model from are properly sourced.

3. Making it Move
That's pretty neat, but our model still doesn't do anything. Robots do more than just say "Hello World", so our simulated robot should be able to to.

So let's take another pass at our plugin and learn how to get a wheel moving.

First thing's first though, we don't want our code to only be executed at the moment that the robot is loaded. We want to have it executed whenever something is simulated. So let's declare a new public function that we are going to be using as a callback function on each nanosecond update tick.

Now we want that function to actually be called.

First create a private member variable of an event callback:

Now in our Load(...) function we are going to bind that event to our onUpdate function, and to the update tick.

To do this, after we shoot off our "Plugin Loaded" message, add this line:

Now we have a function called every nanosecond tick, so we can actually get to some programming.

Now let's get a wheel to turn.

In order to do that though, we need to have a pointer to our model that we can use in our onUpdate function. So we need a new private member variable. Let's add it and initialize it:

Now we're ready to actually turn a wheel.

In onUpdate() add the following command:

So now our plugin file should look like:

Now let's compile it, and spawn a model.

Once we do, we should see our robot shoot off in a straight line. This is because the other wheel joints aren't exerting much friction, so there is little resistance to stop it from shooting off in a straight line.

That being said, our rover moves now! So now the real fun and simulating can begin.

Homework:

1. Create a more advanced rover
It's not a good sign that our rover just shoots off in a straight line, but as a result of our primitive skid steering design, that's what's gonna happen. Let's give our rover some differentials now!

Add two new blocks attached to the body by a moveable joints "lDifJoint" and "rDifJoint" which spin around the Z axis. Attach our front left and front right wheels to this moveable blocks.

2. Control our rover
Update our plugin so that it listens for a Geometry/Twist message on the topic: $(name)/$(name)/cmd
where $(name) is the name of our model.

It should then adjust its forward speed based on the linear.x component, and the twist of its front two wheels based on the angular.z component.

For this part of the homework, it'll be handy to have a link to the gazebo API to see what that model pointer can give you.
Here is that link.

Reference Project

This is a pretty large task, so the next tutorial is going to be talking about a solution to this problem (not the only one), you should make a solid attempt at making this work before looking at the next part of the tutorial, and if you get it all figured out then feel free to skip the next tutorial.

Next Lesson