Designing a ROS2 Robot

where to start when building and simulating robots

14 min readJan 31, 2021

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)

$ ros2 pkg create --build-type ament_cmake my_rotate_bot

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:

doc = xacro.parse(open(urdf_file))
xacro.process_doc(doc)
robot_description = {'robot_description': doc.toxml()}node_robot_state_publisher = Node(
  package='robot_state_publisher',
  executable='robot_state_publisher',
  output='screen',
  parameters=[robot_description]
)

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

install(DIRECTORY
  launch
  urdf
  DESTINATION share/${PROJECT_NAME}/
)

After installing the package, you can launch the robot:

ros2 launch my_rotate_bot robot_state_publisher.launch.py

And look at the topics:

$ ros2 topic list
/joint_states
/parameter_events
/robot_description
/rosout
/tf
/tf_static

robot_state_publisher

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

/tf and /tf_static

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

$ ros2 topic hz /tf_static
average rate: 19.982
 min: 0.050s max: 0.051s std dev: 0.00017s window: 21# no output :(
$ ros2 topic hz /tf

/joint_states

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

sudo apt-get install ros-foxy-joint-state-publisher

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

ros2 run joint_state_publisher joint_state_publisher ./src/my_rotate_bot/urdf/model.urdf

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

$ ros2 topic echo /tf
...
transforms:
- header:
    stamp:
      sec: 1611364018
      nanosec: 380936740
    frame_id: link_1
  child_frame_id: link_2
  transform:
    translation:
      x: 0.0
      y: 0.0
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0$ ros2 topic hz /tf
average rate: 9.992
 min: 0.100s max: 0.101s std dev: 0.00029s window: 12$ ros2 topic hz /joint_states
average rate: 10.000
 min: 0.100s max: 0.100s std dev: 0.00021s window: 12

Visualize with rviz2

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.

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:

sudo apt-get install ros-foxy-joint-state-publisher-guiros2 run joint_state_publisher_gui joint_state_publisher_gui

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.

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.”

Running the Existing Tutorial

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:

ros-controls/ros2_control_demos

This repository provides templates for the development of ros2_control-enabled robots and a simple simulation of a…

github.com

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:

controllers_file = os.path.join(
  my_rotate_bot_path,
  'controllers',
  'ros2_control_controllers.yaml'
)controller_manager = Node(
  package="controller_manager",
  executable="ros2_control_node",
  parameters=[
    robot_description,
    controllers_file
  ],
  output='screen'
)

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

ros2_control_node-2] terminate called after throwing an instance of 'std::runtime_error'
[ros2_control_node-2]   what():  no ros2_control tag

Add the <ros2_control> tag to the URDF:

<robot>
  <link>...
  <joint>...  <ros2_control name="rotate_box_bot" type="system">
  </ros2_control>
</robot>

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

[ros2_control_node-1] terminate called after throwing an instance of 'pluginlib::LibraryLoadException'
[ros2_control_node-1]   what():  According to the loaded plugin descriptions the class  with base class type hardware_interface::SystemInterface does not exist. Declared types are ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware test_robot_hardware/TestTwoJointSystem test_system

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:

<ros2_control name="rotate_box_bot" type="system">
  <hardware>
    <plugin>test_system</plugin>
  </hardware>
</ros2_control>

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

$ ros2 control list_hardware_interfaces
command interfaces
state interfaces

Joint State Interface and Joint Command Interface

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

a specific <plugin> for our robot (ros2_control_demo_hardware)
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:

<ros2_control name="rotate_box_bot" type="system">
  <hardware>
    <plugin>test_system</plugin>
  </hardware>
  <joint name="link_1_JOINT_0">
    <command_interface name="position"/>
    <state_interface name="position"/>
  </joint>
</ros2_control>

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

[ros2_control_node-1] terminate called after throwing an instance of 'std::runtime_error'
[ros2_control_node-1]   what():  wrong state or command interface configuration.
[ros2_control_node-1] missing state interfaces:
[ros2_control_node-1] link_1_JOINT_0/position 
[ros2_control_node-1] missing command interfaces:
[ros2_control_node-1]

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.

subclassing hardware_interface::BaseInterface<…>

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:

<ros2_control name="rotate_box_bot" type="system">
  <hardware
    <plugin>my_rotate_bot_plugin/MyRotateBotHardware</plugin>
    <param name="example_param_hw_start_duration_sec">2.0</param
    <param name="example_param_hw_stop_duration_sec">3.0</param
    <param name="example_param_hw_slowdown">2.0</param>
  </hardware>
  <joint name="link_1_JOINT_0">
    <command_interface name="position"/>
    <state_interface name="position"/>
  </joint>
</ros2_control>

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

$ ros2 launch my_rotate_bot robot_state_publisher.launch.py 
[INFO] [launch]: All log files can be found below /home/jeffg/.ros/log/2021-01-25-17-33-42-867635-ubu-27302
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ros2_control_node-1]: process started with pid [27304]
[ros2_control_node-1] [INFO] [1611624822.949071807] [MyRotateBotHardware]: state joint 
[ros2_control_node-1] [INFO] [1611624822.949172982] [MyRotateBotHardware]: command joint 
[ros2_control_node-1] [INFO] [1611624822.949286576] [MyRotateBotHardware]: Starting ...please wait...
[ros2_control_node-1] [INFO] [1611624823.949582340] [MyRotateBotHardware]: 2.0 seconds left...
[ros2_control_node-1] [INFO] [1611624824.949720220] [MyRotateBotHardware]: 1.0 seconds left...
[ros2_control_node-1] [INFO] [1611624825.949896621] [MyRotateBotHardware]: 0.0 seconds left...
[ros2_control_node-1] [INFO] [1611624825.949981083] [MyRotateBotHardware]: System Sucessfully started!
[ros2_control_node-1] [INFO] [1611624825.956622988] [controller_manager]: update rate is 2 Hz
[ros2_control_node-1] [INFO] [1611624825.956707281] [MyRotateBotHardware]: Reading...
[ros2_control_node-1] [INFO] [1611624825.956714657] [MyRotateBotHardware]: Got state 0.00000 for joint 0!
[ros2_control_node-1] [INFO] [1611624825.956720448] [MyRotateBotHardware]: Joints sucessfully read!
[ros2_control_node-1] [INFO] [1611624825.956731341] [MyRotateBotHardware]: Writing...
[ros2_control_node-1] [INFO] [1611624825.956735449] [MyRotateBotHardware]: Got command 0.00000 for joint 0!
...

The read and write functions

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.

Summary of ros2_control for Reality

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:

# gazebo
gazebo = IncludeLaunchDescription(
  PythonLaunchDescriptionSource(
  [
    os.path.join(
      get_package_share_directory('gazebo_ros'), 'launch'),
    '/gazebo.launch.py'
  ]),
)# spawn robot
spawn_entity = Node(package='gazebo_ros',
  executable='spawn_entity.py',
  arguments=[
    '-topic', 'robot_description',
    '-entity', 'rotate_box_bot'],
  output='screen')

Starting Gazebo’s ros2_control plugin

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:

<gazebo>
  <plugin name="gazebo_ros2_control"
      filename="libgazebo_ros2_control.so">
     <robot_sim_type>
       gazebo_ros2_control/DefaultRobotHWSim
     </robot_sim_type
     <robotNamespace>
       /rotate_box_bot
     </robotNamespace>
    <parameters>
      $(find my_rotate_bot)/controllers/gazebo_ros2_control_controllers.yaml
    </parameters>
    <control_period>0.01</control_period
    <e_stop_topic>/estop</e_stop_topic>
  </plugin>
</gazebo>

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.

Adding Transmission Elements

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:

<transmission name="link_1_JOINT_0_transmission">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="link_1_JOINT_0">
    <hardwareInterface>
      hardware_interface/PositionJointInterface
    </hardwareInterface>
  </joint>
  <actuator name="base_to_top_motor"
    <hardwareInterface>
      hardware_interface/PositionJointInterface
    </hardwareInterface>
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
</transmission>

gazebo_ros2_control_controllers.yaml

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.

Running Everything

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:

Gazebo is started
robot_state_publisher is started and publishes the URDF to a topic
spawn_entity reads the URDF from the robot description topic and instructs Gazebo to instantiate this robot
Gazebo sees the <gazebo> tag and starts the gazebo_system
The gazebo_system reads the URDF and implements hardware_interface APIs for <transmission> elements
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

Issuing a command

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.

jgensler8/my_rotate_bot

copied code from other tutorials. Contribute to jgensler8/my_rotate_bot development by creating an account on GitHub.