Interface Gazebo with custom ROS2 nodes

Interface Gazebo with custom ROS2 nodes#

Added in version Jazzy: This section.

Warning

This topic is under construction and this might not even be its final form. Please feel free to open an issue if you spot any typos or other problems.

Setting up the scene#

We will use a slightly modified version of the shapes_with_tf2_publisher.sdf created previously. Let’s add it to our Gazebo folder.

We will use two systems in this example. We will use gz::sim::systems::UserCommands to allow us to set the pose of entities and gz::sim::systems::ApplyLinkWrench to allow us to set wrenches to links. This is illustrative because poses use Gazebo services and wrenches use Gazebo topics.

We will modify our shapes_with_tf2_publisher.sdf to add the following lines inside the <world> tag.

        <physics type="ode">
          <max_step_size>0.004</max_step_size>
          <real_time_factor>1.0</real_time_factor>
          <real_time_update_rate>250</real_time_update_rate>
        </physics>
        <plugin name='gz::sim::systems::Physics' filename='gz-sim-physics-system'/>
        <plugin filename="gz-sim-scene-broadcaster-system"  name="gz::sim::systems::SceneBroadcaster"/>
        <plugin filename="gz-sim-apply-link-wrench-system"  name="gz::sim::systems::ApplyLinkWrench"/>
        <plugin filename="gz-sim-user-commands-system" name="gz::sim::systems::UserCommands"/>

Here’s a brief explanation of why we need each one of these.

Note

More information is available in the gz-sim/src/systems folder at gazebosim/gz-sim.

`gz::sim::systems::Physics`	The physics behavior needed by the other systems.
`gz::sim::systems::SceneBroadcaster`	Needed by the other systems [doc].
`gz::sim::systems::ApplyLinkWrench`	Creates the `/wrench` topic [doc].
`gz::sim::systems::UserCommands`	Creates the `/set_pose` service [doc].

We start by adding the following file to your ~/gazebo_tutorial_workspace/scenes folder.

shapes_with_tf2_and_wrench.sdf

Contents of shapes_with_tf2_and_wrench.sdf

<?xml version="1.0" ?>
<sdf version="1.11">
    <!--
      Try moving a model using the command in the following CDATA block::
    -->
    <![CDATA[
    gz service -s /world/shapes/set_pose \
               --reqtype gz.msgs.Pose --reptype gz.msgs.Boolean \
               --timeout 300 --req 'name: "box", position: {z: 5.0}'
  ]]>
    <world name="shapes_with_tf2_and_wrench">

        <physics type="ode">
          <max_step_size>0.004</max_step_size>
          <real_time_factor>1.0</real_time_factor>
          <real_time_update_rate>250</real_time_update_rate>
        </physics>
        <plugin name='gz::sim::systems::Physics' filename='gz-sim-physics-system'/>
        <plugin filename="gz-sim-scene-broadcaster-system"  name="gz::sim::systems::SceneBroadcaster"/>
        <plugin filename="gz-sim-apply-link-wrench-system"  name="gz::sim::systems::ApplyLinkWrench"/>
        <plugin filename="gz-sim-user-commands-system" name="gz::sim::systems::UserCommands"/>

        <scene>
            <ambient>1.0 1.0 1.0</ambient>
            <background>0.8 0.8 0.8</background>
        </scene>

        <light type="directional" name="sun">
            <cast_shadows>true</cast_shadows>
            <pose>0 0 10 0 0 0</pose>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.2 0.2 0.2 1</specular>
            <attenuation>
                <range>1000</range>
                <constant>0.9</constant>
                <linear>0.01</linear>
                <quadratic>0.001</quadratic>
            </attenuation>
            <direction>-0.5 0.1 -0.9</direction>
        </light>

        <model name="ground_plane">
            <static>true</static>
            <link name="link">
                <collision name="collision">
                    <geometry>
                        <plane>
                            <normal>0 0 1</normal>
                            <size>100 100</size>
                        </plane>
                    </geometry>
                </collision>
                <visual name="visual">
                    <geometry>
                        <plane>
                            <normal>0 0 1</normal>
                            <size>100 100</size>
                        </plane>
                    </geometry>
                    <material>
                        <ambient>0.8 0.8 0.8 1</ambient>
                        <diffuse>0.8 0.8 0.8 1</diffuse>
                        <specular>0.8 0.8 0.8 1</specular>
                    </material>
                </visual>
            </link>
        </model>

        <model name="box">
            <pose>0 0 0.5 0 0 0</pose>
            <link name="box_link">
                <inertial>
                    <inertia>
                        <ixx>0.16666</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.16666</iyy>
                        <iyz>0</iyz>
                        <izz>0.16666</izz>
                    </inertia>
                    <mass>1.0</mass>
                </inertial>
                <collision name="box_collision">
                    <geometry>
                        <box>
                            <size>1 1 1</size>
                        </box>
                    </geometry>
                </collision>

                <visual name="box_visual">
                    <geometry>
                        <box>
                            <size>1 1 1</size>
                        </box>
                    </geometry>
                    <material>
                        <ambient>1 0 0 1</ambient>
                        <diffuse>1 0 0 1</diffuse>
                        <specular>1 0 0 1</specular>
                    </material>
                </visual>
            </link>

            <plugin
                    filename="gz-sim-pose-publisher-system"
                    name="gz::sim::systems::PosePublisher">
                <use_pose_vector_msg>true</use_pose_vector_msg>
                <publish_link_pose>false</publish_link_pose>
                <publish_collision_pose>false</publish_collision_pose>
                <publish_visual_pose>false</publish_visual_pose>
                <publish_nested_model_pose>true</publish_nested_model_pose>
            </plugin>

        </model>

        <model name="cylinder">
            <pose>0 -1.5 0.5 0 0 0</pose>
            <link name="cylinder_link">
                <inertial>
                    <inertia>
                        <ixx>0.1458</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.1458</iyy>
                        <iyz>0</iyz>
                        <izz>0.125</izz>
                    </inertia>
                    <mass>1.0</mass>
                </inertial>
                <collision name="cylinder_collision">
                    <geometry>
                        <cylinder>
                            <radius>0.5</radius>
                            <length>1.0</length>
                        </cylinder>
                    </geometry>
                </collision>

                <visual name="cylinder_visual">
                    <geometry>
                        <cylinder>
                            <radius>0.5</radius>
                            <length>1.0</length>
                        </cylinder>
                    </geometry>
                    <material>
                        <ambient>0 1 0 1</ambient>
                        <diffuse>0 1 0 1</diffuse>
                        <specular>0 1 0 1</specular>
                    </material>
                </visual>
            </link>

            <plugin
                    filename="gz-sim-pose-publisher-system"
                    name="gz::sim::systems::PosePublisher">
                <use_pose_vector_msg>true</use_pose_vector_msg>
                <publish_link_pose>false</publish_link_pose>
                <publish_collision_pose>false</publish_collision_pose>
                <publish_visual_pose>false</publish_visual_pose>
                <publish_nested_model_pose>true</publish_nested_model_pose>
            </plugin>

        </model>

        <model name="sphere">
            <pose>0 1.5 0.5 0 0 0</pose>
            <link name="sphere_link">
                <inertial>
                    <inertia>
                        <ixx>0.1</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.1</iyy>
                        <iyz>0</iyz>
                        <izz>0.1</izz>
                    </inertia>
                    <mass>1.0</mass>
                </inertial>
                <collision name="sphere_collision">
                    <geometry>
                        <sphere>
                            <radius>0.5</radius>
                        </sphere>
                    </geometry>
                </collision>

                <visual name="sphere_visual">
                    <geometry>
                        <sphere>
                            <radius>0.5</radius>
                        </sphere>
                    </geometry>
                    <material>
                        <ambient>0 0 1 1</ambient>
                        <diffuse>0 0 1 1</diffuse>
                        <specular>0 0 1 1</specular>
                    </material>
                </visual>
            </link>

            <plugin
                    filename="gz-sim-pose-publisher-system"
                    name="gz::sim::systems::PosePublisher">
                <use_pose_vector_msg>true</use_pose_vector_msg>
                <publish_link_pose>false</publish_link_pose>
                <publish_collision_pose>false</publish_collision_pose>
                <publish_visual_pose>false</publish_visual_pose>
                <publish_nested_model_pose>true</publish_nested_model_pose>
            </plugin>

        </model>

        <model name="capsule">
            <pose>0 -3.0 0.5 0 0 0</pose>
            <link name="capsule_link">
                <inertial>
                    <inertia>
                        <ixx>0.074154</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.074154</iyy>
                        <iyz>0</iyz>
                        <izz>0.018769</izz>
                    </inertia>
                    <mass>1.0</mass>
                </inertial>
                <collision name="capsule_collision">
                    <geometry>
                        <capsule>
                            <radius>0.2</radius>
                            <length>0.6</length>
                        </capsule>
                    </geometry>
                </collision>
                <visual name="capsule_visual">
                    <geometry>
                        <capsule>
                            <radius>0.2</radius>
                            <length>0.6</length>
                        </capsule>
                    </geometry>
                    <material>
                        <ambient>1 1 0 1</ambient>
                        <diffuse>1 1 0 1</diffuse>
                        <specular>1 1 0 1</specular>
                    </material>
                </visual>
            </link>

            <plugin
                    filename="gz-sim-pose-publisher-system"
                    name="gz::sim::systems::PosePublisher">
                <use_pose_vector_msg>true</use_pose_vector_msg>
                <publish_link_pose>false</publish_link_pose>
                <publish_collision_pose>false</publish_collision_pose>
                <publish_visual_pose>false</publish_visual_pose>
                <publish_nested_model_pose>true</publish_nested_model_pose>
            </plugin>

        </model>

        <model name="ellipsoid">
            <pose>0 3.0 0.5 0 0 0</pose>
            <link name="ellipsoid_link">
                <inertial>
                    <inertia>
                        <ixx>0.068</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.058</iyy>
                        <iyz>0</iyz>
                        <izz>0.026</izz>
                    </inertia>
                    <mass>1.0</mass>
                </inertial>
                <collision name="ellipsoid_collision">
                    <geometry>
                        <ellipsoid>
                            <radii>0.2 0.3 0.5</radii>
                        </ellipsoid>
                    </geometry>
                </collision>
                <visual name="ellipsoid_visual">
                    <geometry>
                        <ellipsoid>
                            <radii>0.2 0.3 0.5</radii>
                        </ellipsoid>
                    </geometry>
                    <material>
                        <ambient>1 0 1 1</ambient>
                        <diffuse>1 0 1 1</diffuse>
                        <specular>1 0 1 1</specular>
                    </material>
                </visual>
            </link>

            <plugin
                    filename="gz-sim-pose-publisher-system"
                    name="gz::sim::systems::PosePublisher">
                <use_pose_vector_msg>true</use_pose_vector_msg>
                <publish_link_pose>false</publish_link_pose>
                <publish_collision_pose>false</publish_collision_pose>
                <publish_visual_pose>false</publish_visual_pose>
                <publish_nested_model_pose>true</publish_nested_model_pose>
            </plugin>

        </model>

        <model name="cone">
            <pose>0 4.5 0.5 0 0 0</pose>
            <link name="cone_link">
                <inertial auto="true">
                    <density>1</density>
                </inertial>
                <collision name="cone_collision">
                    <geometry>
                        <cone>
                            <radius>0.5</radius>
                            <length>1.0</length>
                        </cone>
                    </geometry>
                </collision>

                <visual name="cone_visual">
                    <geometry>
                        <cone>
                            <radius>0.5</radius>
                            <length>1.0</length>
                        </cone>
                    </geometry>
                    <material>
                        <ambient>1 0.47 0 1</ambient>
                        <diffuse>1 0.47 0 1</diffuse>
                        <specular>1 0.47 0 1</specular>
                    </material>
                </visual>
            </link>

            <plugin
                    filename="gz-sim-pose-publisher-system"
                    name="gz::sim::systems::PosePublisher">
                <use_pose_vector_msg>true</use_pose_vector_msg>
                <publish_link_pose>false</publish_link_pose>
                <publish_collision_pose>false</publish_collision_pose>
                <publish_visual_pose>false</publish_visual_pose>
                <publish_nested_model_pose>true</publish_nested_model_pose>
            </plugin>

        </model>

    </world>


</sdf>

We can open this scene in Gazebo with the following command.

gz sim $HOME/gazebo_tutorial_workspace/scenes/shapes_with_tf2_and_wrench.sdf

Using the following command will show the wrench-related topics.

gz topic -l | grep /wrench

It should result in the following output.

/world/shapes_with_tf2_and_wrench/wrench
/world/shapes_with_tf2_and_wrench/wrench/clear
/world/shapes_with_tf2_and_wrench/wrench/persistent

The message used in this topic can be found with the following command.

gz topic -i -t /world/shapes_with_tf2_and_wrench/wrench

It should result in the following output.

No publishers on topic [/world/shapes_with_tf2_and_wrench/wrench]
Subscribers [Address, Message Type]:
  tcp://172.16.191.128:37021, gz.msgs.EntityWrench

Using the following command will show the pose-related services.

gz service -l | grep set_pose

It should result in the following output.

/world/shapes_with_tf2_and_wrench/set_pose
/world/shapes_with_tf2_and_wrench/set_pose/blocking
/world/shapes_with_tf2_and_wrench/set_pose_vector
/world/shapes_with_tf2_and_wrench/set_pose_vector/blocking

The request and response of this service can be found with the following command.

gz service -i -s /world/shapes_with_tf2_and_wrench/set_pose

It should result in the following output.

Service providers [Address, Request Message Type, Response Message Type]:
  tcp://172.16.191.128:35577, gz.msgs.Pose, gz.msgs.Boolean

Objective#

Our objectives in this section will be as follows.

Send ROS2 messages to the [...]/wrench Gazebo topic. It uses gz.msgs.EntityWrench, therefore the pairing message is ros_gz_interfaces/msg/EntityWrench.
Use a ROS2 service to call the [...]/set_pose Gazebo service. The interface is not listed in the official list, but we will use ros_gz_interfaces/srv/SetEntityPose. See the official repository: gazebosim/ros_gz.

The pose messages will be processed by tf2.

Create the package#

We start by creating a package that depends on the interface packages mentioned above, namely ros_gz_interfaces and tf2_ros.

cd ~/ros2_tutorial_workspace/src
ros2 pkg create python_package_that_uses_gazebo \
--build-type ament_python \
--dependencies rclpy ros_gz_interfaces tf2_ros

Files#

We will be working on the following files.

python_package_that_uses_gazebo/
|-- config_bridge
|   `-- control_shape_thrust.yaml
|-- launch
|   |-- control_shape_thrust_launch.py
|   |-- send_poses_to_gazebo_launch.py
|   `-- send_wrenches_to_gazebo_launch.py
|-- package.xml
|-- python_package_that_uses_gazebo
|   |-- __init__.py
|   |-- control_shape_thrust_node.py
|   |-- send_poses_to_gazebo_node.py
|   `-- send_wrenches_to_gazebo_node.py
|-- resource
|   `-- python_package_that_uses_gazebo
|-- setup.cfg
|-- setup.py
`-- test
    |-- test_copyright.py
    |-- test_flake8.py
    `-- test_pep257.py

Sending poses to Gazebo#

Summary

We will make a node that does the following for us through ROS2.

gz service -s \
/world/shapes_with_tf2_and_wrench/set_pose \
--reqtype gz.msgs.Pose \
--reptype gz.msgs.Boolean \
--req 'name: "box", position: {x: 0.0, y: 0.0, z: 50}'

We’ll be able to send poses to Gazebo with the following node. It is a relatively simple node with a service client.

send_poses_to_gazebo_node.py

import time
from ros_gz_interfaces.srv import SetEntityPose

import rclpy
from rclpy.node import Node

class SendPosesToGazeboNode(Node):
    """A ROS2 Node that sends poses to Gazebo"""

    def __init__(self):
        super().__init__('send_poses_to_gazebo_node')

        self.service_client = self.create_client(
            srv_type=SetEntityPose,
            srv_name='/world/shapes_with_tf2_and_wrench/set_pose')

        while not self.service_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info(f'service {self.service_client.srv_name} not available, waiting...')


    def send_pose_to_gazebo(self):
        """Sample method sending a pose to and entity in Gazebo."""

        request = SetEntityPose.Request()

        # Add the entity name. For this one, Gazebo accepts the name. :)
        request.entity.name = "box"

        # Set the position
        request.pose.position.x = 0.0
        request.pose.position.y = 0.0
        request.pose.position.z = 50.0

        # Set the orientation
        request.pose.orientation.x = 0.0
        request.pose.orientation.y = 0.0
        request.pose.orientation.z = 0.0
        request.pose.orientation.w = 1.0

        return self.service_client.call_async(request)


def main(args=None):
    """
    The main function.
    :param args: Not used directly by the user, but used by ROS2 to configure certain aspects of the Node.
    """
    try:
        rclpy.init(args=args)

        node = SendPosesToGazeboNode()
        future = node.send_pose_to_gazebo()
        rclpy.spin_until_future_complete(node, future)

    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(e)



if __name__ == '__main__':
    main()

There are perhaps only two unfamiliar aspects of this node by now. First, that we need to send the correct entity name for Gazebo to know which entity to set the pose. This name can be obtained in Gazebo`s GUI.

    def send_pose_to_gazebo(self):
        """Sample method sending a pose to and entity in Gazebo."""

        request = SetEntityPose.Request()

        # Add the entity name. For this one, Gazebo accepts the name. :)
        request.entity.name = "box"

        # Set the position
        request.pose.position.x = 0.0
        request.pose.position.y = 0.0
        request.pose.position.z = 50.0

        # Set the orientation
        request.pose.orientation.x = 0.0
        request.pose.orientation.y = 0.0
        request.pose.orientation.z = 0.0
        request.pose.orientation.w = 1.0

        return self.service_client.call_async(request)

The second possibly unfamiliar is the choice of rclpy.spin_until_future_complete to illustrate calling the service client only once.

def main(args=None):
    """
    The main function.
    :param args: Not used directly by the user, but used by ROS2 to configure certain aspects of the Node.
    """
    try:
        rclpy.init(args=args)

        node = SendPosesToGazeboNode()
        future = node.send_pose_to_gazebo()
        rclpy.spin_until_future_complete(node, future)

    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(e)

Exercises

How would you approach this node if you had to, for instance.

Accept the entity information as a parameter for the node.
Accept the pose as a parameter from the node.
Set the pose of multiple entities.

The launch file#

This node will not work without a pairing parameter_bridge. We add the following launch file to the launch folder.

send_poses_to_gazebo_launch.py

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    
    node = Node(
            output='screen',
            emulate_tty=True,
            package='python_package_that_uses_gazebo',
            executable='send_poses_to_gazebo_node',
            name='send_poses_to_gazebo_node'
        )

    bridge = Node(
        package='ros_gz_bridge',
        executable='parameter_bridge',
        arguments=['/world/shapes_with_tf2_and_wrench/set_pose@ros_gz_interfaces/srv/SetEntityPose'],
        output='screen'
    )
    
    return LaunchDescription([
        node,
        bridge
    ])

Sending wrenches to Gazebo#

Summary

We will make a node that does the following for us through ROS2.

gz topic -t \
/world/shapes_with_tf2_and_wrench/wrench \
-m gz.msgs.EntityWrench \
-p  'entity: {id: 9}, wrench: {force: {x: 1000.0, y: 0.0, z: 0.0}, torque: {x: 0.0, y: 0.0, z: 0.0}}'

We’ll be able to send poses to Gazebo with the following node. It is a relatively simple node with a publisher.

send_wrenches_to_gazebo_node.py

import time
from ros_gz_interfaces.msg import EntityWrench

import rclpy
from rclpy.node import Node

class SendWrenchesToGazeboNode(Node):
    """A ROS2 Node that sends wrenches to Gazebo."""

    def __init__(self):
        super().__init__('send_wrenches_to_gazebo_node')

        self.publisher = self.create_publisher(
            msg_type=EntityWrench,
            topic='/world/shapes_with_tf2_and_wrench/wrench',
            qos_profile=1)

        while not self.count_subscribers('/world/shapes_with_tf2_and_wrench/wrench'):
            print(f"Waiting for subscriber to be connected...")
            time.sleep(1)

    def send_wrench_to_gazebo(self):
        """Basic method publishing wrenches to Gazebo."""

        ew = EntityWrench()

        # Add the entity id. The entity.name is currently ignored by Gazebo.
        ew.entity.id = 9

        # Set the force
        ew.wrench.force.x = 1000.0
        ew.wrench.force.y = 0.0
        ew.wrench.force.z = 0.0

        # Set the torque
        ew.wrench.torque.x = 0.0
        ew.wrench.torque.y = 0.0
        ew.wrench.torque.z = 0.0

        self.get_logger().info(f"This sent entity {ew.entity.name} a wrench with force:"
                               f" {ew.wrench.force} "
                               f"and torque:"
                               f" {ew.wrench.torque}.")


        self.publisher.publish(ew)


def main(args=None):
    """
    The main function.
    :param args: Not used directly by the user, but used by ROS2 to configure certain aspects of the Node.
    """
    try:
        rclpy.init(args=args)

        node = SendWrenchesToGazeboNode()
        node.send_wrench_to_gazebo()
        rclpy.spin(node)

    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(e)



if __name__ == '__main__':
    main()

The only unfamiliar aspect is shown below. The id of the Gazebo entity must be sent. You can obtain that information from Gazebo's GUI. Note that here, trying to send a name instead is inconsequential and Gazebo won’t recognise it.

    def send_wrench_to_gazebo(self):
        """Basic method publishing wrenches to Gazebo."""

        ew = EntityWrench()

        # Add the entity id. The entity.name is currently ignored by Gazebo.
        ew.entity.id = 9

        # Set the force
        ew.wrench.force.x = 1000.0
        ew.wrench.force.y = 0.0
        ew.wrench.force.z = 0.0

        # Set the torque
        ew.wrench.torque.x = 0.0
        ew.wrench.torque.y = 0.0
        ew.wrench.torque.z = 0.0

        self.get_logger().info(f"This sent entity {ew.entity.name} a wrench with force:"
                               f" {ew.wrench.force} "
                               f"and torque:"
                               f" {ew.wrench.torque}.")


        self.publisher.publish(ew)

Wait… what?

Yes, for wrenches, we need the id. For the pose, we use the name. Such is life.

Exercises

How would you approach this node if you had to, for instance.

Accept the entity information as a parameter for the node.
Accept the wrench as a parameter from the node.
Publish the wrench of multiple entities.
Define the wrench based on other information?
- The next section shows one example of it.

The launch file#

This node will not work without a pairing parameter_bridge. We add the following launch file to the launch folder.

send_wrenches_to_gazebo_launch.py

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    
    node = Node(
            output='screen',
            emulate_tty=True,
            package='python_package_that_uses_gazebo',
            executable='send_wrenches_to_gazebo_node',
            name='send_wrenches_to_gazebo_node'
        )

    bridge = Node(
        package='ros_gz_bridge',
        executable='parameter_bridge',
        arguments=['/world/shapes_with_tf2_and_wrench/wrench@ros_gz_interfaces/msg/EntityWrench]gz.msgs.EntityWrench'],
        output='screen'
    )
    
    return LaunchDescription([
        node,
        bridge
    ])

Controlling thrust of shapes#

Lastly, we can have a slightly more complex example. In this example, we read the poses through tf2. Then, using that pose information, we publish a wrench. This way, we can attempt to move a shape to a given place in the scene.

This example highlights one possible way of interacting with Gazebo.

control_shape_thrust_node.py

import time
from ros_gz_interfaces.msg import EntityWrench

import rclpy
from rclpy.node import Node

import tf2_ros

class ControlShapeThrustNode(Node):
    """A ROS2 Node that controls the thrust of a shape in Gazebo.
       It will apply force based on the position of an entity."""

    def __init__(self):
        super().__init__('control_shape_thurst_node')

        # Structured easily to be changed into configurable parameters
        self._gazebo_world_name = "shapes_with_tf2_and_wrench"
        self._wrench_topic = f'/world/{self._gazebo_world_name}/wrench'
        self._gazebo_entity_name = "box" # Find this in Gazebo
        self._gazebo_entity_id = 9 # Find this in Gazebo

        # Set up the wrench publisher
        self.wrench_publisher = self.create_publisher(
            msg_type=EntityWrench,
            topic=self._wrench_topic,
            qos_profile=1)

        # This is one way to prevent published messages from being lost, but we don't know what is subscribing.
        while not self.count_subscribers(self._wrench_topic):
            print(f"Waiting for subscriber to be connected to {self._wrench_topic}...")
            time.sleep(1)

        # Setting up the TransformListener.
        self.transform_listener_buffer = tf2_ros.Buffer()
        self.transform_listener = tf2_ros.TransformListener(self.transform_listener_buffer, self)

        # Information about the transform we want to listen to
        self.parent_name = self._gazebo_world_name # It will be populated this way by Gazebo
        self.child_name = self._gazebo_entity_name # It will be populated this way by Gazebo

        self.timer_period: float = 0.001
        self.timer = self.create_timer(self.timer_period, self.timer_callback)

    def compute_control_action(self, current, target, proportional_gain:float = 5.0) -> float:
        """A simple proportional controller"""
        error = current - target
        return -proportional_gain * error

    def send_force_to_gazebo(self, force: tuple[float, float, float] = (0, 0, 0)) -> None:
        """Basic method to send force to an entity."""

        ew = EntityWrench()

        # Add the entity id. The entity.name is currently ignored by Gazebo.
        ew.entity.id = self._gazebo_entity_id

        # Set the force
        ew.wrench.force.x = force[0]
        ew.wrench.force.y = force[1]
        ew.wrench.force.z = force[2]

        # Set the torque
        ew.wrench.torque.x = 0.0
        ew.wrench.torque.y = 0.0
        ew.wrench.torque.z = 0.0

        self.wrench_publisher.publish(ew)

    def timer_callback(self):
        """The timer callback will look up the transforms and send the next control action."""

        try:
            tfs =   self.transform_listener_buffer.lookup_transform(
                    self.parent_name,
                    self.child_name,
                    rclpy.time.Time())

            # A simple proportional controller for the x-axis force
            u = self.compute_control_action(tfs.transform.translation.x, -3.0)

            self.send_force_to_gazebo(
                (u,
                 0.0,
                 0.0)
            )

        except tf2_ros.TransformException as e:

            self.get_logger().warn(
                f'Could not get transform from `{self.parent_name}` to `{self.child_name}`: {e}')


def main(args=None):
    """
    The main function.
    :param args: Not used directly by the user, but used by ROS2 to configure certain aspects of the Node.
    """
    try:
        rclpy.init(args=args)

        node = ControlShapeThrustNode()
        rclpy.spin(node)

    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(e)



if __name__ == '__main__':
    main()

We start by having multiple hard-coded values to configure the node. This is illustrative and because of how this is organised it would be easier to turn them into configurable parameters.

As you have seen in previous examples, _gazebo_world_name is used to define the topic name for the wrench and also the parent reference frame for tf2. The _gazebo_entity_name is used to get the child frame for tf2. Lastly, _gazebo_entity_id is used to send the wrench to the correct entity.

        # Structured easily to be changed into configurable parameters
        self._gazebo_world_name = "shapes_with_tf2_and_wrench"
        self._wrench_topic = f'/world/{self._gazebo_world_name}/wrench'
        self._gazebo_entity_name = "box" # Find this in Gazebo
        self._gazebo_entity_id = 9 # Find this in Gazebo

There is also a simple proportional controller in one dimension.

    def compute_control_action(self, current, target, proportional_gain:float = 5.0) -> float:
        """A simple proportional controller"""
        error = current - target
        return -proportional_gain * error

Lastly, in the timer_callback, we use the current coordinate value to compute the control action and send it to Gazebo.

    def timer_callback(self):
        """The timer callback will look up the transforms and send the next control action."""

        try:
            tfs =   self.transform_listener_buffer.lookup_transform(
                    self.parent_name,
                    self.child_name,
                    rclpy.time.Time())

            # A simple proportional controller for the x-axis force
            u = self.compute_control_action(tfs.transform.translation.x, -3.0)

            self.send_force_to_gazebo(
                (u,
                 0.0,
                 0.0)
            )

        except tf2_ros.TransformException as e:

            self.get_logger().warn(
                f'Could not get transform from `{self.parent_name}` to `{self.child_name}`: {e}')

Exercises

How would you approach this node if you had to, for instance.

Accept relevant information as a parameter for the node.
Control the shape via thrusts in the 2D plane.
Add derivative and integral control actions.
Control the shape in 3D.
- How would the gravity be counterbalanced?

The bridge file#

The bridge file, added to the folder config_bridge, will manage bridging the topics with tf2 and the wrench.

control_shape_thrust.yaml

- ros_topic_name: "/clock"
  gz_topic_name: "/clock"
  ros_type_name: "rosgraph_msgs/msg/Clock"
  gz_type_name: "gz.msgs.Clock"
  direction: GZ_TO_ROS

- ros_topic_name: "/tf"
  gz_topic_name: "/model/box/pose"
  ros_type_name: "tf2_msgs/msg/TFMessage"
  gz_type_name: "gz.msgs.Pose_V"
  direction: GZ_TO_ROS

- ros_topic_name: "/world/shapes_with_tf2_and_wrench/wrench"
  gz_topic_name: "/world/shapes_with_tf2_and_wrench/wrench"
  ros_type_name: "ros_gz_interfaces/msg/EntityWrench"
  gz_type_name: "gz.msgs.EntityWrench"
  direction: ROS_TO_GZ

The launch file#

This node will not work without a pairing parameter_bridge. We add the following launch file to the launch folder.

control_shape_thrust_launch.py

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    
    this_package_share_directory = get_package_share_directory('python_package_that_uses_gazebo')

    node = Node(
            output='screen',
            emulate_tty=True,
            package='python_package_that_uses_gazebo',
            executable='control_shape_thrust_node',
            name='control_shape_thrust_node'
        )

    bridge = Node(
        package='ros_gz_bridge',
        executable='parameter_bridge',
        parameters=[{
            'config_file': os.path.join(this_package_share_directory, 'config_bridge', 'control_shape_thrust.yaml')
        }],
        output='screen'
    )
    
    return LaunchDescription([
        node,
        bridge
    ])

Adjusting the `setup.py`#

This file will include the directives for all the three nodes, added in entry_points. We also have on L15 the directive for the launch files. Lastly, on L16, we have a directive to install the bridge configuration file which is used by control_shape_thrust_launch.py.

setup.py

import os
from glob import glob
from setuptools import find_packages, setup

package_name = 'python_package_that_uses_gazebo'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
        (os.path.join('share', package_name, 'config_bridge'), glob(os.path.join('config_bridge', '*.yaml')))
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='root',
    maintainer_email='murilo.marinho@manchester.ac.uk',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            "send_wrenches_to_gazebo_node = python_package_that_uses_gazebo.send_wrenches_to_gazebo_node:main",
            "send_poses_to_gazebo_node = python_package_that_uses_gazebo.send_poses_to_gazebo_node:main",
            "control_shape_thrust_node = python_package_that_uses_gazebo.control_shape_thrust_node:main"
        ],
    },
)

Build and source#

Before we proceed, let us build and source once.

cd ~/ros2_tutorial_workspace
colcon build
source install/setup.bash

Note

For additional explanation and troubleshooting tips, see Always source after you build.

Warning

colcon will not work properly if your terminal has an active venv.

Testing#

We first open and run the simulation.

gz sim $HOME/gazebo_tutorial_workspace/scenes/shapes_with_tf2_and_wrench.sdf

It is possible to run each of these examples in this particular order and see their effects.

We can run the first example with the following command. Then, stop it with CTRL+C before moving to the following one.

ros2 launch python_package_that_uses_gazebo send_poses_to_gazebo_launch.py

We can run the second example with the following command. Then, stop it with CTRL+C before moving to the following one.

ros2 launch python_package_that_uses_gazebo send_wrenches_to_gazebo_launch.py

We can run the third example with the following command. Then, stop it with CTRL+C.

ros2 launch python_package_that_uses_gazebo control_shape_thrust_launch.py

This is illustrated in the video below.

Interface Gazebo with custom ROS2 nodes

Contents

Interface Gazebo with custom ROS2 nodes#

Setting up the scene#

Objective#

Create the package#

Files#

Sending poses to Gazebo#

The launch file#

Sending wrenches to Gazebo#

The launch file#

Controlling thrust of shapes#

The bridge file#

The launch file#

Adjusting the setup.py#

Build and source#

Testing#

Adjusting the `setup.py`#