gazebo spawn urdf

GAZEBO. Cómo añadir un modelo URDF a una simulación

1. Introducción

En este artículo vas a aprender cómo hacer aparecer (spawn) un modelo URDF en GAZEBO.

Básicamente hay dos formas de hacerlo:

      • Usando el servicio /spawn_entity del paquete gazebo_msgs.
      • Usando el nodo spawn_entity.py  del paquete gazebo_ros.

En los siguientes apartados vamos a ver como añadir a nuestra simulación un modelo URDF muy simple, consistente únicamente en una esfera, empleando ambos métodos.

2 Mostrar el modelo URDF usando el servicio /spawn_entity

Este servicio, que forma parte del paquete gazebo_msgs, permite mostrar una entidad en Gazebo. La entidad no tiene por qué ser necesariamente el modelo de un objeto o un robot, pudiendo ser otros elementos de Gazebo, por ejemplo una fuente de luz.

En la siguiente imagen se muestra la información que hay que pasarle al servicio y cómo es su respuesta. 

gazebo spawn_entity service

2.1 Desde la línea de comando

Desde el terminal podemos llamar al servicio /spawn_entity mediante el comando ros2 service call.

Para ello primero lanzamos Gazebo si no lo hemos hecho:

ros2 launch gazebo_ros gazebo.launch.py

Y a continuación llamamos al servicio:

ros2 service call /spawn_entity 'gazebo_msgs/SpawnEntity' '{name: "urdf_ball", xml: "<?xml version=\"1.0\" ?><robot name=\"will_be_ignored\"><link name=\"link\"><visual><geometry><sphere radius=\"0.5\"/></geometry></visual><collision><geometry><sphere radius=\"0.5\"/></geometry></collision><inertial><mass value=\"1\"/><inertia ixx=\"1\" ixy=\"0.0\" ixz=\"0.0\" iyy=\"1\" iyz=\"0.0\" izz=\"1\"/></inertial></link></robot>", robot_namespace: "ball_namespace", initial_pose: {position: {x: 0.0, y: 0.0, z: 0.5}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}}, reference_frame: "world"}'

2.2 Desde un nodo ROS2

El siguiente código muestra un ejemplo de cómo llamar al servicio /spawn_entity desde un nodo.

from gazebo_msgs.srv import SpawnEntity
import rclpy
from rclpy.node import Node


class ServiceClientAsync(Node):

    def __init__(self):
        super().__init__('service_client_async')
        self.client = self.create_client(SpawnEntity, 'spawn_entity')
        while not self.client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')


    def send_request(self, request_data):
        self.future = self.client.call_async(request_data)
        rclpy.spin_until_future_complete(self, self.future)
        return self.future.result()


def main(args=None):
    rclpy.init(args=args)

    request = SpawnEntity.Request()
    request.name = "urdf_ball"
    request.xml = "<?xml version=\"1.0\" ?><robot name=\"will_be_ignored\"><link name=\"link\"><visual><geometry><sphere radius=\"0.5\"/></geometry></visual><collision><geometry><sphere radius=\"0.5\"/></geometry></collision><inertial><mass value=\"1\"/><inertia ixx=\"1\" ixy=\"0.0\" ixz=\"0.0\" iyy=\"1\" iyz=\"0.0\" izz=\"1\"/></inertial></link></robot>"
    request.robot_namespace = "ball_namespace"
    request.reference_frame = "world"
    request.initial_pose.position.x = 0.0
    request.initial_pose.position.y = 0.0
    request.initial_pose.position.z = 0.0
    request.initial_pose.orientation.x = 0.0
    request.initial_pose.orientation.y = 0.0
    request.initial_pose.orientation.z = 0.0
    request.initial_pose.orientation.w = 0.0

    service_client = ServiceClientAsync()
    response = service_client.send_request(request)

    service_client.get_logger().info(
        'Result of spawing service: %s , %s' %
        (response.success, response.status_message))

    service_client.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

3 Mostrar el modelo URDF usando el nodo spawn_entity

Como ya hemos indicado, otra forma de añadir nuestro modelo URDF a una simulación de Gazebo es mediante el nodo spawn_entity.py que forma parte del paquete gazebo_ros.

Este nodo admite los argumentos que se indican en la siguiente tabla los cuales, entre otras cosas, permiten definir el origen del código XML que define nuestro modelo (admite tanto URDF como SDF) y su pose inicial.

Gazebo argumentos de spawn_entity
Argumentos admitidos por el nodo spawn_entity.py

3.1 Desde la línea de comando

A continuación se muestran algunos ejemplos de cómo añadir un modelo a la simulación usando el nodo spawn_entity.py  con diferentes orígenes desde donde tomar la descripción XML del mismo.

Cargando los datos del modelo desde un archivo
ros2 run gazebo_ros spawn_entity.py -entity myentity -x 0 -y 0 -z 0 -R 0 -P 0 -Y 0 -file /path/to/entity/file
Cargando los datos del modelo usando la entrada stdin del terminal.
cat /path/to/entity/file | ros2 run gazebo_ros spawn_entity.py -entity new_model -x 0 -y 0 -z 0 -stdin
Cargando los datos del modelo desde la base de datos de Gazebo.
ros2 run gazebo_ros spawn_entity.py -entity TheArm -x 0 -y 0 -z 0 -R 0 -P 0 -Y 0 -database simple_arm
Cargando desde un topic los datos del modelo.
ros2 run gazebo_ros spawn_entity.py -entity a_ball -topic /robot_description
ros2 topic pub -1 --qos-durability transient_local /robot_description 'std_msgs/String' '{data: "<?xml version=\"1.0\" ?><robot name=\"will_be_ignored\"><link name=\"link\"><visual><geometry><sphere radius=\"0.5\"/></geometry></visual><collision><geometry><sphere radius=\"0.5\"/></geometry></collision><inertial><mass value=\"1\"/><inertia ixx=\"1\" ixy=\"0.0\" ixz=\"0.0\" iyy=\"1\" iyz=\"0.0\" izz=\"1\"/></inertial></link></robot>"}'

3.2 Desde un archivo launch en ROS2

A continuación se muestra un ejemplo de código a incluir en un archivo launch para mostrar un modelo URDF en Gazebo, el cual realiza las siguientes acciones:

      1. Obtiene la ruta a nuestro archivo con la información URDF de nuestro modelo.
      2. Crea un objeto launch.ros_action.node para ejecutar robot_state publisher al cual le pasamos mediante  su  parámetro robot_description  el  código  URDF, que obtenemos  ejecutando “xacro < urdf_file >” mediante el uso de Command. El nodo robot_state_publisher se encarga de resolver la cinemática directa del modelo.
      3. Crea un objeto launch.ros_action.node para ejecutar spawn_entity.py, que forma parte del paquete gazebo_ros. Este script lee el código URDF que publica el nodo robot_state_publisher y lo muestra en Gazebo.
      4. Crea un objeto IncludeLaunchDescription para incluir la ejecución del archivo launch gazebo.launch.py, del paquete gazebo_ros, que lanza la ejecución de Gazebo.
import xacro, os
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node
from ament_index_python import get_package_prefix, get_package_share_directory


def generate_launch_description():

    # Establece los nombres de nuestro paquete y del archivo con el modelo.
    package_name = 'my_package'
    urdf_file_name = 'my_model.urdf'


    # DESCOMENTAR si el codigo URDF utiliza archivos mesh donde la ruta al
    # archivo de malla es relativa, indicandose de la forma: 
    # <mesh filename ="package://mi_paquete/..../archivo_mesh" />
    # --------------------------------------------------------------------------
    #gazebo_model_path = os.path.join(get_package_prefix(package_name), 'share')


    # DESCOMENTAR si el codigo URDF utiliza archivos mesh donde la ruta al
    # archivo de malla es relativa a la ubicación del archivo setup.py, 
    # indicandose de la forma: <mesh filename ="package://..../archivo_mesh" />
    # --------------------------------------------------------------------------
    #gazebo_model_path = get_package_share_directory(package_name)


    # DESCOMENTAR si el codigo URDF utiliza archivos mesh donde la ruta al
    # archivo de malla se indica de forma relativa.
    # --------------------------------------------------------------------------
    #if 'GAZEBO_MODEL_PATH' in os.environ:
    #    os.environ['GAZEBO_MODEL_PATH'] += os.pathsep + gazebo_model_path
    #else:
    #    os.environ['GAZEBO_MODEL_PATH'] =  gazebo_model_path


    # Obtiene la ruta al archivo que contiene la información de nuestro modelo.
    urdf_file_path = os.path.join(
        get_package_share_directory(package_name), 'urdf', urdf_file_name)


    # Crea y configura el nodo ROBOT_STATE_PUBLISHER, el cual lee el código URDF 
    # del modelo y resuelve la cinemática directa del robot a partir de la 
    # información que proporciona el nodo JOINT_STATE_PUBLISHER_GUI. 
    # Utiliza Command para ejecutar xacro y obtener el código URDF.
    node_robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        parameters=[{'robot_description': Command(['xacro ', urdf_file_path])}],
        arguments=['--ros-args', '--log-level', 'error'],
        output='screen')
    

    # Crea y configura el nodo SPAWN_ENTITY, que es el que va a cargar y mostrar
    # el modelo en Gazebo.
    node_spawn_entity = Node(
        package='gazebo_ros', 
        executable='spawn_entity.py',
        arguments=[
            '-entity', urdf_file_name,
            '-topic', 'robot_description',
            '-x', '0.0',
            '-y', '0.0',
            '-z', '0.0',
            '-R', '0.0',
            '-P', '0.0',
            '-Y', '0.0'],
        output='screen')


    # Incluye la ejecución del archivo launch gazebo.launch.py, que forma parte
    # del paquete gazebo_ros.
    gazebo = IncludeLaunchDescription(
                PythonLaunchDescriptionSource([PathJoinSubstitution(
                    [FindPackageShare('gazebo_ros'), 'launch', 'gazebo.launch.py'])]),
             )


    # Devuelve un objeto LaunchDescription con las acciones a realizar.
    return LaunchDescription([
        gazebo,
        node_robot_state_publisher,
        node_spawn_entity,
    ])

4 Ejemplo

Puedes descargar el paquete py_gazebo_spawn_example de Github que contiene un ejemplo de cómo hacer aparecer un modelo en nuestra simulación en Gazebo usando un archivo launch o usando el servicio spawn_entity desde un nodo de ROS2.

Para ello, en el terminal, ve a la carpeta src de tu espacio de trabajo y ejecuta la siguiente instrucción:

git clone https://github.com/joseecm-github/py_gazebo_spawn_example

Y a continuación compila el paquete:

cd ..
colcon build --packages-select py_gazebo_spawn_example
. install/setup.bash

Para hacer aparecer el modelo usando un archivo launch ejecuta el siguiente comando:

ros2 launch py_gazebo_spawn_example spawn.launch.py

Para ver como funciona usando el servicio primero cerramos Gazebo pulsando en el terminal CTRL+C y lo volvemos a arrancar ejecutando:

ros2 launch gazebo_ros gazebo.launch.py

Y desde otro terminal lanzamos el nodo que llama al servicio:

ros2 run py_gazebo_spawn_example spawn_urdf
Comparte este artículo:

Deja un comentario

Tu dirección de correo electrónico no será publicada. Los campos obligatorios están marcados con *