Manejando Threads Para Crear un Action Server – Spanish ROS Tutorial

Written by Jose Manuel Pacheco Garriazo



Spanish ROS Tutorials



07/05/2024

This tutorial is created by Robotics Ambassador 017 Jose

Rosbotics Ambassador Program (https://www.theconstruct.ai/robotics-ambassador/)

Lo que vamos a aprender

Usar múltiples threads en un nodo.
Crear callbacks groups para el manejo de callbacks.
Emplear un action server para comandar el Turtlebot3.

Lista de recursos usados en esta publicación

Usa este rosject: https://app.theconstruct.ai/l/615821d8/
The Construct: https://app.theconstructsim.com/
Cursos ROS: ROS2 Basics in 5 Days (Python): https://app.theconstructsim.com/courses/132
Documentación ROS Callback Groups: https://docs.ros.org/en/foxy/How-To-Guides/Using-callback-groups.html
Documentación ROS Executors: https://docs.ros.org/en/foxy/Concepts/About-Executors.html

Resumen

ROS (Robot Operating System) se está convirtiendo en el “framework” estándar para programar robots. En este tutorial se hará uso de trheads y grupos de callbacks para poder crear un Action Server para el TurtleBot3.Todo esto es simulado en un mundo de Gazebo. Ten en cuenta que se usará ROS2 Humble para este tutorial.

1. Abriendo el rosject

Para seguir este tutorial, necesitamos tener instalado ROS2 HUMBLE en nuestro sistema, y lo ideal sería tener un ros2_ws (Espacio de Trabajo ROS2). Para facilitarte la vida, ya hemos preparado un rosject para eso: https://app.theconstruct.ai/l/603ea5b0/.

Simplemente copiando el rosject (haciendo clic en el enlace de arriba), tendrás una configuración ya preparada para ti.

Después de haber copiado el rosject a tu propia área de trabajo, deberías ver el botón RUN. Haz clic en ese botón para lanzar el rosject (abajo tienes un ejemplo de rosject).

Tras pulsar el botón RUN, deberías tener cargado el rosject. Ahora, pasemos a la siguiente sección para ponernos manos a la obra.

2. Descripción

En ROS2, los subscribers, timers, service servers y action servers usan callbacks para su funcionamiento. Al recibir un mensaje o un llamado, ejecutan algún método predefinido. Por otro lado, al crear un nodo y definir subscribers, timers, etc. de manera básica, se tiene un thread principal que se encarga de manejar los callbacks. Dicho nodo al tener solo un thread, no puede ejecutar más de un callback al mismo tiempo.

Este comportamiento hace necesario el uso de múltiples threads y callback groups para tareas más complejas que requieran que se ejecute más de un callback al mismo tiempo.

En este tutorial aplicaremos múltiples threads y callbacks para un action server que realizará lo siguiente:

Subscribirse al tópico /odom para conocer la posición y orientación del TurtleBot3
Calcular la distancia recorrida por el robot durante su movimiento usando un Timer
Mover el robot por posiciones específicas que se mandarán al llamar al action server

3. Desarrollo

Múltiples threads

En esta situación es necesario el uso de más de un thread, de lo contrario, al momento de que se haga una petición al action server, se ejecutará su correspondiente callback. Esto evitará que los callbacks del subscriptor y del timer se ejecuten hasta que se termine la ejecución de su callback.

Para usar multiples threads, primero necesitamos importar MultiThreadedExecutor:

from rclpy.executors import MultiThreadedExecutorfloat64

Luego definir el número de threads necesarios (2 para este caso):

    # Use MultiThreadedExecutor
    executor = MultiThreadedExecutor(num_threads=2)
    executor.add_node(my_action_server)
    
    try:
        executor.spin()
    finally:
        my_action_server.destroy_node()
        rclpy.shutdown()

Callback groups

Solamente definir múltiples threads no es suficiente ya que existen los callback groups.

Cuando se ejecuta un nodo en un Multi-Threaded Executor, ROS 2 ofrece dos tipos diferentes de grupos de callbacks para controlar la ejecución de estos:

Mutually Exclusive Callback Group: Los callbacks de este grupo no se pueden ejecutar en paralelo.
Reentrant Callback Group: Los callbacks de este grupo si pueden ejecutarse al mismo tiempo.

Se debe tener en cuenta que, los callbacks pertenecientes a diferentes grupos de callbacks (de cualquier tipo) siempre pueden ejecutarse en paralelo entre sí.

En este tutorial usaremos 2 Mutually Exclusive Callback Groups. A uno de ellos le asignaremos el callback del action server y al otro le asignaremos los callbacks del subscriptor y del timer. Esto nos permitirá que al enviarse un request al action server, los callbacks del subscriptor y del timer se sigan ejecuanto al estar en otro grupo. Adicionalmente, estos dos callbacks al estar en un mismo Mutually Exclusive Callback Group no se ejecutarán al mismo tiempo, lo cual se require ya que el subscriptor actualiza la posición actual mientras que en el timer se calcula la distancia recorrida en ese periodo de tiempo.

Para usar callback groups primero importamos el grupo a usar:

from rclpy.callback_groups import MutuallyExclusiveCallbackGroup

Luego definimos los grupos:

self.mutuallyexclusive_group_1 = MutuallyExclusiveCallbackGroup()
self.mutuallyexclusive_group_2 = MutuallyExclusiveCallbackGroup()

Estos grupos se tienen que asignar como argumento al momento de instanciar el subscriptor, timer y action server. Si no se realiza esto, de manera predefinida los callbacks se asignarán a un mismo Mutually Exclusive Callback Group, el cual no permite la ejecución de los callbacks en paralelo a pesar de usar múltiples threads. Entonces asignamos:

Para el subscriptor:

# Subscriber
        self.subscription = self.create_subscription(
            Odometry,
            'odom',
            self.listener_callback,
            10,
            callback_group=self.mutuallyexclusive_group_1)

Luego definimos los grupos:

# Timer
        timer_period = 0.5  # segundos
        self.timer = self.create_timer(
            timer_period, 
            self.timer_callback, 
            callback_group=self.mutuallyexclusive_group_1)

Luego definimos los grupos:

# Action
        self._action_server = ActionServer(
            self, Move, 'move_as', 
            self.execute_callback, 
            callback_group=self.mutuallyexclusive_group_2)

4. Explicación del código

El action server usá una interfaz personalizada que se compone de:

Request: 2 arrays de coordenadas para las posiciones que el robot debe alcanzar.
Result: Distancia total recorrida durante el movimiento y un flag de confirmación de que todo se ejecuto correctamente.
Feedback: Distancia recorrida hasta el momento y el número de la posición alcanzada.

Move.action

float32[] pos_x
float32[] pos_y
---
bool status
float32 total_dist
---
int32 nro_pos
float32 current_dist

El código completo para el action server es el siguiente:

server_node.py

import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node

from custom_interfaces.action import Move
from nav_msgs.msg import Odometry

from geometry_msgs.msg import Twist
from std_msgs.msg import Float32
import time
import math
import numpy as np

from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup

class MyActionServer(Node):

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

        self.mutuallyexclusive_group_1 = MutuallyExclusiveCallbackGroup()
        self.mutuallyexclusive_group_2 = MutuallyExclusiveCallbackGroup()

        # Subscriber
        self.subscription = self.create_subscription(
            Odometry,
            'odom',
            self.listener_callback,
            10,
            callback_group=self.mutuallyexclusive_group_1)

        # Action
        self._action_server = ActionServer(
            self, Move, 'move_as', 
            self.execute_callback, 
            callback_group=self.mutuallyexclusive_group_2)

        # Publisher for cmd_vel
        self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)

    def timer_callback(self):

        delta_x = (self.pos[0] - self.pos_before[0])**2
        delta_y = (self.pos[1] - self.pos_before[1])**2
        self.distance.data += math.sqrt(delta_x + delta_y)
        self.pos_before = self.pos

    def listener_callback(self, msg):

        self.pos = [msg.pose.pose.position.x, msg.pose.pose.position.y]
        quaternion = [msg.pose.pose.orientation.x,
                      msg.pose.pose.orientation.y,
                      msg.pose.pose.orientation.z,
                      msg.pose.pose.orientation.w]
        self.roll, self.pitch, self.yaw = self.euler_from_quaternion(quaternion)

    def euler_from_quaternion(self, quaternion):
        """
        Converts quaternion (w in last place) to euler roll, pitch, yaw
        quaternion = [x, y, z, w]
        Below should be replaced when porting for ROS2 Python tf_conversions is done.
        """
        x = quaternion[0]
        y = quaternion[1]
        z = quaternion[2]
        w = quaternion[3]

        sinr_cosp = 2 * (w * x + y * z)
        cosr_cosp = 1 - 2 * (x * x + y * y)
        roll = np.arctan2(sinr_cosp, cosr_cosp)

        sinp = 2 * (w * y - z * x)
        pitch = np.arcsin(sinp)

        siny_cosp = 2 * (w * z + x * y)
        cosy_cosp = 1 - 2 * (y * y + z * z)
        yaw = np.arctan2(siny_cosp, cosy_cosp)

        return roll, pitch, yaw

    def execute_movement(self, a, b):

        self.vel = Twist()

        # Velocidad de desplazamiento
        self.vel_linear_x = 0.1
        self.vel_angular_z = 0.2

        # Máximo error
        self.linear_error = 0.08
        self.angular_error = 0.05

        # Calcular ángulo de rotación
        angulo_d = math.atan2(b - self.pos[1], a - self.pos[0])

        while ( abs(self.yaw - angulo_d) > self.angular_error ):

            self.vel.linear.x = 0.0
            self.vel.angular.z = self.vel_angular_z if self.yaw < angulo_d else -self.vel_angular_z  self.publisher_.publish(self.vel) time.sleep(0.5)  while ( math.sqrt((self.pos[0] - a)**2 + (self.pos[1] - b)**2) > self.linear_error ):
            self.vel.linear.x = self.vel_linear_x
            self.vel.angular.z = 0.0

            self.publisher_.publish(self.vel)
            time.sleep(0.5)

        self.vel.linear.x = 0.0 
        self.vel.angular.z = 0.0
        self.publisher_.publish(self.vel)


    def execute_callback(self, goal_handle):
        
        self.get_logger().info('Executing goal...')

        # Timer
        timer_period = 0.5  # segundos
        self.timer = self.create_timer(
            timer_period, 
            self.timer_callback, 
            callback_group=self.mutuallyexclusive_group_1)

        self.distance = Float32()
        self.distance.data = 0.0
        self.pos_before = self.pos

        feedback_msg = Move.Feedback()

        pos_x = goal_handle.request.pos_x
        pos_y = goal_handle.request.pos_y

        for i,val in enumerate(pos_x):

            self.execute_movement(pos_x[i], pos_y[i])

            feedback_msg.nro_pos = i + 1
            feedback_msg.current_dist = self.distance.data

            self.get_logger().info('Distancia recorrida hasta ahora: {0} '.format(feedback_msg.current_dist))
            self.get_logger().info('Posición {0} alcanzada'.format(feedback_msg.nro_pos))
            goal_handle.publish_feedback(feedback_msg)

        goal_handle.succeed()

        result = Move.Result()
        result.total_dist = self.distance.data
        result.status = True
        self.get_logger().info('Distancia total recorrida: {0}'.format(result.total_dist))

        return result

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

    my_action_server = MyActionServer()

    # Use MultiThreadedExecutor
    executor = MultiThreadedExecutor(num_threads=2)
    executor.add_node(my_action_server)
    
    try:
        executor.spin()
    finally:
        my_action_server.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Donde el callback del subscriber self.listener_callback actualiza la posición y rotación del TurtleBot3, mientras que el callback del timer self.timer_callback calcula la distancia recorrida durante un periodo de 0.5 segundos.

En el método self.execute_movement es donde se ejecuta el movimiento del robot, primero rotandolo para que apunte a la posición deseada y luego desplazándolo linealmente hacia esa posición.

En el callback del action server self.execute_callback se inicia el timer, se llama al método self.execute_movement por cada posición deseada, y se envía el feedback y resultado al action cliente.