Designing a ROS2 Robot

where to start when building and simulating robots

This post will cover:

  • The building blocks of Robot design
  • Detailing some components on the ROS2/RVIZ2 side
  • Replicating and explaining interactions between ROS2 + Gazebo + ros2_control (which can be used later for motion planning)

This will include links to other tutorials which are necessary to build knowledge in specific areas yet are required in unison to understand the whole development process. This blog post aims to be the unison of these components.

⚠️⚠️WARNING⚠️⚠️

Implementations described here are subject to change as ros2_control is still under development. This blog post serves mainly to introduce how the components interact with one another. If an code-snippet below doesn’t work for you, reference that individual component’s tutorial to see if there is a more “modern” implementation. Examples include:

  • <transmission> element API
  • <ros2_control> tag in URDF vs metadata on each <joint>

Prototyping a robot

The first place to start are the two building blocks of a model in URDF:

Links: the skeleton of the robot

Joints: how the Links of the skeleton interact with one another. Two wooden links might be attached with a nail and are a static joint. A wheel and an axel may be “attached” together by a revolute joint.

Controlling joint movement is foundational to robotics. There are two terms that help us make sense of joint control:

Actuators: the components conducting the movement

Encoders: hardware that can answer the “where are my joints?” question. What APIs are available to relay this information to the motion planner? Think IMU data for joints. While stereo vision is useful for sensing the world, it usually doesn’t answer the question of where the robots joints are relative to each other

One API that might be familiar is the Servo library for an Arduino. The servo library exposes a read and a write method to allow us to control the servo’s position. The write call can be thought of as an Actuator and the read call can be thought of as the Encoder. (Note: the read call isn’t really an Encoder because most Servo code uses a sleep following a write to wait for the servo to reach its desired position. A real Encoder would be able to answer “the servo is at an angle of {40, 41, 42} degrees” after every read call)

Design can be done in 3d modeling environment. SOLIDWORKS seems to be the most common for professional use cases. You may be able to use Blender or other free/open source alternatives.

Building the URDF will need to be done by you as well. I believe that SOLIDWORKS takes care of this which is why designing there is useful.

Finally, you’ll need to fabricate your robot. You may use a pre-built robot but chances are you’ll need to fabricate something that makes your robot unique. Maybe you’ll use a 3d printer to print each “link” or maybe you’ll use wood or aluminum extrusions.

Introduction to Gazebo and the Model Editor

Start with Model Editor tutorials from Gazebo. this will:

  • Introduce you to some of the menus and configuration of a Gazebo Model
  • Illustrate the interaction of Link and Joints
  • Introduce the built-in Model Editor which will help create a SDF representation of a robot

Build the rotate bot using Gazebo model editor

I don’t have access to a more sophisticated robot design tool but you can follow the Gazebo tutorial above and use Gazebo’s Model Editor to build a simple robot that will rotate in a circle.

My robot is a cylinder sandwiched between two rectangles. It shouldn’t matter which joint connecting the cylinder to a rectangle is a Revolute joint but I only chose to make one of them Revolute and made the other Fixed.

After you are done building, you’ll have to save and export the mode as an SDF file. The ROS2 control ecosystem needs a URDF file. Unfortunately, I wasn’t able to find a way to convert SDF files (which are meant to describe more than just a robot) to URDF. The closest thing I found was the sdformat_urdf package which shows some of the limitations when doing the conversions (like requiring only one robot in the SDF file). I ended up copying the gazebo_ros2_tutorial URDF and copy/pasted the following values: pose, size, and radius/length.

Robot visualization with joint_state

While not directly related to control, you can visualize and “control” the robot created above using rviz2. This will also give us a place to create a ros2 control package later in this article. Start by creating a new ament_cmake package in your workspace: (Note: this blog post requires CPP code and will not work in Python)

Create a urdf/ directory with your URDF file created from the previous step.

Next, create a robot_state_publisher_launch.py file which will read and parse the URDF file and create a robot_state_publisher Node:

You’ll need to add install directive to the CMakeLists.txt file

After installing the package, you can launch the robot:

And look at the topics:

This will publish:

  • static joint data to the /tf_static topic
  • other joint data on /tf when joint_state data is available
  • the xml version of your robot on /robot_description

If you echo this topic you can see that robot state publisher is publishing messages on the /tf_static are the fixed joints. However, you don’t see the same on the /tf topic. This is because robot_state_publisher needs to read our joint states before publishing /tf data

to publish a joint state, you can use the joint_state_publisher node. This will need to be installed as a separate package:

Now, you can run the Node and publish the rotational joint

Now, you can verify the joint’s location is published:

so far you can see some data describing our robot but you haven’t yet visualized it. Rviz2 is one tool you can use visualize the /tf and /tf_static data above.

Add > RobotModel

Change the “Description Topic” to /robot_description and change the “Fixed Frame” to world (or any other joint).

After you have visualized your robot, you can also use a GUI to change the joint data via the joint_state_publisher_gui Node. This requires installing the package first:

Using the GUI, you can change the position of the joint as the GUI relays the joint state information to the joint_state_publisher to publish on the /joint_state_topic. I also added a <limit> tag in my URDF which is correctly interpreted by the GUI.

Joint data is in radians!

How to use ros2_control

The section above helped show how you can use the joint_state_publisher_gui to “control” a robot in RViz. However, we need a programmatic way to access and control our robot for things like motion planners to use. This is what ros2_control is useful for.

We know that our robot will exist in two environments: physical and virtual. Because the control subsystem should act the same way for a simulated robot and a real world robot, there should be an interface implemented by both sides. Specifically, the ros2_control package provides this interface through the ROS2 plugin library which requires writing CPP code. The “easiest” way to visualize both implementations is the following picture from the Gazebo docs:

view larger picture at http://gazebosim.org/tutorials?tut=ros_control&cat=connect_ros

The part we will be focusing on is the hardware_interface::RobotHW class which implements the “Joint State Interface” and “Joint Command Interface.”

The next part of this blog post will be implementing/explaining the code already written in the existing ros2_control_demos tutorial. You should try and run this tutorial to make sure your environment is set up correctly:

Implementing the Hardware/Reality Side

While the Simulation side of the picture might seem like an easier place to start and visualize, starting on the Reality side will illustrate where YOUR robot specific code will be written. Because a simulated robot is itself a Robot, they will also need to implement this “robot specific code” but will instead make calls to Gazebo internals rather than actuators/encoders on your robot.

To start, you can use the URDF file from the previous tutorial (or one specific to your robot). Create a new launch file, controller_manager.launch.py, and have it read in the URDF similar to the previous part of this tutorial.

The ros2_control system needs a controller_manager for orchestration of various components. Add the ros2_controler_manager Node to your launch file and supply the robot URDF as a parameter:

Launching the file, you’ll see the following error:

Add the <ros2_control> tag to the URDF:

Re-launching the controller_manager will give you the next error:

This error message is telling you that you need to supply a hardware_interface for ros2_control to manage. Fortunately, there is a test_system hardware_interface that doesn’t expose any Command or State interfaces. You can add this example hardware plugin with the code below:

This should yield a “working” launch file with no Command or State interfaces:

In the above example, there are two things missing when comparing the URDF file to the ros2_control_demos code:

  1. a specific <plugin> for our robot (ros2_control_demo_hardware)
  2. joint metadata linking a <joint> to a <state_interface> and <command_interface> inside of the plugin from (1)

Instead of diving into (1), try implementing (2) by adding a <joint> tag to the <ros2_control> element:

After rerunning the launch file, you’ll receive the following error:

If you reference the picture above, the “JointStateInterface” is implemented in a robot-specific hardware plugin. After all, our robot might have two or three actuators. How could the test_system plugin possibly control or encode the state of our specific hardware? The above error message is showing us that the hardware Plugin also needs to describe which joints it supports. We will see below that this is accomplished in the two export_* functions.

Copy the example from the ros2_control_demo_hardware. I am new to writing CPP code and didn’t follow any of the ROS2 CPP tutorials other than a package for Message files. At a high level, you’ll have to update the following files:

  • package.xml (for dependencies)
  • XML file describing the hardware interface plugin (ros2_control_demo_hardware.xml in their case)
  • CMakeLists.txt (for compilation targets and `pluginlib_export_plugin_description_file`)
  • .hpp include file
  • .cpp class file

Inspecting the header file, you can see the functions you’ll have to implement:

  • configure
  • export_state_interfaces
  • export_command_interfaces
  • start
  • stop
  • read
  • write

After copying the CPP class file, you can inspect the hardware interface code in more detail. The “API” between your code and the ROS2 control ecosystem is are the following member variables of the class:

  • info_
  • hw_states_ (private members of the class, referenced by return value of exported state interfaces)
  • hw_commands_ (private members of the class, referenced by return value of exported command interfaces)

In the copied code, most of the code ends up writing a value of zero.

After this code compiles, swap out the test_system plugin with your robot-specific plugin:

If you’ve copied and modified the code mentioned above, you should see the same output from the ros2_control_demos tutorial:

The read and write functions are probably the most robot-specific code you’ll end up writing/replacing.

In the the write function, you code reads out of the hw_commands_ array. With that value, you would send some commands to the hardware (like servo or stepper motor)

In the read function, you would want to know the progress of the above command. Just because you told a joint to move from 20 degrees to 90 degrees doesn’t mean it is in its desired position! Your encoders should answer this question and “return” this result by writing to the hw_states_ array.

At this point, we have a piece of code that bridges our robot to the ros2_control ecosystem. Next, let’s understand which components needs to change for implement this control in simulation.

Implementing the Simulation Side

⚠️ Note: Unfortunately, I the functionality hasn’t landed on gazebo_ros2_control package to build out-of-the-box. Fortunately, a fix for this is on this feature branch and checking it out should mostly work for this demo. Also, I also can’t figure out a bug with the gazebo_ros2_control plugin fails to find symbols from the hardware_interface package (though the symbol is definitely in both shared object files). Hopefully the rest of this tutorial remains useful without working commands/output.

We know that Gazebo will need to implement the hardware_interface above so the ros2_control controller_manager Node can instantiate and communicate with the simulated robot. If you look at the gazebo_ros2_control package, you can see the gazebo_hardware_plugins.xml file which signifies that Gazebo is acting as a hardware_interface. Looking through the code, it is the gazebo_system.cpp file that implements the read/write and export_* functions.

I would recommend creating another launch file to organize exactly what you need and also compare side-by-side with other files. You’ll need the robot_state_publisher from before for the /robot_description topic and also the gazebo + spawn_entity nodes:

To instantiate Gazebo’s controller_manager and to provide the “extra” controllers that the controller_manager should start, add the following to the your URDF:

If you want to checkpoint your progress, you can try launching the Gazebo launch file. You can pass an empty YAML file (or comment out existing files) but you will need to provide this parameter or Gazebo will crash.

When the gazebo_ros2_control plugin starts, the gazebo_system hardware_interface will read our URDF and needs to decided which joints it should or shouldn’t manage. The <transmission> elements are used to communicate which joints should be managed as well as reference which type of hardware API the joint supports (position, velocity, effort). Apparently, there are more complex joints that can be modeled and adding your own Gazebo-specific representation would require modifying the gazebo_system to support instantiating your specific hardware_interface.

To add a transmission element for your joint, add the following element for every joint you want Gazebo to control. We know our joint is a revolute joint so lets assume it is going to mimic an Arduino Servo. The Servo “actuator” API (write) is in degrees which can be though of as a “position.” This is opposed to a theoretical Servo API that takes a velocity (maybe radians/sec or revolutions/sec). Due to a “position” based API, we should use the PositionJointInterface to mimic our own hardware_interface. Here is an example <transmission> element for our rotate bot:

This isn’t a traditional ros params file though it is interpreted similarly. This file contains various high-level ros2_control controllers. The two included in the existing examples are:

  • JointTrajectoryController
  • JointStateController

You’ve already seen the JointStateController earlier in this post. The JointTrajectoryController takes care of managing the commands sent to /my_rotate_bot_controller/follow_joint_trajectory . This controller can receive the FollowJointTrajectory message which is useful for sending a list of JointTrajectoryPoints. If our robot can satisfy these sorts of “commands,” it should be possible to layer on even high level controllers for sensing, grasping, and moving objects.

At the time of this article, the gazebo_ros2_control instantiates its own controller_manager. This is because ros2_control requires interaction via the command line to spawn controllers and I think this internal management is for convenience rather than necessity.

To summarize, the whole Gazebo code flow is the following:

  1. Gazebo is started
  2. robot_state_publisher is started and publishes the URDF to a topic
  3. spawn_entity reads the URDF from the robot description topic and instructs Gazebo to instantiate this robot
  4. Gazebo sees the <gazebo> tag and starts the gazebo_system
  5. The gazebo_system reads the URDF and implements hardware_interface APIs for <transmission> elements
  6. The gazebo_system also reads the controller YAML file and starts the various controllers defined

You can see the ros2_controller running by adding the -c gazebo_controller_manager to your various ros2 control commands

Copy the example_velocity.cpp file and change the topics to your controller. You’ll also need to change the values of the points to radians as this robot has a Revolute joint instead of Prismatic.

Summary

The gazebo_ros2_control_demos package is great because it shows how other systems could issues commands that your robot will respond to. All that is left is to add these high level controllers to our “Reality” launch file and see if we can replicate similar commands. If you were to implement the actuator/encoder API calls in the write/read parts of your Hardware interface, you should be able to send the same trajectory message to both Gazebo and your real-world robot. Hopefully, I can implement this for the next blog post and demo how MoveIt! can be used to send such messages.