Robot Operating System: Expose Control Nodes for an Interactive Simulation in Gazebo

In the recent articles about ROS and my project RADU, I showed how to launch a custom robot model in RVIZ and in Gazebo. In RVIZ, the robot was visually rendered, and with a small build-in GUI application, we could modify the joints of the robot. The Gazebo simulation that we finished in the last post was only visual. However, the goal is to have a fully working, controllable representation of the robot that can move inside its environment.

There are two ways to achieve this goal. Exposing controller nodes explicitly and work with them - the focus of this article. Or to use predefined Gazebo plugin that will do the heavy lifting for you.

In this article, I present a tutorial how to configure your URDF model and explicitly start control nodes. These nodes expose commands to control the robot model, but to work, they need additional glue code, which will not be covered here. However, you will learn fundamental information about ROS internals and can use this when you want to implement code that moves your robot in the real world too.

This article originally appeared at my blog admantium.com.

Background: ROS2 Controller High Level View

Controller nodes provide a convenient interface to control the joints of your robot. When the joints of your robot are exposed, you can send commands and they will translate the current state to the desired state. Controller nodes are separated into three groups: Effort, velocity, and position. Each controller will modify the state in accordance with this type.

Controller nodes communicate with the hardware interface of your robot. This hardware interface is again an intermediate layer that represents either a simulated robot, e.g. in Gazebo, or a real robot. Therefore, once you have the simulation running, you theoretically just need to provide a hardware controller with the same interface as the simulated controller. And also, this allows you to move your robot in the real environment and see real-time simulation with Gazebo.

Controllers are provided with the ros-control package, which provides way more features than sketched here. For a complete overview about ros-control, check the official documentation.

Overview: Expose Controller Nodes in Gazebo

Exposing controller nodes explicitly involves a number of steps. Loosely following the official documentation, ros control demo repository and my own experience, these are:

• Dependency Management
  • Add dependencies to the package XML
• URDF model updates
  • Add <transmission> tags for each joint
  • Add the ROS2 hardware plugin
  • Add the gazebo-ros plugin
• Controller Nodes Configuration & Startup
  • Provide a controller config
  • Define a launch script for the controller nodes

Step 1: Dependency Management

Add the following dependencies to your pacakge.xml.

  <depend>urdf</depend>
  <depend>angles</depend>
  <depend>gazebo_dev</depend>
  <depend>gazebo_ros</depend>
  <depend>controller_manager</depend>
  <depend>hardware_interface</depend>
  <depend>pluginlib</depend>
  <depend>std_msgs</depend>
  <depend>joint_state_publisher</depend>
  <depend>robot_state_publisher</depend>
  <depend>gazebo_ros_pkgs</depend>
  <depend>ros2launch</depend>

And install them.

rosdep install --from-paths ./ -i -y --rosdistro foxy       --ignore-src
#All required rosdeps installed successfully

Also, be sure you have the ros2 control packages installed.

apt-get install ros-foxy-ros2-control ros-foxy-ros2-controllers

Step 2: URDF Model Updates

Add Transmission Tags in your URDF Model

The controller nodes need access to the joints via an effort, velocity or position interface. For each joint, decide which interface you want to expose, and then provide additional transmission tags.

Here is an example how to add an effort interface to my robots’ front wheel joint.

<ros2_control name="GazeboSystem" type="system">
  <transmission name="base_link_left_wheel_frontside_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="base_link_left_wheel_frontside">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="base_link_left_wheel_frontside_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
</ros2_control>

To make this - and all of the other following changes - more feasible, I define additional XACRO macros. The macro provides one transmission block joint. You always include the joint_state_controller as shown above, and then make a section in the form joint_name + _position_controller.

Add the Hardware Plugin

The URDF model needs to instruct the ROS2 system with which hardware plugin the robot can be accessed. For simulation purposes, you use the following. Note that you need to include it in the same <ros2_control> tag as above.

<ros2_control name="GazeboSystem" type="system">
  <hardware>
    <plugin>gazebo_ros2_control/GazeboSystem</plugin>
  </hardware>
</ros2_control>

Add the Gazebo ROS Plugin

To interact with the robot in the simulation, you need to add the special gazebo-ros plugin and configure it with the topic name of the robot description, the namespace of the robot, the name of the node where the robot description can be found, and a link to the robot configuration file (covered in next section).

<gazebo>
  <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
    <robot_param>robot_description</robot_param>
    <robot_namespace>box_bot</robot_namespace>
    <robot_param_node>robot_state_publisher</robot_param_node>
    <parameters>$(find radu_bot)/config/controller.yaml</parameters>
  </plugin>
</gazebo>

Step 3: Controller Nodes Configuration & Startup

Provide a Controller Configuration File

The controller configuration file defines which controller types you want to use, and which joints can be interfaced by which controller type. The following config/controller.yaml file declares that an effort and velocity controller are used, and that each controller accesses all joints.

controller_manager:
  ros__parameters:
    update_rate: 100 #Hz

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    joint_state_controller:
      type: joint_state_controller/JointStateController

    effort_controllers:
      type: effort_controllers/JointGroupEffortController

    velocity_controller:
      type: velocity_controllers/JointGroupVelocityController

effort_controllers:
  ros__parameters:
    joints:
      - base_link_right_wheel_frontside
      - base_link_left_wheel_frontside
      - base_link_right_wheel_backside
      - base_link_left_wheel_backside

velocity_controller:
  ros__parameters:
    joints:
      - base_link_right_wheel_frontside
      - base_link_left_wheel_frontside
      - base_link_right_wheel_backside
      - base_link_left_wheel_backside

Define a launch script for the controller nodes

The final step is to create a launch file. The following script starts three nodes: The ros2 control node, which will read the controller configuration file an create the defined controller interface services/topics, and the nodes for publishing the joint state and the robot state.

#!/usr/bin/python3
# -*- coding: utf-8 -*-
import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

from time import sleep
import filecmp

package_name = 'radu_bot'
world_file = 'empty.world'

import xacro

def generate_launch_description():

    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
    pkg_radu_simulation = get_package_share_directory(package_name)

    robot_description_path =  os.path.join(
        pkg_radu_simulation,
        "urdf2",
        "gazebo.xacro",
    )

    robot_description = {"robot_description": xacro.process_file(robot_description_path).toxml()}

    controller_config = os.path.join(
        pkg_radu_simulation,
        "config",
        "controller.yaml",
    )

    print("MODEL %s" % robot_description['robot_description'])
    sleep(3)
    print("Config %s" % open(controller_config, 'r').read())

    control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[robot_description, controller_config],
        output="both",
    )

    joint_state_publisher_node = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher'
    )

    robot_state_publisher_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="both",
        parameters=[robot_description],
    )

    return LaunchDescription([
      control_node,
      joint_state_publisher_node,
      robot_state_publisher_node
    ])

Putting it all together

Order is important - Gazebo, controller, robot description need to interact in a precise order. Getting this right was quite difficult. Therefore, I used the ros2 demo repository for a comparison. This project provides a Docker container that includes a complete Ubuntu runtime with ROS2 foxy, all required plugins, and the robot’s description and Gazebo simulation configuration. When this container is started, I see the following log message.

[INFO] [gzserver-1]: process started with pid [31]
[INFO] [robot_state_publisher-2]: process started with pid [34]
[robot_state_publisher-2] Parsing robot urdf xml string.
[spawn_entity.py-3] [INFO] [1622572355.263095787] [spawn_entity]: Spawn Entity started
[spawn_entity.py-3] [INFO] [1622572358.704011723] [spawn_entity]: Spawn status: SpawnEntity: Successfully spawned entity [cartpole]
[gzserver-1] [INFO] [1622572358.886969317] [gazebo_ros2_control]: Loading gazebo_ros2_control plugin
[gzserver-1] [INFO] [1622572358.896913314] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in namespace: /
[gzserver-1] [INFO] [1622572358.898607083] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in ros 2 node: gazebo_ros2_control
[gzserver-1] [INFO] [1622572358.899384425] [gazebo_ros2_control]: Loading parameter file /home/ros2_ws/install/share/gazebo_ros2_control_demos/config/cartpole_controller.yaml
[gzserver-1] [INFO] [1622572360.067497363] [gazebo_ros2_control]: connected to service!! robot_state_publisher
[gzserver-1] [INFO] [1622572360.075006386] [gazebo_ros2_control]: Recieved urdf from param server, parsing...
[gzserver-1] [INFO] [1622572360.207253145] [gazebo_ros2_control]: Loading joint: slider_to_cart
[gzserver-1] [INFO] [1622572360.207419217] [gazebo_ros2_control]:  Command:
[gzserver-1] [INFO] [1622572360.207595559] [gazebo_ros2_control]:    position
[gzserver-1] [INFO] [1622572360.211177069] [gazebo_ros2_control]:  State:
[gzserver-1] [INFO] [1622572360.211314621] [gazebo_ros2_control]:    position
[gzserver-1] [INFO] [1622572360.213291135] [gazebo_ros2_control]:    velocity
[gzserver-1] [INFO] [1622572360.213430680] [gazebo_ros2_control]:    effort
[gzserver-1] [INFO] [1622572360.221339236] [gazebo_ros2_control]: Loading controller_manager
[gzserver-1] [INFO] [1622572360.297823090] [gazebo_ros2_control]: Loaded gazebo_ros2_control.
[gzserver-1] [INFO] [1622572361.105781106] [controller_manager]: Loading controller 'joint_state_controller'
[gzserver-1] [INFO] [1622572361.192078267] [controller_manager]: Configuring controller 'joint_state_controller'
[ros2-4] Successfully loaded and started controller joint_state_controller

We see these stages:

• The model is spawned
• Gazebo loads the control plugins
• Gazebo connects to the robot state publisher
• Gazebo parses the URDF, detect the controller types and for which joints they are effective
• The controller manager loads the joint-state-controller

I tried to get the exact same order of status messages when starting my applications, and succeed by launching Gazebo, start the controller nodes, then spawn the robot. Then, in reverse order, the URDF model will be used to start the controller nodes for the joints, and Gazebo will register the control plugin.

STARTING ALL NODES
[INFO] [ros2_control_node-1]: process started with pid [12284]
[INFO] [joint_state_publisher-2]: process started with pid [12286]
[INFO] [robot_state_publisher-3]: process started with pid [12288]
[INFO] [ros2-4]: process started with pid [12290]
[INFO] [ros2-5]: process started with pid [12292]
[INFO] [ros2-6]: process started with pid [12294]
[robot_state_publisher-3] Parsing robot urdf xml string.
[robot_state_publisher-3] The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[robot_state_publisher-3] Link left_wheel_backside had 0 children
[robot_state_publisher-3] Link left_wheel_frontside had 0 children
[robot_state_publisher-3] Link right_wheel_backside had 0 children
[robot_state_publisher-3] Link right_wheel_frontside had 0 children
[robot_state_publisher-3] [INFO] [1622905120.355071330] [robot_state_publisher]: got segment base_link
[robot_state_publisher-3] [INFO] [1622905120.355477881] [robot_state_publisher]: got segment left_wheel_backside
[robot_state_publisher-3] [INFO] [1622905120.355543125] [robot_state_publisher]: got segment left_wheel_frontside
[robot_state_publisher-3] [INFO] [1622905120.355576706] [robot_state_publisher]: got segment right_wheel_backside
[robot_state_publisher-3] [INFO] [1622905120.355609645] [robot_state_publisher]: got segment right_wheel_frontside
[ros2_control_node-1] [INFO] [1622905120.818789225] [controller_manager]: update rate is 100 Hz Hz
[ros2_control_node-1] [INFO] [1622905125.182432589] [controller_manager]: Loading controller 'velocity_ controller'
[ros2_control_node-1] [INFO] [1622905125.241460316] [controller_manager]: Configuring controller 'velocity_controller'
[ros2_control_node-1] [INFO] [1622905125.243706806] [velocity_controller]: configure successful
[ros2_control_node-1] [INFO] [1622905125.252504529] [controller_manager]: Loading controller 'effort_controllers'
[ros2-6] deprecated warning: Please use 'load_controller --set_state start'
[ros2-6] Successfully loaded and started controller velocity_controller
[ros2_control_node-1] [INFO] [1622905125.342950455] [controller_manager]: Configuring controller 'effort_controllers'
[ros2_control_node-1] [INFO] [1622905125.346626669] [effort_controllers]: configure successful
[ros2-5] deprecated warning: Please use 'load_controller --set_state start'
[ros2-5] Successfully loaded and started controller effort_controllers
[ros2_control_node-1] [INFO] [1622905125.410014821] [controller_manager]: Loading controller 'joint_state_controller'
[ros2_control_node-1] [INFO] [1622905125.528520009] [controller_manager]: Configuring controller 'joint_state_controller'
[ros2-4] deprecated warning: Please use 'load_controller --set_state start'
[ros2-4] Successfully loaded and started controller joint_state_controller
[INFO] [ros2-5]: process has finished cleanly [pid 12292]
[INFO] [ros2-6]: process has finished cleanly [pid 12294]
[INFO] [ros2-4]: process has finished cleanly [pid 12290]
[joint_state_publisher-2] [INFO] [1622905126.082525692] [joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic...

This looks good – all nodes are started, no error messages. We will see several new topics – the /velocity_controller, the /effort_controller, and the /controller_manager.

Let’s publishing a velocity and an effort command to the controllers:

ros2topic pub /velocity_controller/commands  std_msgs/msg/Float64MultiArray "data:
- 0.5
- 0.5
- 0.0
- 0.0"

ros2topic pub /effort_controllers/commands  std_msgs/msg/Float64MultiArray "data:
- 0.5
- 0.5
- 0.0
- 0.0"

The commands were processed correctly. But as stated in the introduction, the robot does not move in the simulation by its own, you need another node that interprets these commands and interacts with the Gazebo simulation to move the robot. We can write this on our own, e.g. as a Python script. This is the role of plugins, and ros-control comes with a set of plugins for actuators and sensors. This will be explored in the next article.

Conclusion

To control a robot in Nodes, you can explicitly define controllers of type effort, velocity and position. These controllers accept commands and access a common hardware interface to instruct a robot. In theory, these control nodes can access a robot inside a Gazebo simulation - by using ros-control plugins - or a real robot when you provide the interface code. This article showed how to define and publish explicit controller nodes. You need modify the URDF model of your robot, add special <transmission>, hardware and Gazebo plugins. Second, you need to provide a controller config file and a launch file. Third, you need to start the nodes in a specific order. However, to actually move the robot inside the simulation, you need custom scripts that translate commands from the command nodes to your robot. Most ROS simulations use plugins explicitly for this purpose, and we will explore plugins in the next article.