Services and Communication between ROS2 and Gazebo

This tutorial is based on ROS2 Humble and Gazebo Fortress on Ubuntu and the code is implemented in C++.

Introduction

In this tutorial, we will guild you through the processe of seeting up a service node in ROS2. But we're adding a twist! Our service will bridge ROS2 and Gazebo. Specifically, you'll learn to implement an entity management service, which aloows you to dynamcially add entities to the simulation.

Related Readings

  1. Writing a simple service and client in C++

  2. Creating custom msg and srv files

  3. Implementing custom interface

  4. ros_gz_bridge Github repository

The first document offers guidelines for developing a simple service in C++. In the second document, the focus shifts to the creation of custom msg and srv files, which are designed for usage by external packages. The third document details the setup process for a project to enable the same-package usage of the custom msg and srv files. Lastly, the fourth link features code that facilitates the conversion of built-in ROS types to Gazebo formats and vice versa.

What does this tutorial cover?

  • Implementation of a simple service in C++

  • Send a request to Gazebo from a ROS node in the code

  • An example of message type conversion

  • An example of python launch file which lanches both the ROS2 service node and Gazebo

  • An example of Gazebo world and model file

What is outside the scope of this tutorial?

  • The tutorial does not provide a Python implementation of the service

  • Service client implementation is not provided

  • Detailed explanation of the launch file is not provided

  • Detailed explanation of SDF file is not provided

Objective and results

The goal is to develop a ROS2 service that facilitates the process of introducing a new entity into Gazebo. Users can define specific details such as the entity's name, model, and initial position in their request and send it to the ROS2 service. this request is then forwarded to Gazebo. The figure below illustrates the end outcome.

Step 1: Create custom service file

The first step is to create a custom service file. The service request type is a direct translation of the use case.

string name
string model_filepath
geometry_msgs/Point location
float64 theta
---
bool result

This file can be found in ros2-service-demo/src/entity_management/src/entity_management_service.cpp file.

Additional dependencies are needed to generate the implementation of the service request data type. Add the following code to the ros2-service-demo/src/entity_management/CMakeLists.txt file:

rosidl_generate_interfaces(${PROJECT_NAME}
        "srv/AddEntity.srv"
        DEPENDENCIES geometry_msgs
)

ament_export_dependencies(rosidl_default_runtime)
rosidl_get_typesupport_target(cpp_typesupport_target
        ${PROJECT_NAME} rosidl_typesupport_cpp)

The configuration above enables the build-system to create code for the service request data type, which can be used outside the entity_management package. However, to make this code accessible within the entity_management package itself, we need to link additional libraries to the target:

target_link_libraries(entity_management_service
        PUBLIC
        "${cpp_typesupport_target}")

We also need to add the following code to the ros2-service-demo/src/entity_management/package.xml file:

  <depend>geometry_msgs</depend>
  <buildtool_depend>rosidl_default_generators</buildtool_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

Build the package and the service request data type should become available.

Step 2: Implement a service

The service implementation has a simple structure as presented below. One thing worth noting is the name of the header file. The srv file we created is called AddEntity.srv and the header file becomes add_entity.hpp. However, when we access the Request and Response type, the namespace uses srv::AddEntity. This naming convention may cause confusion.

The structure of the service implementation, as shown below, is straightforward. An important detail to note is the naming convention of the header file. For instance, our srv file is named AddEntity.srv, but the corresponding header file is named add_entity.hpp. When referring to the Request and Response types, the namespace used is srv::AddEntity. This difference in naming conventions between the file names and the namespace usage is a subtle point that might lead to some confusion. It's essential to be aware of these distinctions to ensure proper referencing and usage in the service implementation.

#include "entity_management/srv/add_entity.hpp"

namespace entity_management_service {
    std::string SERVICE_NODE_NAME = "add_entity_service_node";
    std::string SERVICE_NAME = "add_entity";

    class EntityManagementService : public rclcpp::Node {
    private:
        void add_entity(const std::shared_ptr<entity_management::srv::AddEntity::Request> request,
                               std::shared_ptr<entity_management::srv::AddEntity::Response> response) {

        }

    public:
        EntityManagementService(std::string name): rclcpp::Node(name) {
            this->service = this->create_service<entity_management::srv::AddEntity>(
                    SERVICE_NAME,
                std::bind(&EntityManagementService::add_entity, this, std::placeholders::_1, std::placeholders::_2));
        }
    };
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<entity_management_service::EntityManagementService>(
            entity_management_service::SERVICE_NODE_NAME);
    rclcpp::spin(node);
    rclcpp::shutdown();

}

The next step is to implement the add_entity function, which executes the following two tasks:

  • Convert entity_management::srv::AddEntity::Request into a Gazbo type

  • Send the request to the Gazebo service.

The Gazebo service responsible for adding new models is identified as /world/demo/create. The naming of this service is directly dependent on the world name specified in the world SDF file. To create a new entity, the Gazebo service uses the type gz::msgs::EntityFactory. Consequently, the add_entity function is structured as follows, with the section pertaining to the conversion process omitted for brevity:

void add_entity(const std::shared_ptr<entity_management::srv::AddEntity::Request> request,
                       std::shared_ptr<entity_management::srv::AddEntity::Response> response) {

    gz::msgs::EntityFactory gz_req;
    // Convert request to gz_req
    // ...
    ignition::msgs::Boolean reply;
    bool call_succeeded;
    this->gz_node->Request("/world/demo/create", gz_req, 5000, reply, call_succeeded);
    response->result = call_succeeded && reply.data();
}

Step 3: Prepare world and model files

In this tutorial, we use ros2-service-demo/src/bringup/worlds/empty_world.sdf as the world file and ros2-service-demo/src/bringup/models/diff_drive/model.sdf for the model file.

The name of the world file is specified in the launch file, while the name of the model file is referenced in the terminal command when calling the add_entity service. It's important to ensure that these files are correctly installed during the build process. If they are not properly set up, ROS2 may have difficulties in locating them.

Step 4: Prepare the launch file

We need two nodes in the launch file. One for the add_entity service and the othe for the Gazebo process.

# ----- Launch Entity Management Service Node -----
entity_management_service_node = Node(
    package="entity_management",
    executable="entity_management_service",
    parameters=[{"use_sim_time": False}],
)

# ----- Launch Gazebo -----
# Locate Gazebo executable. It's provided in the ros_gz_sim package.
ros_gz_sim_package = get_package_share_directory("ros_gz_sim")

gazebo = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(path.join(ros_gz_sim_package, 'launch', 'gz_sim.launch.py')),
    launch_arguments={'gz_args': PathJoinSubstitution([
        bringup_package,
        'worlds',
        "empty_world.sdf",
    ])}.items(),
)

Step 5: Launch the system

Before launch the system, make sure no other ROS2 and Gazebo processing are running on your machine. You can checkout this article to properly terminate the ROS2 and Gazego processes.

To launch the system, use the command below:

# Open a new terminal and go to the workspace directory ros2-service-demo
# Run . install/setup.bash before launching the system.
ros2 launch bringup entity_management_server.launch.py

You should see the ROS2 starts running and Gazebo is open. Now, you could add some robots to the simulation using the following commands:

# Open a new terminal and go to the workspace directory ros2-service-demo
# Run . install/setup.bash before executing the following commands.

ros2 service call /add_entity entity_management/srv/AddEntity "{name: 'model-0', model_filepath: 'diff_drive/model.sdf', location: {x: 10, y: 0, z: 0}, theta: 0}"

ros2 service call /add_entity entity_management/srv/AddEntity "{name: 'model-90', model_filepath: 'diff_drive/model.sdf', location: {x: 0, y: 10, z: 0}, theta: 1.57}"

ros2 service call /add_entity entity_management/srv/AddEntity "{name: 'model-180', model_filepath: 'diff_drive/model.sdf', location: {x: -10, y: 0, z: 0}, theta: 3.14}"

ros2 service call /add_entity entity_management/srv/AddEntity "{name: 'model-270', model_filepath: 'diff_drive/model.sdf', location: {x: 0, y: -10, z: 0}, theta: -1.57}"

Conclusion

In this tutorial, we've implement a simple ROS2 service in C++ that forwards user request to Gazego to add new entities to the simulation. We've highlighted the structure of the add_entity function, which converts the user request to a Gazebo type gz::msgs::EntityFactory and then send it to Gazebo. We have also demonstrated how to initiate the ROS2 service call from the terminal.

Download Code

The code is available for download at this link.

©2023 - 2024 all rights reserved

PreviousTutorialsNextSubscription and Message Filters Demo

Last updated